diff --git a/test/benchmark/parse_benchmark_test.cc b/test/benchmark/parse_benchmark_test.cc index 73ebc1d5..fab49a3c 100644 --- a/test/benchmark/parse_benchmark_test.cc +++ b/test/benchmark/parse_benchmark_test.cc @@ -120,8 +120,7 @@ void BM_CompileManyTextures(benchmark::State& state) { for (auto s : state) { char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); } // Reset cache capacity to default value. diff --git a/test/engine/engine_collision_box_test.cc b/test/engine/engine_collision_box_test.cc index 0ac0ae62..e84e1296 100644 --- a/test/engine/engine_collision_box_test.cc +++ b/test/engine/engine_collision_box_test.cc @@ -72,8 +72,8 @@ TEST_F(MjCollisionBoxTest, BadContacts) { g2 = g2new; // call low-level box-box collider - int num = mjc_BoxBox(model, data, precon.data(), g1, g2, - con->includemargin); + int num = + mjc_BoxBox(model, data, precon.data(), g1, g2, con->includemargin); // allocate and clear arrays marking already matched contacts mju_zeroInt(match_raw.data(), num); @@ -83,8 +83,7 @@ TEST_F(MjCollisionBoxTest, BadContacts) { int nmatched = 0; for (int i = 0; i < num; i++) { for (int j = 0; j < data->ncon; j++) { - if (!match[j] && - precon[i].pos[0] == data->contact[j].pos[0] && + if (!match[j] && precon[i].pos[0] == data->contact[j].pos[0] && precon[i].pos[1] == data->contact[j].pos[1] && precon[i].pos[2] == data->contact[j].pos[2]) { match_raw[i] = match[j] = 1; @@ -110,17 +109,17 @@ TEST_F(MjCollisionBoxTest, BadContacts) { if (!match_raw[i]) { // === check if outside - mjtNum sz1[3] = {size1[0]+margin, size1[1]+margin, size1[2]+margin}; - mjtNum sz2[3] = {size2[0]+margin, size2[1]+margin, size2[2]+margin}; + mjtNum sz1[3] = {size1[0] + margin, size1[1] + margin, + size1[2] + margin}; + mjtNum sz2[3] = {size2[0] + margin, size2[1] + margin, + size2[2] + margin}; // relative distance (1%) outside of which contacts are removed static mjtNum kRatio = 1.01; // is the contact outside: 1, inside: -1, within the removal width: 0 - int out1 = mju_outsideBox(precon[i].pos, pos1, mat1, sz1, - kRatio); - int out2 = mju_outsideBox(precon[i].pos, pos2, mat2, sz2, - kRatio); + int out1 = mju_outsideBox(precon[i].pos, pos1, mat1, sz1, kRatio); + int out2 = mju_outsideBox(precon[i].pos, pos2, mat2, sz2, kRatio); // mark as bad if outside one box and not inside the other box bool outside = (out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1); @@ -151,7 +150,6 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) { std::vector match_raw(mjMAXCONPAIR); std::vector match(data->ncon); - int g1 = -1; int g2 = -1; for (int c = 0; c < data->ncon; c++) { @@ -174,8 +172,8 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) { g2 = g2new; // call low-level box-box collider - int num = mjc_BoxBox(model, data, precon.data(), g1, g2, - con->includemargin); + int num = + mjc_BoxBox(model, data, precon.data(), g1, g2, con->includemargin); // allocate and clear arrays marking already matched contacts mju_zeroInt(match_raw.data(), num); @@ -185,8 +183,7 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) { int nmatched = 0; for (int i = 0; i < num; i++) { for (int j = 0; j < data->ncon; j++) { - if (!match[j] && - precon[i].pos[0] == data->contact[j].pos[0] && + if (!match[j] && precon[i].pos[0] == data->contact[j].pos[0] && precon[i].pos[1] == data->contact[j].pos[1] && precon[i].pos[2] == data->contact[j].pos[2]) { match_raw[i] = match[j] = 1; @@ -255,20 +252,17 @@ TEST_F(MjCollisionBoxTest, BoxSphere) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); for (mjtNum z : {-.015, -.00501, -.005, -.00499, 0.0, 0.004}) { data->qpos[2] = z; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_EQ(data->ncon, 2); EXPECT_THAT(data->contact[0].dist, MjNear(data->contact[1].dist, 1e-8, 1e-6)); } - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) { @@ -281,22 +275,19 @@ TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; - mjData* data = mj_makeData(model); - mj_kinematics(model, data); + MjDataPtr data = MakeData(model); + mj_kinematics(model.get(), data.get()); mjPreContact precon[mjMAXCONPAIR]; for (mjfCollision collision : {mjc_BoxBox, mjc_Convex}) { - int n = collision(model, data, precon, 0, 1, 0.0); + int n = collision(model.get(), data.get(), precon, 0, 1, 0.0); for (int i = 0; i < n; i++) { EXPECT_NEAR(precon[i].dist, -0.5, MjTol(1e-8, 1e-6)); } } - - mj_deleteData(data); - mj_deleteModel(model); } } // namespace diff --git a/test/engine/engine_collision_driver_test.cc b/test/engine/engine_collision_driver_test.cc index aef1d9fe..5aa9600e 100644 --- a/test/engine/engine_collision_driver_test.cc +++ b/test/engine/engine_collision_driver_test.cc @@ -14,6 +14,8 @@ // Tests for engine/engine_collision_driver.c. +#include "src/engine/engine_collision_driver.h" + #include #include #include @@ -26,22 +28,20 @@ #include #include #include "test/fixture.h" -#include "src/engine/engine_collision_driver.h" - namespace mujoco { namespace { using MjCollisionTest = MujocoTest; using GeomPair = std::pair; -using ::testing::IsEmpty; using ::testing::ElementsAre; +using ::testing::IsEmpty; using ::testing::NotNull; // Returns a sorted list of pairs of colliding geom names, where each pair of // geom names is sorted. -static std::vector colliding_pairs( - const mjModel* model, const mjData* data) { +static std::vector colliding_pairs(const mjModel* model, + const mjData* data) { std::vector result; for (int i = 0; i < data->ncon; i++) { std::string geom1 = mj_id2name(model, mjOBJ_GEOM, data->contact[i].geom[0]); @@ -53,18 +53,16 @@ static std::vector colliding_pairs( } TEST_F(MjCollisionTest, AllCollisions) { - static const char* const kModelFilePath = - "engine/testdata/collisions.xml"; + static const char* const kModelFilePath = "engine/testdata/collisions.xml"; const std::string xml_path = GetTestDataFilePath(kModelFilePath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); mjData* data = mj_makeData(model); // mjCOL_ALL is the default mj_fwdPosition(model, data); - EXPECT_THAT(colliding_pairs(model, data), ElementsAre( - GeomPair("box", "sphere_collides"), - GeomPair("box", "sphere_predefined") - )); + EXPECT_THAT(colliding_pairs(model, data), + ElementsAre(GeomPair("box", "sphere_collides"), + GeomPair("box", "sphere_predefined"))); mj_deleteData(data); mj_deleteModel(model); @@ -72,20 +70,16 @@ TEST_F(MjCollisionTest, AllCollisions) { TEST_F(MjCollisionTest, EmptyModel) { char error[1024]; - mjModel* model = LoadModelFromString("", error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString("", error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); - mj_fwdPosition(model, data); - EXPECT_THAT(colliding_pairs(model, data), IsEmpty()); - - mj_deleteData(data); - mj_deleteModel(model); + mj_fwdPosition(model.get(), data.get()); + EXPECT_THAT(colliding_pairs(model.get(), data.get()), IsEmpty()); } TEST_F(MjCollisionTest, ZeroedHessian) { - static const char* const kModelFilePath = - "engine/testdata/collisions.xml"; + static const char* const kModelFilePath = "engine/testdata/collisions.xml"; const std::string xml_path = GetTestDataFilePath(kModelFilePath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); mjData* data = mj_makeData(model); @@ -123,18 +117,15 @@ TEST_F(MjCollisionTest, ContactCount) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); ASSERT_THAT(d, NotNull()); - mj_forward(m, d); + mj_forward(m.get(), d.get()); // there are 8 spheres, all touching the floor EXPECT_EQ(d->ncon, 8); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(MjCollisionTest, FilterParent) { @@ -159,12 +150,12 @@ TEST_F(MjCollisionTest, FilterParent) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); ASSERT_THAT(d, NotNull()); - mj_fwdPosition(m, d); + mj_fwdPosition(m.get(), d.get()); // there should be zero contacts, because colliding1 and colliding2 are in // bodies that have a parent-child relationship, through welds @@ -172,13 +163,10 @@ TEST_F(MjCollisionTest, FilterParent) { // when this filtering is disabled, the geoms should collide m->opt.disableflags |= mjDSBL_FILTERPARENT; - mj_fwdPosition(m, d); + mj_fwdPosition(m.get(), d.get()); - EXPECT_THAT(colliding_pairs(m, d), + EXPECT_THAT(colliding_pairs(m.get(), d.get()), ElementsAre(GeomPair("colliding1", "colliding2"))); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) { @@ -194,20 +182,17 @@ TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); ASSERT_THAT(d, NotNull()); - mj_fwdPosition(m, d); + mj_fwdPosition(m.get(), d.get()); // even though colliding1 and colliding2 are have a parent-child relationship, // they collide because colliding1 is in - EXPECT_THAT(colliding_pairs(m, d), + EXPECT_THAT(colliding_pairs(m.get(), d.get()), ElementsAre(GeomPair("colliding1", "colliding2"))); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(MjCollisionTest, TestOBB) { @@ -219,14 +204,18 @@ TEST_F(MjCollisionTest, TestOBB) { mjtNum mat2[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; EXPECT_THAT( - mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0), true); + mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0), + true); // rotate by 45 degrees - mat2[0] = 1./mju_sqrt(2.); mat2[1] = -1./mju_sqrt(2.); - mat2[3] = 1./mju_sqrt(2.); mat2[4] = 1./mju_sqrt(2.); + mat2[0] = 1. / mju_sqrt(2.); + mat2[1] = -1. / mju_sqrt(2.); + mat2[3] = 1. / mju_sqrt(2.); + mat2[4] = 1. / mju_sqrt(2.); EXPECT_THAT( - mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0), false); + mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0), + false); } TEST_F(MjCollisionTest, PlaneInBody) { @@ -244,13 +233,11 @@ TEST_F(MjCollisionTest, PlaneInBody) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); ASSERT_THAT(d, NotNull()); - mj_step(m, d); - mj_deleteData(d); - mj_deleteModel(m); + mj_step(m.get(), d.get()); } TEST_F(MjCollisionTest, PinchingSucceeds) { @@ -299,13 +286,13 @@ TEST_F(MjCollisionTest, PinchingSucceeds) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); ASSERT_THAT(d, NotNull()); - int lift_id = mj_name2id(m, mjOBJ_ACTUATOR, "lift"); - int grasp_id = mj_name2id(m, mjOBJ_ACTUATOR, "grasp"); + int lift_id = mj_name2id(m.get(), mjOBJ_ACTUATOR, "lift"); + int grasp_id = mj_name2id(m.get(), mjOBJ_ACTUATOR, "grasp"); // Phase 1: Lower gripper. // The gripper base starts at z=0.5. The finger has length 0.2 (size 0.1), @@ -316,21 +303,21 @@ TEST_F(MjCollisionTest, PinchingSucceeds) { for (int i = 0; i < 500; ++i) { d->ctrl[lift_id] = -0.35; // Lower d->ctrl[grasp_id] = 0; // Open - mj_step(m, d); + mj_step(m.get(), d.get()); } // Phase 2: Pinch for (int i = 0; i < 100; ++i) { d->ctrl[lift_id] = -0.35; // Hold height d->ctrl[grasp_id] = 0.8; // Close (max 1) - mj_step(m, d); + mj_step(m.get(), d.get()); } // Phase 3: Lift for (int i = 0; i < 1000; ++i) { d->ctrl[lift_id] = 0.5; // Lift up d->ctrl[grasp_id] = 0.8; // Keep closed - mj_step(m, d); + mj_step(m.get(), d.get()); } // Check if cloth is lifted @@ -352,9 +339,6 @@ TEST_F(MjCollisionTest, PinchingSucceeds) { // Specialized primitives (mjraw_BoxTriangle, mjraw_CapsuleTriangle) should // enable stable pinching, so we expect the cloth to be lifted. EXPECT_GT(avg_z, 0.2) << "Cloth slipped out of gripper!"; - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(MjCollisionTest, MarginSumming) { @@ -376,18 +360,15 @@ TEST_F(MjCollisionTest, MarginSumming) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); ASSERT_THAT(d, NotNull()); - mj_fwdPosition(m, d); + mj_fwdPosition(m.get(), d.get()); // With margin summing, we expect 1 contact EXPECT_EQ(d->ncon, 1); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(MjCollisionTest, MaxContact) { @@ -414,48 +395,45 @@ TEST_F(MjCollisionTest, MaxContact) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); ASSERT_THAT(d, NotNull()); - int mesh = mj_name2id(m, mjOBJ_GEOM, "mesh"); - int box = mj_name2id(m, mjOBJ_GEOM, "box"); - int plane = mj_name2id(m, mjOBJ_GEOM, "plane"); - int sphere = mj_name2id(m, mjOBJ_GEOM, "sphere"); - int capsule = mj_name2id(m, mjOBJ_GEOM, "capsule"); - int ellipsoid = mj_name2id(m, mjOBJ_GEOM, "ellipsoid"); - int cylinder = mj_name2id(m, mjOBJ_GEOM, "cylinder"); + int mesh = mj_name2id(m.get(), mjOBJ_GEOM, "mesh"); + int box = mj_name2id(m.get(), mjOBJ_GEOM, "box"); + int plane = mj_name2id(m.get(), mjOBJ_GEOM, "plane"); + int sphere = mj_name2id(m.get(), mjOBJ_GEOM, "sphere"); + int capsule = mj_name2id(m.get(), mjOBJ_GEOM, "capsule"); + int ellipsoid = mj_name2id(m.get(), mjOBJ_GEOM, "ellipsoid"); + int cylinder = mj_name2id(m.get(), mjOBJ_GEOM, "cylinder"); - EXPECT_EQ(mj_maxContact(m, mesh, box, -1), 4); - EXPECT_EQ(mj_maxContact(m, mesh, plane, -1), 3); - EXPECT_EQ(mj_maxContact(m, box, plane, -1), 4); - EXPECT_EQ(mj_maxContact(m, mesh, mesh, -1), 4); - EXPECT_EQ(mj_maxContact(m, box, box, -1), 8); - EXPECT_EQ(mj_maxContact(m, capsule, capsule, -1), 2); - EXPECT_EQ(mj_maxContact(m, capsule, box, -1), 4); - EXPECT_EQ(mj_maxContact(m, capsule, plane, -1), 2); - EXPECT_EQ(mj_maxContact(m, cylinder, plane, -1), 4); - EXPECT_EQ(mj_maxContact(m, sphere, sphere, -1), 1); - EXPECT_EQ(mj_maxContact(m, sphere, capsule, -1), 1); - EXPECT_EQ(mj_maxContact(m, sphere, box, -1), 1); - EXPECT_EQ(mj_maxContact(m, sphere, mesh, -1), 1); - EXPECT_EQ(mj_maxContact(m, sphere, plane, -1), 1); - EXPECT_EQ(mj_maxContact(m, sphere, cylinder, -1), 1); - EXPECT_EQ(mj_maxContact(m, ellipsoid, ellipsoid, -1), 1); - EXPECT_EQ(mj_maxContact(m, ellipsoid, box, -1), 1); - EXPECT_EQ(mj_maxContact(m, ellipsoid, mesh, -1), 1); - EXPECT_EQ(mj_maxContact(m, ellipsoid, plane, -1), 1); - EXPECT_EQ(mj_maxContact(m, ellipsoid, cylinder, -1), 1); - EXPECT_EQ(mj_maxContact(m, ellipsoid, capsule, -1), 1); - EXPECT_EQ(mj_maxContact(m, capsule, cylinder, -1), 5); - EXPECT_EQ(mj_maxContact(m, capsule, mesh, -1), 5); - EXPECT_EQ(mj_maxContact(m, cylinder, cylinder, -1), 5); - EXPECT_EQ(mj_maxContact(m, cylinder, box, -1), 5); - EXPECT_EQ(mj_maxContact(m, cylinder, mesh, -1), 5); - - mj_deleteData(d); - mj_deleteModel(m); + EXPECT_EQ(mj_maxContact(m.get(), mesh, box, -1), 4); + EXPECT_EQ(mj_maxContact(m.get(), mesh, plane, -1), 3); + EXPECT_EQ(mj_maxContact(m.get(), box, plane, -1), 4); + EXPECT_EQ(mj_maxContact(m.get(), mesh, mesh, -1), 4); + EXPECT_EQ(mj_maxContact(m.get(), box, box, -1), 8); + EXPECT_EQ(mj_maxContact(m.get(), capsule, capsule, -1), 2); + EXPECT_EQ(mj_maxContact(m.get(), capsule, box, -1), 4); + EXPECT_EQ(mj_maxContact(m.get(), capsule, plane, -1), 2); + EXPECT_EQ(mj_maxContact(m.get(), cylinder, plane, -1), 4); + EXPECT_EQ(mj_maxContact(m.get(), sphere, sphere, -1), 1); + EXPECT_EQ(mj_maxContact(m.get(), sphere, capsule, -1), 1); + EXPECT_EQ(mj_maxContact(m.get(), sphere, box, -1), 1); + EXPECT_EQ(mj_maxContact(m.get(), sphere, mesh, -1), 1); + EXPECT_EQ(mj_maxContact(m.get(), sphere, plane, -1), 1); + EXPECT_EQ(mj_maxContact(m.get(), sphere, cylinder, -1), 1); + EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, ellipsoid, -1), 1); + EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, box, -1), 1); + EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, mesh, -1), 1); + EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, plane, -1), 1); + EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, cylinder, -1), 1); + EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, capsule, -1), 1); + EXPECT_EQ(mj_maxContact(m.get(), capsule, cylinder, -1), 5); + EXPECT_EQ(mj_maxContact(m.get(), capsule, mesh, -1), 5); + EXPECT_EQ(mj_maxContact(m.get(), cylinder, cylinder, -1), 5); + EXPECT_EQ(mj_maxContact(m.get(), cylinder, box, -1), 5); + EXPECT_EQ(mj_maxContact(m.get(), cylinder, mesh, -1), 5); } } // namespace diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 1e17cd37..6a4260cf 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -42,9 +42,6 @@ using ::testing::ElementsAre; using ::testing::Pointwise; using ::testing::DoubleNear; -using TestModel = std::unique_ptr; -using TestData = std::unique_ptr; - constexpr mjtNum kTolerance = 1e-6; constexpr int kMaxIterations = 1000; constexpr char kEllipsoidXml[] = R"( @@ -66,18 +63,7 @@ constexpr char kEllipsoidXml[] = R"( )"; -TestModel LoadModel(std::string_view xml) { - char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - EXPECT_NE(model, nullptr) << "Failed to load model: " << error; - return TestModel(model, mj_deleteModel); -} - -TestData MakeData(const mjModel* model) { - return TestData(mj_makeData(model), mj_deleteData); -} - -mjtNum GeomDist(const TestModel& m, const TestData& d, int g1, int g2, +mjtNum GeomDist(const MjModelPtr& m, const MjDataPtr& d, int g1, int g2, mjtNum x1[3], mjtNum x2[3], mjtNum cutoff = mjMAX_LIMIT) { mjCCDConfig config; mjCCDStatus status; @@ -102,8 +88,8 @@ mjtNum GeomDist(const TestModel& m, const TestData& d, int g1, int g2, } int Penetration(mjCCDStatus& status, mjtNum& depth, std::vector& dir, - std::vector& pos, const TestModel& model, - const TestData& data, int g1, int g2, mjtNum margin = 0, + std::vector& pos, const MjModelPtr& model, + const MjDataPtr& data, int g1, int g2, mjtNum margin = 0, int max_contacts = 1) { mjCCDObj obj1, obj2; mjc_initCCDObj(&obj1, model.get(), data.get(), g1, margin); @@ -180,8 +166,8 @@ TEST_F(MjGjkTest, SphereSphereDist) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -203,8 +189,8 @@ TEST_F(MjGjkTest, SphereSphereDistCutoff) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -223,8 +209,8 @@ TEST_F(MjGjkTest, SphereSphereNoDist) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -247,8 +233,8 @@ TEST_F(MjGjkTest, SphereSphereIntersect) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -284,8 +270,8 @@ TEST_F(MjGjkTest, BoxBoxDepth) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -313,8 +299,8 @@ TEST_F(MjGjkTest, BoxBoxDepth2) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat + 9; @@ -358,8 +344,8 @@ TEST_F(MjGjkTest, BoxBoxDepth3) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat; @@ -421,8 +407,8 @@ TEST_F(MjGjkTest, BoxBoxSize05) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat; @@ -479,8 +465,8 @@ TEST_F(MjGjkTest, BoxBoxSize05b) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat; @@ -540,8 +526,8 @@ TEST_F(MjGjkTest, BoxBoxSize05c) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat; @@ -601,8 +587,8 @@ TEST_F(MjGjkTest, BoxBoxTouching) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -627,8 +613,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -661,8 +647,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD2) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -695,8 +681,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD3) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat + 9; @@ -737,8 +723,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD4) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat; @@ -800,8 +786,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD5) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat; @@ -864,8 +850,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD6) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat + 9; @@ -910,8 +896,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD7) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat; @@ -969,8 +955,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD8) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat; @@ -1027,8 +1013,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD9) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat; @@ -1086,8 +1072,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD10) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xpos = data->geom_xpos; @@ -1123,8 +1109,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD11) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xpos = data->geom_xpos; @@ -1184,8 +1170,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD12) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xpos = data->geom_xpos; @@ -1243,8 +1229,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD13) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xpos = data->geom_xpos; @@ -1305,8 +1291,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD14) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xpos = data->geom_xpos; @@ -1362,8 +1348,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD15) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat; @@ -1441,8 +1427,8 @@ TEST_F(MjGjkTest, SmallBoxMesh) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -1485,8 +1471,8 @@ TEST_F(MjGjkTest, BoxMesh) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -1515,8 +1501,8 @@ TEST_F(MjGjkTest, BoxMesh2) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -1545,8 +1531,8 @@ TEST_F(MjGjkTest, BoxMeshPrune) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -1577,8 +1563,8 @@ TEST_F(MjGjkTest, MeshMesh) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -1609,8 +1595,8 @@ TEST_F(MjGjkTest, MeshMeshPrune) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -1633,8 +1619,8 @@ TEST_F(MjGjkTest, BoxEdge) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "box1"); @@ -1657,8 +1643,8 @@ TEST_F(MjGjkTest, BoxEdge2) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat; @@ -1715,8 +1701,8 @@ TEST_F(MjGjkTest, BoxEdgeEdge) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat; @@ -1779,8 +1765,8 @@ TEST_F(MjGjkTest, MeshEdge) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "box1"); @@ -1819,8 +1805,8 @@ TEST_F(MjGjkTest, MeshEdge2) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "floor"); @@ -1835,8 +1821,8 @@ TEST_F(MjGjkTest, MeshEdge2) { } TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) { - TestModel model = LoadModel(kEllipsoidXml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(kEllipsoidXml); + MjDataPtr data = MakeData(model); mj_resetDataKeyframe(model.get(), data.get(), 0); mj_forward(model.get(), data.get()); @@ -1861,8 +1847,8 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -1884,8 +1870,8 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidSlowConvergence) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum* xmat = data->geom_xmat; @@ -1943,8 +1929,8 @@ TEST_F(MjGjkTest, BoxBox) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -1963,8 +1949,8 @@ TEST_F(MjGjkTest, BoxBoxLarge) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -2013,8 +1999,8 @@ static constexpr char xml[] = R"( )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -2046,8 +2032,8 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -2071,8 +2057,8 @@ TEST_F(MjGjkTest, CapsuleCapsule) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); @@ -2101,8 +2087,8 @@ TEST_F(MjGjkTest, CylinderBoxMargin) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); // margin=0.1 means forces generated when dist<0.1 @@ -2121,8 +2107,8 @@ TEST_F(MjGjkTest, BoxEdgeFlipped) { )"; - TestModel model = LoadModel(xml); - TestData data = MakeData(model.get()); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); diff --git a/test/engine/engine_collision_sdf_test.cc b/test/engine/engine_collision_sdf_test.cc index 1d4cf1ce..b747a2d7 100644 --- a/test/engine/engine_collision_sdf_test.cc +++ b/test/engine/engine_collision_sdf_test.cc @@ -14,14 +14,16 @@ // Tests for sdf collisions. +#include "src/engine/engine_collision_sdf.h" + #include + #include #include #include #include #include #include -#include "src/engine/engine_collision_sdf.h" #include "test/fixture.h" namespace mujoco { @@ -48,22 +50,24 @@ static constexpr char kSdfModel[] = R"( TEST_F(SdfTest, SdfPrimitive) { char error[1024]; - mjModel* model = LoadModelFromString(kSdfModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(kSdfModel, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); ASSERT_THAT(model->ngeom, kgeoms); mjSDF sdf; const mjpPlugin* null_plugin = NULL; - mjtNum gradient[3], dist[kgeoms][kpoints] = { - {0, 0, 0, 0, 1, 1}, // plane - {-1, 0, 0, mju_sqrt(2)-1, mju_sqrt(2)-1, mju_sqrt(3)-1}, // sphere - {(mjtNum)-.1, (mjtNum).9, (mjtNum).9, - mju_sqrt(2)-(mjtNum).1, (mjtNum).9, mju_sqrt(2)-(mjtNum).1}, // capsule - {-1, 0, 0, mju_sqrt(2)-1, 0, mju_sqrt(2)-1}, // cylinder - {-mju_sqrt(3), 0, 0, 0, 0, 0}, // box - }; + mjtNum gradient[3], + dist[kgeoms][kpoints] = { + {0, 0, 0, 0, 1, 1}, // plane + {-1, 0, 0, mju_sqrt(2) - 1, mju_sqrt(2) - 1, + mju_sqrt(3) - 1}, // sphere + {(mjtNum)-.1, (mjtNum).9, (mjtNum).9, mju_sqrt(2) - (mjtNum).1, + (mjtNum).9, mju_sqrt(2) - (mjtNum).1}, // capsule + {-1, 0, 0, mju_sqrt(2) - 1, 0, mju_sqrt(2) - 1}, // cylinder + {-mju_sqrt(3), 0, 0, 0, 0, 0}, // box + }; mjtNum points[kpoints][3] = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {1, 1, 0}, {0, 1, 1}, {1, 1, 1}}; @@ -71,16 +75,13 @@ TEST_F(SdfTest, SdfPrimitive) { sdf.plugin = &null_plugin; sdf.id = &i; sdf.type = mjSDFTYPE_SINGLE; - sdf.geomtype = (mjtGeom*)(model->geom_type+i); + sdf.geomtype = (mjtGeom*)(model->geom_type + i); for (int j = 0; j < kpoints; j++) { - EXPECT_NEAR(mjc_distance(model, data, &sdf, points[j]), dist[i][j], - MjTol(1e-9, 5e-7)); - mjc_gradient(model, data, &sdf, gradient, points[j]); + EXPECT_NEAR(mjc_distance(model.get(), data.get(), &sdf, points[j]), + dist[i][j], MjTol(1e-9, 5e-7)); + mjc_gradient(model.get(), data.get(), &sdf, gradient, points[j]); } } - - mj_deleteData(data); - mj_deleteModel(model); } static constexpr char kFlexSdfModel[] = R"( @@ -118,9 +119,9 @@ static constexpr char kFlexSdfModel[] = R"( TEST_F(SdfTest, FlexSdfCollision) { char error[1024]; - mjModel* model = LoadModelFromString(kFlexSdfModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(kFlexSdfModel, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); // check there is at least one flex @@ -128,7 +129,7 @@ TEST_F(SdfTest, FlexSdfCollision) { // simulate for a few steps to let flex fall onto torus for (int i = 0; i < 100; i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); } // should have contacts between flex and SDF @@ -143,11 +144,7 @@ TEST_F(SdfTest, FlexSdfCollision) { } } EXPECT_TRUE(has_flex_contact) << "Expected at least one flex contact"; - - mj_deleteData(data); - mj_deleteModel(model); } } // namespace } // namespace mujoco - diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 0541bd06..f8ff8553 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -14,6 +14,8 @@ // Tests for engine/engine_core_constraint.c. +#include "src/engine/engine_core_constraint.h" + #include #include #include @@ -23,7 +25,6 @@ #include #include #include -#include "src/engine/engine_core_constraint.h" #include "src/engine/engine_core_util.h" #include "src/engine/engine_support.h" #include "src/engine/engine_util_misc.h" @@ -51,31 +52,32 @@ TEST_F(CoreConstraintTest, RestPenetration) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, testing::NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), testing::NotNull()) << error; mjtNum gravity = -model->opt.gravity[2]; mjtNum damping_ratio = 0.8; - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); for (const mjtNum reference : {-100.0, -10.0, 0.1, 0.01}) { for (const mjtNum impedance : {0.3, 0.9, 0.99}) { // set solimp - for (int i=0; i < model->ngeom; i++) { - model->geom_solimp[i*mjNIMP + 0] = impedance; - model->geom_solimp[i*mjNIMP + 1] = impedance; + for (int i = 0; i < model->ngeom; i++) { + model->geom_solimp[i * mjNIMP + 0] = impedance; + model->geom_solimp[i * mjNIMP + 1] = impedance; } // set solref - for (int i=0; i < model->ngeom; i++) { - model->geom_solref[i*mjNREF + 0] = reference; - model->geom_solref[i*mjNREF + 1] = reference < 0 ? -10 : damping_ratio; + for (int i = 0; i < model->ngeom; i++) { + model->geom_solref[i * mjNREF + 0] = reference; + model->geom_solref[i * mjNREF + 1] = + reference < 0 ? -10 : damping_ratio; } // simulate for 50 seconds - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); while (data->time < 50) { mjtNum time = data->time; - mj_step(model, data); + mj_step(model.get(), data.get()); ASSERT_GT(data->time, time) << "Divergence detected"; } @@ -91,9 +93,6 @@ TEST_F(CoreConstraintTest, RestPenetration) { EXPECT_THAT(depth, MjNear(expected_depth, 1e-10, 1e-3)); } } - - mj_deleteData(data); - mj_deleteModel(model); } static const char* const kDoflessContactPath = @@ -153,7 +152,7 @@ TEST_F(CoreConstraintTest, EqualityBodySite) { mj_resetData(model, data); // turn site-defined equalities off, equivalent body-defined equalities on - for (int e=0; e < 4; e++) data->eq_active[e] = 1 - data->eq_active[e]; + for (int e = 0; e < 4; e++) data->eq_active[e] = 1 - data->eq_active[e]; // simulate again, get diag(A) while (data->time < 0.1) { @@ -220,12 +219,12 @@ TEST_F(CoreConstraintTest, ConstraintUpdateImpl) { // iterate over islands, check match mjtNum cost2 = 0; - for (int island=0; island < nisland; island++) { + for (int island = 0; island < nisland; island++) { // clear outputs from data2 - for (int i=0; i < nefc; i++) d2->efc_state[i] = -1; + for (int i = 0; i < nefc; i++) d2->efc_state[i] = -1; mju_zero(d2->efc_force, nefc); mju_zero(d2->qfrc_constraint, nv); - for (int i=0; i < d2->ncon; i++) mju_zero(d2->contact[i].H, 36); + for (int i = 0; i < d2->ncon; i++) mju_zero(d2->contact[i].H, 36); // sizes and indices, in this island int efcnum = d2->island_nefc[island]; @@ -237,26 +236,19 @@ TEST_F(CoreConstraintTest, ConstraintUpdateImpl) { // update constraints for this island mjtNum cost2i; - int ne = d2->island_ne[island]; - int nf = d2->island_nf[island]; + int ne = d2->island_ne[island]; + int nf = d2->island_nf[island]; int adr = d2->island_iefcadr[island]; int* state = d2->iefc_state + adr; - mjtNum *force = d2->iefc_force + adr; - mj_constraintUpdate_impl(ne, nf, efcnum, - d2->iefc_D + adr, - d2->iefc_R + adr, - d2->iefc_frictionloss + adr, - jari, - d2->iefc_type + adr, - d2->iefc_id + adr, - d2->contact, - state, - force, - &cost2i, + mjtNum* force = d2->iefc_force + adr; + mj_constraintUpdate_impl(ne, nf, efcnum, d2->iefc_D + adr, + d2->iefc_R + adr, d2->iefc_frictionloss + adr, + jari, d2->iefc_type + adr, d2->iefc_id + adr, + d2->contact, state, force, &cost2i, /*flg_coneHessian=*/1); // compare nefc vectors - for (int c=0; c < efcnum; c++) { + for (int c = 0; c < efcnum; c++) { int i = map2efc[c]; EXPECT_EQ(d2->efc_island[i], island); EXPECT_EQ(state[c], d1->efc_state[i]); @@ -265,11 +257,11 @@ TEST_F(CoreConstraintTest, ConstraintUpdateImpl) { // compare cone Hessians if (cone == mjCONE_ELLIPTIC) { - for (int c=0; c < d2->ncon; c++) { + for (int c = 0; c < d2->ncon; c++) { int efcadr = d2->contact[c].efc_address; if (d2->efc_island[efcadr] == island && d2->efc_state[efcadr] == mjCNSTRSTATE_CONE) { - for (int j=0; j < 36; j++) { + for (int j = 0; j < 36; j++) { EXPECT_THAT(d2->contact[c].H[j], MjNear(d1->contact[c].H[j], 1e-12, 1e-4)); } @@ -308,9 +300,9 @@ TEST_F(CoreConstraintTest, FlexvertEquality) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, testing::NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), testing::NotNull()) << error; + MjDataPtr data = MakeData(model); ASSERT_EQ(model->neq, 1); ASSERT_EQ(model->eq_type[0], mjEQ_FLEXVERT); ASSERT_EQ(model->nflex, 1); @@ -318,8 +310,8 @@ TEST_F(CoreConstraintTest, FlexvertEquality) { ASSERT_EQ(model->flex_edgenum[0], 16); // step1 to populate flexvert_length - mj_step1(model, data); - EXPECT_EQ(data->ne, 2*model->flex_vertnum[0]); + mj_step1(model.get(), data.get()); + EXPECT_EQ(data->ne, 2 * model->flex_vertnum[0]); EXPECT_EQ(data->nefc, 18); for (int i = 0; i < 18; ++i) { EXPECT_EQ(data->efc_type[i], mjCNSTR_EQUALITY); @@ -334,9 +326,9 @@ TEST_F(CoreConstraintTest, FlexvertEquality) { for (int i = 0; i < 3; ++i) { mju_zero(qvel.data(), model->nv); for (int j = 0; j < model->flex_vertnum[0]; ++j) { - qvel[3*j+i] = 1.0; + qvel[3 * j + i] = 1.0; } - mj_mulJacVec(model, data, Jqvel.data(), qvel.data()); + mj_mulJacVec(model.get(), data.get(), Jqvel.data(), qvel.data()); for (int j = 0; j < data->nefc; ++j) { EXPECT_NEAR(Jqvel[j], 0, MjTol(1e-9, 1e-5)); } @@ -355,14 +347,11 @@ TEST_F(CoreConstraintTest, FlexvertEquality) { qvel[3 * j + 1] = linvel[1]; qvel[3 * j + 2] = linvel[2]; } - mj_mulJacVec(model, data, Jqvel.data(), qvel.data()); + mj_mulJacVec(model.get(), data.get(), Jqvel.data(), qvel.data()); for (int j = 0; j < data->nefc; ++j) { EXPECT_NEAR(Jqvel[j], 0, MjTol(1e-9, 1e-5)); } } - - mj_deleteData(data); - mj_deleteModel(model); } // Test flex strain constraint with pinned nodes attached to freejoint parent @@ -392,12 +381,12 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); + MjDataPtr d = MakeData(m); - mj_resetData(m, d); - mj_forward(m, d); + mj_resetData(m.get(), d.get()); + mj_forward(m.get(), d.get()); // Check that we have constraints EXPECT_GT(d->nefc, 0) << "No constraints generated"; @@ -425,7 +414,7 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) { for (int i = 0; i < d->nefc; i++) { if (d->efc_type[i] == mjCNSTR_EQUALITY) { for (int j = 0; j < nv; j++) { - mjtNum val = d->efc_J[i*nv + j]; + mjtNum val = d->efc_J[i * nv + j]; if (mju_isBad(val) || mju_abs(val) > 1e10) { has_bad_jacobian = true; } @@ -449,12 +438,12 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) { mju_copy(d->qpos, qpos0.data(), m->nq); mjtNum dqpos[100] = {0}; dqpos[j] = eps; - mj_integratePos(m, d->qpos, dqpos, 1); - mj_forward(m, d); + mj_integratePos(m.get(), d->qpos, dqpos, 1); + mj_forward(m.get(), d.get()); for (int i = 0; i < num_constraints_to_check; i++) { mjtNum fd = (d->efc_pos[i] - efc_pos0[i]) / eps; - mjtNum analytic = d->efc_J[i*nv + j]; + mjtNum analytic = d->efc_J[i * nv + j]; // Use relative tolerance with absolute floor to handle near-zero values mjtNum tol = mju_max(1e-8, 0.1 * (mju_abs(fd) + mju_abs(analytic))); if (mju_abs(fd - analytic) > tol) { @@ -467,12 +456,12 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) { // Test rotation invariance: rotate via freejoint quaternion mju_copy(d->qpos, qpos0.data(), m->nq); - mjtNum angle = 0.785398; // 45 degrees - d->qpos[3] = mju_cos(angle/2); // w + mjtNum angle = 0.785398; // 45 degrees + d->qpos[3] = mju_cos(angle / 2); // w d->qpos[4] = 0; d->qpos[5] = 0; - d->qpos[6] = mju_sin(angle/2); // z - mj_forward(m, d); + d->qpos[6] = mju_sin(angle / 2); // z + mj_forward(m.get(), d.get()); mjtNum max_strain_rotated = 0; for (int i = 0; i < d->ne; i++) { @@ -494,11 +483,11 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) { mju_copy(d->qpos, qpos_rot.data(), m->nq); mjtNum dqpos[100] = {0}; dqpos[j] = eps; - mj_integratePos(m, d->qpos, dqpos, 1); - mj_forward(m, d); + mj_integratePos(m.get(), d->qpos, dqpos, 1); + mj_forward(m.get(), d.get()); mjtNum fd = (d->efc_pos[0] - efc_pos_rot[0]) / eps; - mjtNum analytic = d->efc_J[0*nv + j]; + mjtNum analytic = d->efc_J[0 * nv + j]; mjtNum tol = 0.1 * (mju_abs(fd) + mju_abs(analytic) + 1e-8); if ((mju_abs(fd) > 1e-8 || mju_abs(analytic) > 1e-8) && mju_abs(fd - analytic) > tol) { @@ -510,12 +499,12 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) { // Reset for simulation mju_copy(d->qpos, qpos0.data(), m->nq); - mj_forward(m, d); + mj_forward(m.get(), d.get()); // Run simulation only if checks pass if (!has_bad_constraint && !has_bad_jacobian) { for (int i = 0; i < 2000; i++) { - mj_step(m, d); + mj_step(m.get(), d.get()); ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation became unstable at step " << i; @@ -529,9 +518,6 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) { } } } - - mj_deleteData(d); - mj_deleteModel(m); } // Test flex strain constraint WITHOUT pinned nodes (simpler case) @@ -554,12 +540,12 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); + MjDataPtr d = MakeData(m); - mj_resetData(m, d); - mj_forward(m, d); + mj_resetData(m.get(), d.get()); + mj_forward(m.get(), d.get()); // Check constraints EXPECT_GT(d->ne, 0) << "Expected strain constraints"; @@ -581,7 +567,7 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) { bool has_bad_jacobian = false; for (int i = 0; i < d->ne; i++) { for (int j = 0; j < nv; j++) { - if (mju_isBad(d->efc_J[i*nv + j])) { + if (mju_isBad(d->efc_J[i * nv + j])) { has_bad_jacobian = true; } } @@ -592,12 +578,12 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) { std::vector qpos0(m->nq); mju_copy(qpos0.data(), d->qpos, m->nq); // Rotate by 45 degrees around Z axis via quaternion - mjtNum angle = 0.785398; // 45 degrees - d->qpos[3] = mju_cos(angle/2); // w - d->qpos[4] = 0; // x - d->qpos[5] = 0; // y - d->qpos[6] = mju_sin(angle/2); // z - mj_forward(m, d); + mjtNum angle = 0.785398; // 45 degrees + d->qpos[3] = mju_cos(angle / 2); // w + d->qpos[4] = 0; // x + d->qpos[5] = 0; // y + d->qpos[6] = mju_sin(angle / 2); // z + mj_forward(m.get(), d.get()); mjtNum max_strain_rotated = 0; for (int i = 0; i < d->ne; i++) { @@ -610,15 +596,12 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) { // Run simulation for a few steps to check stability mju_copy(d->qpos, qpos0.data(), m->nq); - mj_forward(m, d); + mj_forward(m.get(), d.get()); for (int i = 0; i < 100; i++) { - mj_step(m, d); + mj_step(m.get(), d.get()); ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation unstable at step " << i; } - - mj_deleteData(d); - mj_deleteModel(m); } // Test flex strain constraint with quadratic interpolation @@ -641,12 +624,12 @@ TEST_F(CoreConstraintTest, StrainConstraintQuadratic) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); + MjDataPtr d = MakeData(m); - mj_resetData(m, d); - mj_forward(m, d); + mj_resetData(m.get(), d.get()); + mj_forward(m.get(), d.get()); // Check constraints generated EXPECT_GT(d->ne, 0) << "Expected strain constraints"; @@ -665,7 +648,7 @@ TEST_F(CoreConstraintTest, StrainConstraintQuadratic) { bool has_bad_jacobian = false; for (int i = 0; i < d->ne; i++) { for (int j = 0; j < nv; j++) { - if (mju_isBad(d->efc_J[i*nv + j])) { + if (mju_isBad(d->efc_J[i * nv + j])) { has_bad_jacobian = true; } } @@ -674,13 +657,9 @@ TEST_F(CoreConstraintTest, StrainConstraintQuadratic) { // Run simulation for a few steps for (int i = 0; i < 100; i++) { - mj_step(m, d); - ASSERT_FALSE(mju_isBad(d->qpos[0])) - << "Simulation unstable at step " << i; + mj_step(m.get(), d.get()); + ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation unstable at step " << i; } - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) { @@ -701,11 +680,11 @@ TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; + MjDataPtr d = MakeData(m); - mj_forward(m, d); + mj_forward(m.get(), d.get()); // Check number of equalities EXPECT_EQ(m->neq, 6); @@ -719,9 +698,6 @@ TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) { EXPECT_NEAR(d->efc_pos[i], 0, 1e-10) << "nonzero constraint residual at " << i; } - - mj_deleteData(d); - mj_deleteModel(m); } // Test quadratic passive forces (no constraints) for stability @@ -741,13 +717,13 @@ TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); + MjDataPtr d = MakeData(m); // Run for 500 steps — should stay stable for (int i = 0; i < 500; i++) { - mj_step(m, d); + mj_step(m.get(), d.get()); ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Passive quadratic unstable at step " << i; for (int j = 0; j < m->nv; j++) { @@ -755,9 +731,6 @@ TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) { << "Velocity exploded at step " << i; } } - - mj_deleteData(d); - mj_deleteModel(m); } // Test quadratic with anisotropic cells (like what mesh bounding box creates) @@ -782,27 +755,24 @@ TEST_F(CoreConstraintTest, QuadraticAnisotropicStrain) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); + MjDataPtr d = MakeData(m); - mj_forward(m, d); + mj_forward(m.get(), d.get()); EXPECT_GT(d->ne, 0) << "Expected strain constraints"; // Run for 200 steps with gravity + contact for (int i = 0; i < 200; i++) { - mj_step(m, d); + mj_step(m.get(), d.get()); ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Anisotropic quadratic unstable at step " << i; for (int j = 0; j < m->nv; j++) { ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) - << "Velocity exploded at step " << i - << ", qvel[" << j << "]=" << d->qvel[j]; + << "Velocity exploded at step " << i << ", qvel[" << j + << "]=" << d->qvel[j]; } } - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(CoreConstraintTest, ContactSharedDofJacobian) { @@ -826,21 +796,18 @@ TEST_F(CoreConstraintTest, ContactSharedDofJacobian) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; ASSERT_EQ(model->nv, 3); - ASSERT_TRUE(mj_isSparse(model)); - mjData* data = mj_makeData(model); + ASSERT_TRUE(mj_isSparse(model.get())); + MjDataPtr data = MakeData(model); - mj_forward(model, data); + mj_forward(model.get(), data.get()); ASSERT_EQ(data->ncon, 1); ASSERT_GE(data->nefc, 1); EXPECT_EQ(data->efc_J_rownnz[0], 2); - - mj_deleteData(data); - mj_deleteModel(model); } static const char* const kJdotvConnect2dPath = @@ -852,10 +819,8 @@ static const char* const kJdotvWeld3dPath = // validate mj_Jdotv against finite-differenced constraint Jacobian TEST_F(CoreConstraintTest, JdotvFiniteDifference) { - - for (const char* path : {kJdotvConnect2dPath, - kJdotvConnect3dPath, - kJdotvWeld3dPath}) { + for (const char* path : + {kJdotvConnect2dPath, kJdotvConnect3dPath, kJdotvWeld3dPath}) { const std::string xml_path = GetTestDataFilePath(path); char err[1024]; mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err)); @@ -876,8 +841,8 @@ TEST_F(CoreConstraintTest, JdotvFiniteDifference) { // get dense J_0 (ne x nv) std::vector J0(ne * nv); if (mj_isSparse(m)) { - mju_sparse2dense(J0.data(), d->efc_J, ne, nv, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); + mju_sparse2dense(J0.data(), d->efc_J, ne, nv, d->efc_J_rownnz, + d->efc_J_rowadr, d->efc_J_colind); } else { mju_copy(J0.data(), d->efc_J, ne * nv); } @@ -900,8 +865,8 @@ TEST_F(CoreConstraintTest, JdotvFiniteDifference) { ASSERT_EQ(d->ne, ne) << "constraint count changed after integration"; std::vector Jh(ne * nv); if (mj_isSparse(m)) { - mju_sparse2dense(Jh.data(), d->efc_J, ne, nv, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); + mju_sparse2dense(Jh.data(), d->efc_J, ne, nv, d->efc_J_rownnz, + d->efc_J_rowadr, d->efc_J_colind); } else { mju_copy(Jh.data(), d->efc_J, ne * nv); } @@ -911,7 +876,7 @@ TEST_F(CoreConstraintTest, JdotvFiniteDifference) { std::vector jdv_fd(ne, 0); for (int i = 0; i < ne; i++) { for (int j = 0; j < nv; j++) { - jdv_fd[i] -= (Jh[i*nv+j] - J0[i*nv+j]) / h * qvel0[j]; + jdv_fd[i] -= (Jh[i * nv + j] - J0[i * nv + j]) / h * qvel0[j]; } } @@ -927,9 +892,8 @@ TEST_F(CoreConstraintTest, JdotvFiniteDifference) { // Test 2: forward-inverse identity preserved with Jdot*v correction TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) { - for (const char* path : {kJdotvConnect2dPath, - kJdotvConnect3dPath, - kJdotvWeld3dPath}) { + for (const char* path : + {kJdotvConnect2dPath, kJdotvConnect3dPath, kJdotvWeld3dPath}) { const std::string xml_path = GetTestDataFilePath(path); char err[1024]; mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err)); @@ -946,8 +910,7 @@ TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) { mjtNum epsilon = MjTol(1e-10, 1e-2); EXPECT_LT(fwdinv, epsilon) - << "fwdinv broken for " << path - << " (fwdinv=" << fwdinv << ")"; + << "fwdinv broken for " << path << " (fwdinv=" << fwdinv << ")"; mj_deleteData(d); mj_deleteModel(m); @@ -964,10 +927,9 @@ struct StrainConstraintTestCase { std::string flex_xyaxes; }; -class StrainConstraintRotatedTest : public CoreConstraintTest, - public ::testing::WithParamInterface< - StrainConstraintTestCase> { -}; +class StrainConstraintRotatedTest + : public CoreConstraintTest, + public ::testing::WithParamInterface {}; TEST_P(StrainConstraintRotatedTest, ResidualIsZero) { auto param = GetParam(); @@ -1006,11 +968,11 @@ TEST_P(StrainConstraintRotatedTest, ResidualIsZero) { )"; std::array error; - mjModel* m = LoadModelFromString(xml.c_str(), error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml.c_str(), error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); + MjDataPtr d = MakeData(m); - mj_forward(m, d); + mj_forward(m.get(), d.get()); // Check we have strain constraints EXPECT_GT(d->ne, 0) << "Expected strain constraints"; @@ -1021,24 +983,19 @@ TEST_P(StrainConstraintRotatedTest, ResidualIsZero) { for (int i = 0; i < d->ne; i++) { max_pos = mju_max(max_pos, mju_abs(d->efc_pos[i])); } - EXPECT_LT(max_pos, 1e-6) - << "Strain constraint residual should be ~0" - << " (max_pos=" << max_pos << ")"; + EXPECT_LT(max_pos, 1e-6) << "Strain constraint residual should be ~0" + << " (max_pos=" << max_pos << ")"; // Verify stability for (int i = 0; i < 200; i++) { - mj_step(m, d); - ASSERT_FALSE(mju_isBad(d->qpos[0])) - << "Simulation unstable at step " << i; + mj_step(m.get(), d.get()); + ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation unstable at step " << i; for (int j = 0; j < m->nv; j++) { ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) - << "Velocity exploded at step " << i - << ", qvel[" << j << "]=" << d->qvel[j]; + << "Velocity exploded at step " << i << ", qvel[" << j + << "]=" << d->qvel[j]; } } - - mj_deleteData(d); - mj_deleteModel(m); } INSTANTIATE_TEST_SUITE_P( @@ -1106,11 +1063,11 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, testing::NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), testing::NotNull()) << error; + MjDataPtr data = MakeData(model); - mj_forward(model, data); + mj_forward(model.get(), data.get()); // find central vertex index (13 for 3x3x3 grid) int central_idx = 13; @@ -1121,7 +1078,8 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) { int rest = central_idx % (nx * ny); int j = rest / nx; int i = rest % nx; - ASSERT_TRUE(i > 0 && i < nx-1 && j > 0 && j < ny-1 && k > 0 && k < nz-1); + ASSERT_TRUE(i > 0 && i < nx - 1 && j > 0 && j < ny - 1 && k > 0 && + k < nz - 1); // create manual contact with central vertex mjContact con; @@ -1135,21 +1093,21 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) { con.flex[1] = 0; con.vert[1] = central_idx; con.dim = 1; - mju_copy3(con.pos, data->flexvert_xpos + 3*central_idx); - con.frame[0] = 0; con.frame[1] = 0; con.frame[2] = 1; // normal + mju_copy3(con.pos, data->flexvert_xpos + 3 * central_idx); + con.frame[0] = 0; + con.frame[1] = 0; + con.frame[2] = 1; // normal // buffer for Jacobian - std::vector jacdif(3*model->nv, 0.0); + std::vector jacdif(3 * model->nv, 0.0); // call mj_contactJacobian - mj_contactJacobian(model, data, &con, 1, nullptr, jacdif.data(), - nullptr, nullptr, nullptr, - nullptr, nullptr, nullptr, + mj_contactJacobian(model.get(), data.get(), &con, 1, nullptr, jacdif.data(), + nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr); // check that boundary nodes have non-zero entries, and central node has zero - bool boundary_has_dof = false; bool interior_has_dof = false; @@ -1185,9 +1143,6 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) { << "Boundary nodes should receive contact force"; EXPECT_FALSE(interior_has_dof) << "Interior nodes should not receive contact force"; - - mj_deleteData(data); - mj_deleteModel(model); } } // namespace diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index f689105a..73a4b1e8 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -15,8 +15,6 @@ // Tests for engine/engine_core_smooth.c. #include "src/engine/engine_core_smooth.h" -#include "src/engine/engine_util_misc.h" -#include "src/engine/engine_util_sparse.h" #include #include @@ -30,6 +28,8 @@ #include #include #include +#include "src/engine/engine_util_misc.h" +#include "src/engine/engine_util_sparse.h" #include "test/fixture.h" namespace mujoco { @@ -44,7 +44,6 @@ using ::testing::NotNull; using ::testing::Pointwise; using CoreSmoothTest = MujocoTest; - constexpr bool EndsWith(std::string_view str, std::string_view suffix) { return str.size() >= suffix.size() && str.substr(str.size() - suffix.size()) == suffix; @@ -64,31 +63,28 @@ TEST_F(CoreSmoothTest, MjDataWorldBodyValuesAreInitialized) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); - mj_resetDataDebug(model, data, 'd'); - mj_forward(model, data); - mj_rnePostConstraint(model, data); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mj_resetDataDebug(model.get(), data.get(), 'd'); + mj_forward(model.get(), data.get()); + mj_rnePostConstraint(model.get(), data.get()); { -#define X(type, name, d0, d1) \ - if constexpr (std::string_view(#d0) == "nbody") { \ - absl::Span values(data->name, model->d0 * d1); \ - if constexpr (EndsWith(#name, "quat")) { \ - EXPECT_THAT(values, ElementsAre(1, 0, 0, 0)) << #name; \ - } else if constexpr (EndsWith(#name, "mat")) { \ - EXPECT_THAT(values, ElementsAre(1, 0, 0, 0, 1, 0, 0, 0, 1)) << #name; \ - } else if constexpr (std::string_view(#type) == "mjtNum") { \ - EXPECT_THAT(values, Each(MjNear(0.0, 1e-7, 1e-7))) << #name; \ - } \ - } +#define X(type, name, d0, d1) \ + if constexpr (std::string_view(#d0) == "nbody") { \ + absl::Span values(data->name, model->d0 * d1); \ + if constexpr (EndsWith(#name, "quat")) { \ + EXPECT_THAT(values, ElementsAre(1, 0, 0, 0)) << #name; \ + } else if constexpr (EndsWith(#name, "mat")) { \ + EXPECT_THAT(values, ElementsAre(1, 0, 0, 0, 1, 0, 0, 0, 1)) << #name; \ + } else if constexpr (std::string_view(#type) == "mjtNum") { \ + EXPECT_THAT(values, Each(MjNear(0.0, 1e-7, 1e-7))) << #name; \ + } \ + } MJDATA_POINTERS #undef X } - - mj_deleteData(data); - mj_deleteModel(model); } // --------------------------- mj_kinematics ----------------------------------- @@ -101,16 +97,13 @@ TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); - mj_resetDataDebug(model, data, 'd'); - mj_kinematics(model, data); + mj_resetDataDebug(model.get(), data.get(), 'd'); + mj_kinematics(model.get(), data.get()); EXPECT_THAT(AsVector(&data->xipos[0], 3), ElementsAre(0, 0, 0)); - - mj_deleteData(data); - mj_deleteModel(model); } // ----------------------------- mj_tendon ------------------------------------- @@ -145,19 +138,16 @@ TEST_F(CoreSmoothTest, FixedTendonSortedIndices) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; ASSERT_EQ(model->ntendon, 1); ASSERT_EQ(model->nwrap, 3); - mjData* data = mj_makeData(model); - mj_fwdPosition(model, data); + MjDataPtr data = MakeData(model); + mj_fwdPosition(model.get(), data.get()); mjtNum* J = data->ten_J; EXPECT_THAT(vector(J, J + 3), ElementsAre(1, 2, 3)); - - mj_deleteData(data); - mj_deleteModel(model); } static const char* const kTen_J0 = "engine/testdata/core_smooth/ten_J0.xml"; @@ -232,33 +222,33 @@ TEST_F(CoreSmoothTest, TendonArmature) { mj_forward(m, d); // get full M, includes both CRB and tendon inertia - vector M(nv*nv); + vector M(nv * nv); mj_fullM(m, d, M.data()); // put only CRB inertia in M2 mj_crb(m, d); mju_scatter(d->qM, d->M, m->mapM2M, m->nC); - vector M2(nv*nv); + vector M2(nv * nv); mj_fullM(m, d, M2.data()); - vector ten_J(nv); // tendon Jacobian - vector ten_M(nv*nv); // tendon inertia + vector ten_J(nv); // tendon Jacobian + vector ten_M(nv * nv); // tendon inertia // add tendon inertias to M2 using outer product - for (int j=0; j < m->ntendon; j++) { + for (int j = 0; j < m->ntendon; j++) { // get tendon Jacobian int rowadr = m->ten_J_rowadr[j]; int* rownnz = m->ten_J_rownnz + j; int zero = 0; - mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv, - rownnz, &zero, m->ten_J_colind + rowadr); + mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv, rownnz, &zero, + m->ten_J_colind + rowadr); // get tendon inertia only, using outer product mju_mulMatMat(ten_M.data(), ten_J.data(), ten_J.data(), nv, 1, nv); mju_scl(ten_M.data(), ten_M.data(), m->tendon_armature[j], nv * nv); // manually add values, at nonzeros only - for (int i=0; i < nv*nv; i++) { + for (int i = 0; i < nv * nv; i++) { if (M[i]) M2[i] += ten_M[i]; } } @@ -304,7 +294,7 @@ TEST_F(CoreSmoothTest, TendonArmatureConservesEnergy) { mj_step(m, d); ASSERT_GT(d->time, time) << "Divergence detected"; mjtNum energy_t = d->energy[0] + d->energy[1]; - EXPECT_NEAR(energy_t, energy_0, MjTol(eps, 3*eps)); + EXPECT_NEAR(energy_t, energy_0, MjTol(eps, 3 * eps)); } } mj_deleteData(d); @@ -378,13 +368,13 @@ TEST_F(CoreSmoothTest, TendonInertiaEquivalent) { mjtNum time = d->time; mj_step(m, d); ASSERT_GT(d->time, time) << "Divergence detected"; - vector xpos = AsVector(d->geom_xpos + 3*gid, 3); + vector xpos = AsVector(d->geom_xpos + 3 * gid, 3); time = d_e->time; mj_step(m_e, d_e); ASSERT_GT(d_e->time, time) << "Divergence detected"; - vector xpos_e = AsVector(d_e->geom_xpos + 3*gid_e, 3); - EXPECT_THAT(xpos, Pointwise(MjNear(eps, 10*eps), xpos_e)); + vector xpos_e = AsVector(d_e->geom_xpos + 3 * gid_e, 3); + EXPECT_THAT(xpos, Pointwise(MjNear(eps, 10 * eps), xpos_e)); } mj_deleteData(d); mj_deleteModel(m); @@ -393,20 +383,18 @@ TEST_F(CoreSmoothTest, TendonInertiaEquivalent) { } } - // --------------------------- connect constraint ------------------------------ // test that bodies hanging on connects lead to expected force sensor readings void TestConnect(const char* const filepath) { const std::string xml_path = GetTestDataFilePath(filepath); - mjModel* model = - mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); mjData* data = mj_makeData(model); // settle physics: - for (int i=0; i < 1000; i++) { + for (int i = 0; i < 1000; i++) { mj_step(model, data); } - for (int i=0; i < 3; i++) { + for (int i = 0; i < 3; i++) { EXPECT_THAT(data->sensordata[i] - model->sensor_user[i], MjNear(0, 1e-6, 2e-4)); } @@ -414,116 +402,100 @@ void TestConnect(const char* const filepath) { mj_deleteModel(model); } - TEST_F(CoreSmoothTest, RnePostConnectForceSlide) { constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/connect/force_slide.xml"; TestConnect(kModelFilePath); } - TEST_F(CoreSmoothTest, RnePostConnectForceSlideRotated) { constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/connect/force_slide_rotated.xml"; TestConnect(kModelFilePath); } - TEST_F(CoreSmoothTest, RnePostConnectForceFree) { constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/connect/force_free.xml"; TestConnect(kModelFilePath); } - TEST_F(CoreSmoothTest, RnePostConnectTorque) { constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/connect/torque_free.xml"; TestConnect(kModelFilePath); } - TEST_F(CoreSmoothTest, RnePostConnectMultipleConstraints) { constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/connect/multiple_constraints.xml"; TestConnect(kModelFilePath); } - // --------------------------- weld constraint --------------------------------- // test that bodies attached with welds lead to expected force sensor readings void TestWeld(const char* const filepath) { const std::string xml_path = GetTestDataFilePath(filepath); - mjModel* model = - mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); mjData* data = mj_makeData(model); // settle physics: - for (int i=0; i < 1000; i++) { + for (int i = 0; i < 1000; i++) { mj_step(model, data); } - for (int sensor_index=0; sensor_index < model->nsensor; sensor_index++) { - for (int i=0; i < 3; i++) { - EXPECT_NEAR( - data->sensordata[model->sensor_adr[sensor_index] + i], - model->sensor_user[model->nuser_sensor*sensor_index + i], - MjTol(1e-6, 5e-5)); + for (int sensor_index = 0; sensor_index < model->nsensor; sensor_index++) { + for (int i = 0; i < 3; i++) { + EXPECT_NEAR(data->sensordata[model->sensor_adr[sensor_index] + i], + model->sensor_user[model->nuser_sensor * sensor_index + i], + MjTol(1e-6, 5e-5)); } } mj_deleteData(data); mj_deleteModel(model); } - TEST_F(CoreSmoothTest, RnePostWeldForceFree) { constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/weld/force_free.xml"; TestWeld(kModelFilePath); } - TEST_F(CoreSmoothTest, RnePostWeldForceFreeRotated) { constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/weld/force_free_rotated.xml"; TestWeld(kModelFilePath); } - TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFree) { constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/weld/force_torque_free.xml"; TestWeld(kModelFilePath); } - TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFreeRotated) { constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/weld/force_torque_free_rotated.xml"; TestWeld(kModelFilePath); } - TEST_F(CoreSmoothTest, WeldRatioForceFree) { constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml"; TestConnect(kModelFilePath); } - TEST_F(CoreSmoothTest, WeldRatioForceSlide) { constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml"; TestConnect(kModelFilePath); } - TEST_F(CoreSmoothTest, WeldRatioTorqueFree) { constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml"; TestConnect(kModelFilePath); } - TEST_F(CoreSmoothTest, WeldRatioForceSlideRotated) { constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/weld/" @@ -555,7 +527,7 @@ TEST_F(CoreSmoothTest, EqualityBodySite) { mj_resetData(model, data); // turn site-defined equalities off, equivalent body-defined equalities on - for (int e=0; e < 4; e++) data->eq_active[e] = 1 - data->eq_active[e]; + for (int e = 0; e < 4; e++) data->eq_active[e] = 1 - data->eq_active[e]; // simulate again, get sensordata while (data->time < 0.1) { @@ -586,7 +558,7 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) { mjtNum targetpos[] = {.01, .02, .03}; mjtNum targetrot[] = {.1, .2, .3}; mju_copy3(data->ctrl, targetpos); - mju_copy3(data->ctrl+3, targetrot); + mju_copy3(data->ctrl + 3, targetrot); // step for 5 seconds while (data->time < 10) { @@ -601,13 +573,14 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) { // check that position matches target to within 1e-3 length units mjtNum relpos[3]; - mju_sub3(relpos, data->site_xpos+3*site_id, data->site_xpos+3*refsite_id); + mju_sub3(relpos, data->site_xpos + 3 * site_id, + data->site_xpos + 3 * refsite_id); EXPECT_THAT(relpos, Pointwise(MjNear(1e-3, 5e-3), targetpos)); // check that orientation matches target to within 0.06 radians mjtNum site_xquat[4], refsite_xquat[4], relrot[3]; - mju_mat2Quat(refsite_xquat, data->site_xmat+9*refsite_id); - mju_mat2Quat(site_xquat, data->site_xmat+9*site_id); + mju_mat2Quat(refsite_xquat, data->site_xmat + 9 * refsite_id); + mju_mat2Quat(site_xquat, data->site_xmat + 9 * site_id); mju_subQuat(relrot, site_xquat, refsite_xquat); EXPECT_THAT(relrot, Pointwise(MjNear(0.06, 0.06), targetrot)); @@ -632,7 +605,7 @@ TEST_F(CoreSmoothTest, RefsiteConservesMomentum) { mjtNum time = data->time; mj_step(model, data); ASSERT_GT(data->time, time) << "Divergence detected"; - for (int i=0; i < 6; i++) { + for (int i = 0; i < 6; i++) { EXPECT_LT(mju_abs(data->sensordata[i]), eps); } } @@ -654,33 +627,33 @@ TEST_F(CoreSmoothTest, FactorI) { // dense L matrix int nv = model->nv; - vector Ldense(nv*nv, 0); - mju_sparse2dense(Ldense.data(), data->qLD, nv, nv, - model->M_rownnz, model->M_rowadr, model->M_colind); - for (int i=0; i < nv; i++) { + vector Ldense(nv * nv, 0); + mju_sparse2dense(Ldense.data(), data->qLD, nv, nv, model->M_rownnz, + model->M_rowadr, model->M_colind); + for (int i = 0; i < nv; i++) { // set diagonal to 1 - Ldense[i*nv+i] = 1; + Ldense[i * nv + i] = 1; } // dense D matrix - vector Ddense(nv*nv); - mju_sparse2dense(Ddense.data(), data->qLD, nv, nv, - model->M_rownnz, model->M_rowadr, model->M_colind); - for (int i=0; i < nv; i++) { - for (int j=0; j < nv; j++) { + vector Ddense(nv * nv); + mju_sparse2dense(Ddense.data(), data->qLD, nv, nv, model->M_rownnz, + model->M_rowadr, model->M_colind); + for (int i = 0; i < nv; i++) { + for (int j = 0; j < nv; j++) { // zero everything except the diagonal - if (i != j) Ddense[i*nv+j] = 0; + if (i != j) Ddense[i * nv + j] = 0; } } // perform multiplication: M = L^T * D * L - vector tmp(nv*nv); - vector M(nv*nv); + vector tmp(nv * nv); + vector M(nv * nv); mju_mulMatMat(tmp.data(), Ddense.data(), Ldense.data(), nv, nv, nv); mju_mulMatTMat(M.data(), Ldense.data(), tmp.data(), nv, nv, nv); // dense M matrix - vector Mexpected(nv*nv); + vector Mexpected(nv * nv); mj_fullM(model, data, Mexpected.data()); // expect matrices to match to floating point precision @@ -693,12 +666,12 @@ TEST_F(CoreSmoothTest, FactorI) { // Convert legacy-format symmetric matrix to dense (local helper for tests). static void legacyToDense(const mjModel* m, mjtNum* dst, const mjtNum* M) { int adr = 0, nv = m->nv; - mju_zero(dst, nv*nv); + mju_zero(dst, nv * nv); for (int i = 0; i < nv; i++) { int j = i; while (j >= 0) { - dst[i*nv+j] = M[adr]; - dst[j*nv+i] = M[adr]; + dst[i * nv + j] = M[adr]; + dst[j * nv + i] = M[adr]; j = m->dof_parentid[j]; adr++; } @@ -723,31 +696,32 @@ TEST_F(CoreSmoothTest, SolveLDs) { mju_scatter(LDlegacy.data(), d->qLD, m->mapM2M, nC); // compare LD and LDs densified matrices - vector LDdense(nv*nv); - mju_sparse2dense(LDdense.data(), d->qLD, nv, nv, - m->M_rownnz, m->M_rowadr, m->M_colind); - vector LDdense2(nv*nv); + vector LDdense(nv * nv); + mju_sparse2dense(LDdense.data(), d->qLD, nv, nv, m->M_rownnz, m->M_rowadr, + m->M_colind); + vector LDdense2(nv * nv); legacyToDense(m, LDdense2.data(), LDlegacy.data()); // expect lower triangles to match exactly - for (int i=0; i < nv; i++) { - for (int j=0; j < i; j++) { - EXPECT_NEAR(LDdense[i*nv+j], LDdense2[i*nv+j], MjTol(1e-14, 1e-6)); + for (int i = 0; i < nv; i++) { + for (int j = 0; j < i; j++) { + EXPECT_NEAR(LDdense[i * nv + j], LDdense2[i * nv + j], + MjTol(1e-14, 1e-6)); } } // compare legacy and CSR LD vector solve vector vec(nv); vector vec2(nv); - for (int i=0; i < nv; i++) vec[i] = vec2[i] = 20 + 30*i; - for (int i=0; i < nv; i+=2) vec[i] = vec2[i] = 0; + for (int i = 0; i < nv; i++) vec[i] = vec2[i] = 20 + 30 * i; + for (int i = 0; i < nv; i += 2) vec[i] = vec2[i] = 0; mj_solveLD_legacy(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv); - mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1, - m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); + mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1, m->M_rownnz, m->M_rowadr, + m->M_colind, nullptr); // expect vectors to match up to floating point precision - for (int i=0; i < nv; i++) { + for (int i = 0; i < nv; i++) { EXPECT_NEAR(vec[i], vec2[i], MjTol(1e-14, 5e-6)); } @@ -772,17 +746,17 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) { // compare n LD and LDs vector solve int n = 3; - vector vec(nv*n); - vector vec2(nv*n); - for (int i=0; i < nv*n; i++) vec[i] = vec2[i] = 2 + 3*i; - for (int i=0; i < nv*n; i+=3) vec[i] = vec2[i] = 0; + vector vec(nv * n); + vector vec2(nv * n); + for (int i = 0; i < nv * n; i++) vec[i] = vec2[i] = 2 + 3 * i; + for (int i = 0; i < nv * n; i += 3) vec[i] = vec2[i] = 0; mj_solveLD_legacy(m, vec.data(), n, LDlegacy.data(), d->qLDiagInv); - mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, - m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); + mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, m->M_rownnz, m->M_rowadr, + m->M_colind, nullptr); // expect vectors to match up to floating point precision - for (int i=0; i < nv*n; i++) { + for (int i = 0; i < nv * n; i++) { EXPECT_NEAR(vec[i], vec2[i], MjTol(1e-14, 5e-6)); } @@ -802,27 +776,27 @@ TEST_F(CoreSmoothTest, SolveM2) { // inverse square root of D from inertia LDL decomposition int nv = m->nv; vector sqrtInvD(nv); - for (int i=0; i < nv; i++) { + for (int i = 0; i < nv; i++) { int diag = m->M_rowadr[i] + m->M_rownnz[i] - 1; sqrtInvD[i] = 1 / mju_sqrt(d->qLD[diag]); } // compare full solve and half solve int n = 3; - vector vec(nv*n); - vector vec2(nv*n); - for (int i=0; i < nv*n; i++) vec[i] = vec2[i] = 2 + 3*i; - for (int i=0; i < nv*n; i+=3) vec[i] = vec2[i] = 0; - vector res(nv*n); + vector vec(nv * n); + vector vec2(nv * n); + for (int i = 0; i < nv * n; i++) vec[i] = vec2[i] = 2 + 3 * i; + for (int i = 0; i < nv * n; i += 3) vec[i] = vec2[i] = 0; + vector res(nv * n); mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n); - mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, - m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); + mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, m->M_rownnz, m->M_rowadr, + m->M_colind, nullptr); // expect equality of dot(v, M^-1 * v) and dot(M^-1/2 * v, M^-1/2 * v) - for (int i=0; i < n; i++) { - EXPECT_NEAR(mju_dot(vec2.data() + i*nv, vec.data() + i*nv, nv), - mju_dot(res.data() + i*nv, res.data() + i*nv, nv), + for (int i = 0; i < n; i++) { + EXPECT_NEAR(mju_dot(vec2.data() + i * nv, vec.data() + i * nv, nv), + mju_dot(res.data() + i * nv, res.data() + i * nv, nv), MjTol(1e-10, 1e-2)); } @@ -856,8 +830,8 @@ TEST_F(CoreSmoothTest, FactorIs) { vector qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv); vector qLDiagInv(nv, 0); - mj_factorI(qLD.data(), qLDiagInv.data(), nv, - m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); + mj_factorI(qLD.data(), qLDiagInv.data(), nv, m->M_rownnz, m->M_rowadr, + m->M_colind, nullptr); // expect outputs to match to floating point precision EXPECT_THAT(qLD, Pointwise(MjNear(1e-12, 1e-4), qLDexpected)); @@ -891,9 +865,9 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); // check that nJfv is correct: // corner vertices: 2 * (1+3) * 3 + 2 * (1+2) * 3 = 42 @@ -903,33 +877,33 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) { EXPECT_EQ(m->nJfv, 123); // Run kinematics to populate xpos/xmat initially - mj_fwdKinematics(m, d); + mj_fwdKinematics(m.get(), d.get()); // Check invariants for scale=1 // The constraints should be satisfied int nvert = m->flex_vertnum[0]; ASSERT_EQ(nvert, 9); - for (int i=0; i < nvert; i++) { - EXPECT_NEAR(d->flexvert_length[2*i+0], 0.0, MjTol(1e-5, 5e-5)); - EXPECT_NEAR(d->flexvert_length[2*i+1], 0.0, MjTol(1e-5, 5e-5)); + for (int i = 0; i < nvert; i++) { + EXPECT_NEAR(d->flexvert_length[2 * i + 0], 0.0, MjTol(1e-5, 5e-5)); + EXPECT_NEAR(d->flexvert_length[2 * i + 1], 0.0, MjTol(1e-5, 5e-5)); } // set qvel to rigid rotation - ASSERT_EQ(m->nv, 3*nvert); + ASSERT_EQ(m->nv, 3 * nvert); mju_zero(d->qvel, m->nv); - for (int i=0; i < nvert; i++) { - const mjtNum* p = d->xpos + 3*m->flex_vertbodyid[i]; - d->qvel[3*i+0] = -p[1]; - d->qvel[3*i+1] = p[0]; - d->qvel[3*i+2] = 1.0; + for (int i = 0; i < nvert; i++) { + const mjtNum* p = d->xpos + 3 * m->flex_vertbodyid[i]; + d->qvel[3 * i + 0] = -p[1]; + d->qvel[3 * i + 1] = p[0]; + d->qvel[3 * i + 2] = 1.0; } // check that Jacobian times velocity is zero for rigid body motion - vector Jv(2*nvert, 0); - for (int i=0; i < 2*nvert; i++) { + vector Jv(2 * nvert, 0); + for (int i = 0; i < 2 * nvert; i++) { int row_start = m->flexvert_J_rowadr[i]; int row_nnz = m->flexvert_J_rownnz[i]; - for (int j=0; j < row_nnz; j++) { + for (int j = 0; j < row_nnz; j++) { Jv[i] += d->flexvert_J[row_start + j] * d->qvel[m->flexvert_J_colind[row_start + j]]; } @@ -941,37 +915,37 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) { int edges[] = {1, 3, 5, 7}; int center[] = {4}; for (int i : corners) { - EXPECT_EQ(m->flexvert_J_rownnz[2*i+0], (i == 0 || i == 8) ? 12 : 9); - EXPECT_EQ(m->flexvert_J_rownnz[2*i+1], (i == 0 || i == 8) ? 12 : 9); + EXPECT_EQ(m->flexvert_J_rownnz[2 * i + 0], (i == 0 || i == 8) ? 12 : 9); + EXPECT_EQ(m->flexvert_J_rownnz[2 * i + 1], (i == 0 || i == 8) ? 12 : 9); } for (int i : edges) { - EXPECT_EQ(m->flexvert_J_rownnz[2*i+0], 15); - EXPECT_EQ(m->flexvert_J_rownnz[2*i+1], 15); + EXPECT_EQ(m->flexvert_J_rownnz[2 * i + 0], 15); + EXPECT_EQ(m->flexvert_J_rownnz[2 * i + 1], 15); } for (int i : center) { - EXPECT_EQ(m->flexvert_J_rownnz[2*i+0], 21); - EXPECT_EQ(m->flexvert_J_rownnz[2*i+1], 21); + EXPECT_EQ(m->flexvert_J_rownnz[2 * i + 0], 21); + EXPECT_EQ(m->flexvert_J_rownnz[2 * i + 1], 21); } // check rowadr EXPECT_EQ(m->flexvert_J_rowadr[0], 0); - for (int i=1; i < 2*nvert; i++) { + for (int i = 1; i < 2 * nvert; i++) { EXPECT_EQ(m->flexvert_J_rowadr[i], - m->flexvert_J_rowadr[i-1] + m->flexvert_J_rownnz[i-1]); + m->flexvert_J_rowadr[i - 1] + m->flexvert_J_rownnz[i - 1]); } // check that colind are sorted and unique int nnzJ = 0; - for (int i = 0; i < 2*nvert; i++) { + for (int i = 0; i < 2 * nvert; i++) { nnzJ += m->flexvert_J_rownnz[i]; } - EXPECT_EQ(nnzJ, 2*m->nJfv); - for (int i=0; i < 2*nvert; i++) { + EXPECT_EQ(nnzJ, 2 * m->nJfv); + for (int i = 0; i < 2 * nvert; i++) { int row_start = m->flexvert_J_rowadr[i]; int row_nnz = m->flexvert_J_rownnz[i]; - for (int j=0; j < row_nnz-1; j++) { - EXPECT_LE(m->flexvert_J_colind[row_start+j], - m->flexvert_J_colind[row_start+j+1]); + for (int j = 0; j < row_nnz - 1; j++) { + EXPECT_LE(m->flexvert_J_colind[row_start + j], + m->flexvert_J_colind[row_start + j + 1]); } } @@ -979,8 +953,8 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) { auto fd_check = [&](double tolerance) { std::vector qpos0(m->nq); mju_copy(qpos0.data(), d->qpos, m->nq); - mj_kinematics(m, d); - mj_flex(m, d); + mj_kinematics(m.get(), d.get()); + mj_flex(m.get(), d.get()); mjtNum eps = MjTol(1e-6, 1e-4); int nflexvert = m->flex_vertnum[0]; @@ -988,15 +962,15 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) { std::vector qpos_backup(m->nq); mju_copy(qpos_backup.data(), d->qpos, m->nq); - for (int i=0; i < m->nv; ++i) { + for (int i = 0; i < m->nv; ++i) { std::vector qvel(m->nv, 0); qvel[i] = 1.0; // plus mju_copy(d->qpos, qpos_backup.data(), m->nq); - mj_integratePos(m, d->qpos, qvel.data(), eps); - mj_kinematics(m, d); - mj_flex(m, d); + mj_integratePos(m.get(), d->qpos, qvel.data(), eps); + mj_kinematics(m.get(), d.get()); + mj_flex(m.get(), d.get()); std::vector L_plus(2 * nflexvert); for (int e = 0; e < 2 * nflexvert; ++e) { L_plus[e] = d->flexvert_length[e]; @@ -1004,21 +978,21 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) { // minus mju_copy(d->qpos, qpos_backup.data(), m->nq); - mj_integratePos(m, d->qpos, qvel.data(), -eps); - mj_kinematics(m, d); - mj_flex(m, d); + mj_integratePos(m.get(), d->qpos, qvel.data(), -eps); + mj_kinematics(m.get(), d.get()); + mj_flex(m.get(), d.get()); std::vector L_minus(2 * nflexvert); for (int e = 0; e < 2 * nflexvert; ++e) { L_minus[e] = d->flexvert_length[e]; } for (int e = 0; e < 2 * nflexvert; ++e) { - jac_fd[e*m->nv + i] = (L_plus[e] - L_minus[e]) / (2*eps); + jac_fd[e * m->nv + i] = (L_plus[e] - L_minus[e]) / (2 * eps); } } mju_copy(d->qpos, qpos_backup.data(), m->nq); - mj_kinematics(m, d); - mj_flex(m, d); + mj_kinematics(m.get(), d.get()); + mj_flex(m.get(), d.get()); // Compare with analytic std::vector jac_analytic(2 * nflexvert * m->nv); @@ -1027,27 +1001,27 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) { int row_start = m->flexvert_J_rowadr[e]; int row_nnz = m->flexvert_J_rownnz[e]; for (int i = 0; i < row_nnz; ++i) { - jac_analytic[e*m->nv + m->flexvert_J_colind[row_start+i]] = - d->flexvert_J[row_start+i]; + jac_analytic[e * m->nv + m->flexvert_J_colind[row_start + i]] = + d->flexvert_J[row_start + i]; } } EXPECT_THAT(jac_analytic, Not(Each(MjNear(0.0, 1e-7, 1e-4)))); EXPECT_THAT(jac_analytic, Pointwise(MjNear(tolerance, 1e-1), jac_fd)); mju_copy(d->qpos, qpos0.data(), m->nq); - mj_kinematics(m, d); - mj_flex(m, d); + mj_kinematics(m.get(), d.get()); + mj_flex(m.get(), d.get()); }; fd_check(MjTol(5e-5, 5e-2)); // Set qpos to put flex in scale=2 configuration. - for (int i=0; i < nvert; i++) { - d->qpos[3*i+0] = d->xpos[3*(i+1)+0]; - d->qpos[3*i+1] = d->xpos[3*(i+1)+1]; - d->qpos[3*i+2] = d->xpos[3*(i+1)+2]; + for (int i = 0; i < nvert; i++) { + d->qpos[3 * i + 0] = d->xpos[3 * (i + 1) + 0]; + d->qpos[3 * i + 1] = d->xpos[3 * (i + 1) + 1]; + d->qpos[3 * i + 2] = d->xpos[3 * (i + 1) + 2]; } - mj_fwdKinematics(m, d); + mj_fwdKinematics(m.get(), d.get()); // Get mass scaling factor mjtNum scale = 1.0; @@ -1062,19 +1036,16 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) { // Invariant 0: Trace(E) = 3 + 3 = 6 // Invariant 1: Det(C) - 1 = 4 * 4 - 1 = 15 // Note: constraints are now scaled by sqrt(mass) - for (int i=0; i < nvert; i++) { + for (int i = 0; i < nvert; i++) { EXPECT_NEAR(d->flexvert_length[2 * i + 0], 6.0 * scale, MjTol(1e-5, 5e-4)); EXPECT_NEAR(d->flexvert_length[2 * i + 1], 15.0 * scale, MjTol(1e-5, 5e-4)); } // Perturb z-positions so configuration is not flat - for (int i=0; i < nvert; i++) { - d->qpos[3*i+2] += 0.01 * (i%2 ? 1 : -1); + for (int i = 0; i < nvert; i++) { + d->qpos[3 * i + 2] += 0.01 * (i % 2 ? 1 : -1); } fd_check(5e-5); - - mj_deleteData(d); - mj_deleteModel(m); } // Test failure case for flexvert_J sparsity with skipped flexes @@ -1107,12 +1078,12 @@ TEST_F(CoreSmoothTest, FlexvertJSparsitySkippedFlex) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // Forward dynamics to compute Jacobians - mj_forward(model, data); + mj_forward(model.get(), data.get()); // Check sparsity overlap // Flex 0 starts at row 0 @@ -1136,9 +1107,6 @@ TEST_F(CoreSmoothTest, FlexvertJSparsitySkippedFlex) { // Verify that Flex 2 starts AFTER Flex 0 ends EXPECT_GE(f2_start_adr, f0_end_adr) << "Flex 2 Jacobian overwrites Flex 0 Jacobian due to skipped Flex 1"; - - mj_deleteData(data); - mj_deleteModel(model); } // Test stability of flexvert constraint under different integrator/solver diff --git a/test/engine/engine_core_util_test.cc b/test/engine/engine_core_util_test.cc index d3fd2df7..7351d793 100644 --- a/test/engine/engine_core_util_test.cc +++ b/test/engine/engine_core_util_test.cc @@ -49,10 +49,10 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); - mj_forward(model, data); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mj_forward(model.get(), data.get()); ASSERT_EQ(model->nflex, 1); int f = 0; @@ -63,29 +63,26 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) { ASSERT_TRUE(model->flex_centered[f]); for (int i = 0; i < nodenum; i++) { int b = model->flex_nodebodyid[nstart + i]; - mjtNum x = data->xpos[3*b + 0]; - mjtNum y = data->xpos[3*b + 1]; - mjtNum z = data->xpos[3*b + 2]; + mjtNum x = data->xpos[3 * b + 0]; + mjtNum y = data->xpos[3 * b + 1]; + mjtNum z = data->xpos[3 * b + 2]; // Rotate 90 degrees around Z: (x, y, z) -> (-y, x, z) - data->xpos[3*b + 0] = -y; - data->xpos[3*b + 1] = x; - data->xpos[3*b + 2] = z; + data->xpos[3 * b + 0] = -y; + data->xpos[3 * b + 1] = x; + data->xpos[3 * b + 2] = z; } std::vector xpos(3 * nodenum); - mju_flexGatherState(model, data, f, xpos.data(), NULL); + mju_flexGatherState(model.get(), data.get(), f, xpos.data(), NULL); // Verify that gathered xpos matches the rotated data->xpos for (int i = 0; i < nodenum; i++) { int b = model->flex_nodebodyid[nstart + i]; - EXPECT_NEAR(xpos[3*i + 0], data->xpos[3*b + 0], 1e-5); - EXPECT_NEAR(xpos[3*i + 1], data->xpos[3*b + 1], 1e-5); - EXPECT_NEAR(xpos[3*i + 2], data->xpos[3*b + 2], 1e-5); + EXPECT_NEAR(xpos[3 * i + 0], data->xpos[3 * b + 0], 1e-5); + EXPECT_NEAR(xpos[3 * i + 1], data->xpos[3 * b + 1], 1e-5); + EXPECT_NEAR(xpos[3 * i + 2], data->xpos[3 * b + 2], 1e-5); } - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) { @@ -102,15 +99,15 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; ASSERT_EQ(model->nflex, 1); int f = 0; model->flex_interp[f] = -1; - mjData* data = mj_makeData(model); - mj_forward(model, data); + MjDataPtr data = MakeData(model); + mj_forward(model.get(), data.get()); int nodenum = model->flex_nodenum[f]; int nstart = model->flex_nodeadr[f]; @@ -118,26 +115,23 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) { // Move boundary nodes, keep interior node stuck (it is pinned) mjtNum shift[3] = {0.1, 0.2, 0.3}; for (int i = 0; i < nodenum; i++) { - if (i == 13) continue; // Skip center node + if (i == 13) continue; // Skip center node int b = model->flex_nodebodyid[nstart + i]; - data->xpos[3*b + 0] += shift[0]; - data->xpos[3*b + 1] += shift[1]; - data->xpos[3*b + 2] += shift[2]; + data->xpos[3 * b + 0] += shift[0]; + data->xpos[3 * b + 1] += shift[1]; + data->xpos[3 * b + 2] += shift[2]; } std::vector xpos(3 * nodenum); - mju_flexGatherState(model, data, f, xpos.data(), NULL); + mju_flexGatherState(model.get(), data.get(), f, xpos.data(), NULL); - // Verify that gathered xpos for center node (13) is the TFI reconstructed position - EXPECT_NEAR(xpos[3*13 + 0], shift[0], 1e-5); - EXPECT_NEAR(xpos[3*13 + 1], shift[1], 1e-5); - EXPECT_NEAR(xpos[3*13 + 2], shift[2], 1e-5); - - mj_deleteData(data); - mj_deleteModel(model); + // Verify that gathered xpos for center node (13) is the TFI reconstructed + // position + EXPECT_NEAR(xpos[3 * 13 + 0], shift[0], 1e-5); + EXPECT_NEAR(xpos[3 * 13 + 1], shift[1], 1e-5); + EXPECT_NEAR(xpos[3 * 13 + 2], shift[2], 1e-5); } - using AngMomMatTest = MujocoTest; static constexpr char AngMomTestingModel[] = R"( @@ -172,26 +166,26 @@ static constexpr char AngMomTestingModel[] = R"( // compare subtree angular momentum computed in two ways TEST_F(AngMomMatTest, CompareAngMom) { char error[1024]; - mjModel* model = + MjModelPtr model = LoadModelFromString(AngMomTestingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + ASSERT_THAT(model.get(), NotNull()) << error; int nv = model->nv; - int bodyid = mj_name2id(model, mjOBJ_BODY, "link1"); + int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "link1"); - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); // reset to the keyframe with some angular velocities - mj_resetDataKeyframe(model, data, 0); - mj_forward(model, data); + mj_resetDataKeyframe(model.get(), data.get(), 0); + mj_forward(model.get(), data.get()); // get the reference value of angular momentum - mj_subtreeVel(model, data); + mj_subtreeVel(model.get(), data.get()); mjtNum angmom_ref[3]; - mju_copy3(angmom_ref, data->subtree_angmom+3*bodyid); + mju_copy3(angmom_ref, data->subtree_angmom + 3 * bodyid); // compute angular momentum using the angular momentum matrix - mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); - mj_angmomMat(model, data, angmom_mat, bodyid); + mjtNum* angmom_mat = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv); + mj_angmomMat(model.get(), data.get(), angmom_mat, bodyid); mjtNum angmom_test[3]; mju_mulMatVec(angmom_test, angmom_mat, data->qvel, 3, nv); @@ -201,28 +195,26 @@ TEST_F(AngMomMatTest, CompareAngMom) { } mju_free(angmom_mat); - mj_deleteData(data); - mj_deleteModel(model); } // compare subtree angular momentum matrix: analytical and findiff TEST_F(AngMomMatTest, CompareAngMomMats) { char error[1024]; - mjModel* model = + MjModelPtr model = LoadModelFromString(AngMomTestingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + ASSERT_THAT(model.get(), NotNull()) << error; int nv = model->nv; - int bodyid = mj_name2id(model, mjOBJ_BODY, "link1"); - mjData* data = mj_makeData(model); - mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); - mjtNum* angmom_mat_fd = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); + int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "link1"); + MjDataPtr data = MakeData(model); + mjtNum* angmom_mat = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv); + mjtNum* angmom_mat_fd = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv); // reset to the keyframe with some angular velocities - mj_resetDataKeyframe(model, data, 0); - mj_forward(model, data); + mj_resetDataKeyframe(model.get(), data.get(), 0); + mj_forward(model.get(), data.get()); // compute the angular momentum matrix using the analytical method - mj_angmomMat(model, data, angmom_mat, bodyid); + mj_angmomMat(model.get(), data.get(), angmom_mat, bodyid); // compute the angular momentum matrix using finite differences static const mjtNum eps = MjTol(1e-6, 1e-3); @@ -230,34 +222,32 @@ TEST_F(AngMomMatTest, CompareAngMomMats) { // reset vel, forward nudge i-th dof, get angmom mju_copy(data->qvel, model->key_qvel, model->nv); data->qvel[i] += eps; - mj_forward(model, data); - mj_subtreeVel(model, data); + mj_forward(model.get(), data.get()); + mj_subtreeVel(model.get(), data.get()); mjtNum agmf[3]; - mju_copy3(agmf, data->subtree_angmom+3*bodyid); + mju_copy3(agmf, data->subtree_angmom + 3 * bodyid); // reset vel, backward nudge i-th dof, get angmom mju_copy(data->qvel, model->key_qvel, model->nv); data->qvel[i] -= eps; - mj_forward(model, data); - mj_subtreeVel(model, data); + mj_forward(model.get(), data.get()); + mj_subtreeVel(model.get(), data.get()); mjtNum agmb[3]; - mju_copy3(agmb, data->subtree_angmom+3*bodyid); + mju_copy3(agmb, data->subtree_angmom + 3 * bodyid); // finite-difference the angmom matrix for (int j = 0; j < 3; j++) { - angmom_mat_fd[nv*j+i] = (agmf[j] - agmb[j]) / (2 * eps); + angmom_mat_fd[nv * j + i] = (agmf[j] - agmb[j]) / (2 * eps); } } // compare the two matrices - for (int i = 0; i < 3*nv; i++) { + for (int i = 0; i < 3 * nv; i++) { EXPECT_THAT(angmom_mat_fd[i], MjNear(angmom_mat[i], 1e-8, 2e-4)); } mju_free(angmom_mat_fd); mju_free(angmom_mat); - mj_deleteData(data); - mj_deleteModel(model); } using JacobianTest = MujocoTest; @@ -297,100 +287,97 @@ static constexpr char kJacobianTestingModel[] = R"( // compare analytic and finite-differenced subtree-com Jacobian TEST_F(JacobianTest, SubtreeJac) { char error[1024]; - mjModel* model = + MjModelPtr model = LoadModelFromString(kJacobianTestingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + ASSERT_THAT(model.get(), NotNull()) << error; int nv = model->nv; - int bodyid = mj_name2id(model, mjOBJ_BODY, "main"); - mjData* data = mj_makeData(model); - mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); - mjtNum* qpos = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nq); - mjtNum* nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv); + int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "main"); + MjDataPtr data = MakeData(model); + mjtNum* jac_subtree = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv); + mjtNum* qpos = (mjtNum*)mju_malloc(sizeof(mjtNum) * model->nq); + mjtNum* nudge = (mjtNum*)mju_malloc(sizeof(mjtNum) * nv); // all we need for Jacobians are kinematics and CoM-related quantities - mj_kinematics(model, data); - mj_comPos(model, data); + mj_kinematics(model.get(), data.get()); + mj_comPos(model.get(), data.get()); // get subtree CoM Jacobian of free body - mj_jacSubtreeCom(model, data, jac_subtree, bodyid); + mj_jacSubtreeCom(model.get(), data.get(), jac_subtree, bodyid); // save current subtree-com and qpos, clear nudge mjtNum subtree_com[3]; - mju_copy3(subtree_com, data->subtree_com+3*bodyid); + mju_copy3(subtree_com, data->subtree_com + 3 * bodyid); mju_copy(qpos, data->qpos, model->nq); mju_zero(nudge, nv); // compare analytic Jacobian to finite-difference approximation static const mjtNum eps = 1e-6; - for (int i=0; i < nv; i++) { + for (int i = 0; i < nv; i++) { // reset qpos, nudge i-th dof, update data->qpos, reset nudge mju_copy(data->qpos, qpos, model->nq); nudge[i] = 1; - mj_integratePos(model, data->qpos, nudge, eps); + mj_integratePos(model.get(), data->qpos, nudge, eps); nudge[i] = 0; // kinematics and comPos to get nudged com - mj_kinematics(model, data); - mj_comPos(model, data); + mj_kinematics(model.get(), data.get()); + mj_comPos(model.get(), data.get()); // compare finite-differenced and analytic Jacobian - for (int j=0; j < 3; j++) { - mjtNum findiff = (data->subtree_com[3*bodyid+j] - subtree_com[j]) / eps; - EXPECT_THAT(jac_subtree[nv*j+i], MjNear(findiff, eps, 1e-2)); + for (int j = 0; j < 3; j++) { + mjtNum findiff = + (data->subtree_com[3 * bodyid + j] - subtree_com[j]) / eps; + EXPECT_THAT(jac_subtree[nv * j + i], MjNear(findiff, eps, 1e-2)); } } mju_free(nudge); mju_free(qpos); mju_free(jac_subtree); - mj_deleteData(data); - mj_deleteModel(model); } // confirm that applying linear forces via the subtree-com Jacobian only creates // the expected linear accelerations (no accelerations of internal joints) TEST_F(JacobianTest, SubtreeJacNoInternalAcc) { char error[1024]; - mjModel* model = + MjModelPtr model = LoadModelFromString(kJacobianTestingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + ASSERT_THAT(model.get(), NotNull()) << error; int nv = model->nv; - int bodyid = mj_name2id(model, mjOBJ_BODY, "main"); - mjData* data = mj_makeData(model); - mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); + int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "main"); + MjDataPtr data = MakeData(model); + mjtNum* jac_subtree = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv); // all we need for Jacobians are kinematics and CoM-related quantities - mj_kinematics(model, data); - mj_comPos(model, data); + mj_kinematics(model.get(), data.get()); + mj_comPos(model.get(), data.get()); // get subtree CoM Jacobian of free body - mj_jacSubtreeCom(model, data, jac_subtree, bodyid); + mj_jacSubtreeCom(model.get(), data.get(), jac_subtree, bodyid); // uncomment for debugging // mju_printMat(jac_subtree, 3, nv); // call fwdPosition since we'll need the factorised mass matrix in the test - mj_fwdPosition(model, data); + mj_fwdPosition(model.get(), data.get()); // treating the subtree Jacobian as the projection of 3 axis-aligned unit // forces into joint space, solve for the resulting accelerations in-place - mj_solveM(model, data, jac_subtree, jac_subtree, 3); + mj_solveM(model.get(), data.get(), jac_subtree, jac_subtree, 3); // expect to find accelerations of magnitude 1/subtreemass in the first 3 // coordinates of the free joint and 0s elsewhere, since applying forces to // the CoM should accelerate the whole mechanism without any internal motion int body_dofadr = model->body_dofadr[bodyid]; - mjtNum invtreemass = 1.0/model->body_subtreemass[bodyid]; + mjtNum invtreemass = 1.0 / model->body_subtreemass[bodyid]; for (int r = 0; r < 3; r++) { for (int c = 0; c < nv; c++) { mjtNum expected = c - body_dofadr == r ? invtreemass : 0.0; - EXPECT_THAT(jac_subtree[nv*r+c], MjNear(expected, max_abs_err, 1e-4)); + EXPECT_THAT(jac_subtree[nv * r + c], MjNear(expected, max_abs_err, 1e-4)); } } mju_free(jac_subtree); - mj_deleteData(data); - mj_deleteModel(model); } static constexpr char kQuat[] = R"( @@ -519,65 +506,65 @@ static constexpr char kHinge[] = R"( TEST_F(JacobianTest, JacDot) { for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) { char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; int nv = model->nv; - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); // load keyframe if present, step for a bit - if (model->nkey) mj_resetDataKeyframe(model, data, 0); + if (model->nkey) mj_resetDataKeyframe(model.get(), data.get(), 0); while (data->time < 0.1) { - mj_step(model, data); + mj_step(model.get(), data.get()); } // minimal call required for mj_jacDot outputs to be valid - mj_kinematics(model, data); - mj_comPos(model, data); - mj_comVel(model, data); + mj_kinematics(model.get(), data.get()); + mj_comPos(model.get(), data.get()); + mj_comVel(model.get(), data.get()); // get bodyid - int bodyid = mj_name2id(model, mjOBJ_BODY, "query"); + int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "query"); EXPECT_GT(bodyid, 0); // get site position - int siteid = mj_name2id(model, mjOBJ_SITE, "query"); + int siteid = mj_name2id(model.get(), mjOBJ_SITE, "query"); EXPECT_GT(siteid, -1); mjtNum point[3]; - mju_copy3(point, data->site_xpos+3*siteid); + mju_copy3(point, data->site_xpos + 3 * siteid); // jac, jac_dot - std::vector jacp(3*nv); - std::vector jacr(3*nv); - mj_jac(model, data, jacp.data(), jacr.data(), point, bodyid); - std::vector jacp_dot(3*nv); - std::vector jacr_dot(3*nv); - mj_jacDot(model, data, jacp_dot.data(), jacr_dot.data(), point, bodyid); + std::vector jacp(3 * nv); + std::vector jacr(3 * nv); + mj_jac(model.get(), data.get(), jacp.data(), jacr.data(), point, bodyid); + std::vector jacp_dot(3 * nv); + std::vector jacr_dot(3 * nv); + mj_jacDot(model.get(), data.get(), jacp_dot.data(), jacr_dot.data(), point, + bodyid); // jac_h: jacobian after integrating qpos with a timestep of h const mjtNum h = MjTol(1e-7, 5e-4); - mj_integratePos(model, data->qpos, data->qvel, h); - mj_kinematics(model, data); - mj_comPos(model, data); - std::vector jacp_h(3*nv); - std::vector jacr_h(3*nv); - mju_copy3(point, data->site_xpos+3*siteid); // get updated site position - mj_jac(model, data, jacp_h.data(), jacr_h.data(), point, bodyid); + mj_integratePos(model.get(), data->qpos, data->qvel, h); + mj_kinematics(model.get(), data.get()); + mj_comPos(model.get(), data.get()); + std::vector jacp_h(3 * nv); + std::vector jacr_h(3 * nv); + mju_copy3(point, + data->site_xpos + 3 * siteid); // get updated site position + mj_jac(model.get(), data.get(), jacp_h.data(), jacr_h.data(), point, + bodyid); // jac_dot_h finite-difference approximation - std::vector jacp_dot_h(3*nv); - mju_sub(jacp_dot_h.data(), jacp_h.data(), jacp.data(), 3*nv); - mju_scl(jacp_dot_h.data(), jacp_dot_h.data(), 1/h, 3*nv); - std::vector jacr_dot_h(3*nv); - mju_sub(jacr_dot_h.data(), jacr_h.data(), jacr.data(), 3*nv); - mju_scl(jacr_dot_h.data(), jacr_dot_h.data(), 1/h, 3*nv); + std::vector jacp_dot_h(3 * nv); + mju_sub(jacp_dot_h.data(), jacp_h.data(), jacp.data(), 3 * nv); + mju_scl(jacp_dot_h.data(), jacp_dot_h.data(), 1 / h, 3 * nv); + std::vector jacr_dot_h(3 * nv); + mju_sub(jacr_dot_h.data(), jacr_h.data(), jacr.data(), 3 * nv); + mju_scl(jacr_dot_h.data(), jacr_dot_h.data(), 1 / h, 3 * nv); // compare finite-differenced and analytic mjtNum tol = 1e-5; EXPECT_THAT(jacp_dot, Pointwise(MjNear(tol, 5e-2), jacp_dot_h)); EXPECT_THAT(jacr_dot, Pointwise(MjNear(tol, 5e-2), jacr_dot_h)); - - mj_deleteData(data); - mj_deleteModel(model); } } @@ -585,34 +572,35 @@ TEST_F(JacobianTest, JacDot) { TEST_F(JacobianTest, JacDotSparse) { for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) { char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; int nv = model->nv; - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); // load keyframe if present, step for a bit - if (model->nkey) mj_resetDataKeyframe(model, data, 0); + if (model->nkey) mj_resetDataKeyframe(model.get(), data.get(), 0); while (data->time < 0.1) { - mj_step(model, data); + mj_step(model.get(), data.get()); } // minimal call required for mj_jacDot outputs to be valid - mj_kinematics(model, data); - mj_comPos(model, data); - mj_comVel(model, data); + mj_kinematics(model.get(), data.get()); + mj_comPos(model.get(), data.get()); + mj_comVel(model.get(), data.get()); // get bodyid and site position - int bodyid = mj_name2id(model, mjOBJ_BODY, "query"); + int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "query"); EXPECT_GT(bodyid, 0); - int siteid = mj_name2id(model, mjOBJ_SITE, "query"); + int siteid = mj_name2id(model.get(), mjOBJ_SITE, "query"); EXPECT_GT(siteid, -1); mjtNum point[3]; - mju_copy3(point, data->site_xpos+3*siteid); + mju_copy3(point, data->site_xpos + 3 * siteid); // dense jacDot - std::vector jacp_dense(3*nv); - std::vector jacr_dense(3*nv); - mj_jacDot(model, data, jacp_dense.data(), jacr_dense.data(), point, bodyid); + std::vector jacp_dense(3 * nv); + std::vector jacr_dense(3 * nv); + mj_jacDot(model.get(), data.get(), jacp_dense.data(), jacr_dense.data(), + point, bodyid); // compute body chain using public mjModel fields std::vector chain(nv); @@ -629,32 +617,28 @@ TEST_F(JacobianTest, JacDotSparse) { EXPECT_GT(NV, 0); // sparse jacDot - std::vector jacp_sparse(3*NV); - std::vector jacr_sparse(3*NV); - mj_jacDotSparse(model, data, jacp_sparse.data(), jacr_sparse.data(), - point, bodyid, NV, chain.data()); + std::vector jacp_sparse(3 * NV); + std::vector jacr_sparse(3 * NV); + mj_jacDotSparse(model.get(), data.get(), jacp_sparse.data(), + jacr_sparse.data(), point, bodyid, NV, chain.data()); // expand sparse to dense and compare - std::vector jacp_expanded(3*nv, 0); - std::vector jacr_expanded(3*nv, 0); + std::vector jacp_expanded(3 * nv, 0); + std::vector jacr_expanded(3 * nv, 0); for (int ci = 0; ci < NV; ci++) { int di = chain[ci]; for (int r = 0; r < 3; r++) { - jacp_expanded[di+r*nv] = jacp_sparse[ci+r*NV]; - jacr_expanded[di+r*nv] = jacr_sparse[ci+r*NV]; + jacp_expanded[di + r * nv] = jacp_sparse[ci + r * NV]; + jacr_expanded[di + r * nv] = jacr_sparse[ci + r * NV]; } } // expect bitwise equality EXPECT_EQ(jacp_expanded, jacp_dense); EXPECT_EQ(jacr_expanded, jacr_dense); - - mj_deleteData(data); - mj_deleteModel(model); } } - // validate rotational Jacobian used in welds TEST_F(JacobianTest, WeldRotJacobian) { #ifdef mjUSESINGLE @@ -678,23 +662,23 @@ TEST_F(JacobianTest, WeldRotJacobian) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, testing::NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), testing::NotNull()) << error; ASSERT_EQ(model->nq, 7); ASSERT_EQ(model->nv, 6); static const int nv = 6; // for increased readability - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); // arbitrary initial values for the ball and hinge joints mjtNum qpos0[7] = {.5, .5, .5, .5, .7, .8, .9}; // compute required quantities using mj_step1 - mj_step1(model, data); + mj_step1(model.get(), data.get()); // get orientation error mjtNum res[3]; // compute rotation residual following formula in mj_instantiateEquality - auto RotationResidual = [](const mjModel *model, mjData *data, + auto RotationResidual = [](const mjModel* model, mjData* data, const mjtNum qpos[7], const mjtNum dqpos[6], mjtNum res[3]) { // copy configuration, compute required quantities with mj_step1 @@ -710,80 +694,75 @@ TEST_F(JacobianTest, WeldRotJacobian) { // compute orientation residual mjtNum quat1[4], quat2[4], quat3[4]; - mju_copy4(quat1, data->xquat+4*1); - mju_negQuat(quat2, data->xquat+4*2); + mju_copy4(quat1, data->xquat + 4 * 1); + mju_negQuat(quat2, data->xquat + 4 * 2); mju_mulQuat(quat3, quat2, quat1); - mju_copy3(res, quat3+1); + mju_copy3(res, quat3 + 1); }; - RotationResidual(model, data, qpos0, NULL, res); + RotationResidual(model.get(), data.get(), qpos0, NULL, res); // compute Jacobian with finite-differencing - mjtNum jacFD[3*nv]; + mjtNum jacFD[3 * nv]; mjtNum dqpos[nv] = {0}; mjtNum dres[3]; const mjtNum eps = 1e-6; - for (int i=0; i < nv; i++) { + for (int i = 0; i < nv; i++) { // nudge i-th dof dqpos[i] = eps; // get nudged residual - RotationResidual(model, data, qpos0, dqpos, dres); + RotationResidual(model.get(), data.get(), qpos0, dqpos, dres); // remove nudge dqpos[i] = 0.0; // compute Jacobian column - for (int j=0; j < 3; j++) { - jacFD[nv*j + i] = (dres[j] - res[j]) / eps; + for (int j = 0; j < 3; j++) { + jacFD[nv * j + i] = (dres[j] - res[j]) / eps; } } // reset mjData to qpos0 mju_copy(data->qpos, qpos0, 7); - mj_step1(model, data); + mj_step1(model.get(), data.get()); // intermediate quaternions quat1 and quat2 mjtNum quat1[4], negQuat2[4]; - mju_copy4(quat1, data->xquat+4*1); - mju_negQuat(negQuat2, data->xquat+4*2); + mju_copy4(quat1, data->xquat + 4 * 1); + mju_negQuat(negQuat2, data->xquat + 4 * 2); // get analytical Jacobian following formula in mj_instantiateEquality - mjtNum jacdif[3*nv], jac0[3*nv], jac1[3*nv]; + mjtNum jacdif[3 * nv], jac0[3 * nv], jac1[3 * nv]; mjtNum point[3] = {0}; // rotational Jacobian difference - mj_jacDifPair(model, data, NULL, 2, 1, point, point, - NULL, NULL, NULL, jac0, jac1, jacdif, mj_isSparse(model), + mj_jacDifPair(model.get(), data.get(), NULL, 2, 1, point, point, NULL, NULL, + NULL, jac0, jac1, jacdif, mj_isSparse(model.get()), /*flg_skipcommon=*/0); // formula: 0.5 * neg(quat2) * (jac1-jac2) * quat1 mjtNum axis[3], quat3[4], quat4[4]; - for (int j=0; j < nv; j++) { + for (int j = 0; j < nv; j++) { // axis = [jac1-jac2]_col(j) - axis[0] = jacdif[0*nv+j]; - axis[1] = jacdif[1*nv+j]; - axis[2] = jacdif[2*nv+j]; + axis[0] = jacdif[0 * nv + j]; + axis[1] = jacdif[1 * nv + j]; + axis[2] = jacdif[2 * nv + j]; // apply formula mju_mulQuatAxis(quat3, negQuat2, axis); mju_mulQuat(quat4, quat3, quat1); // correct Jacobian - jacdif[0*nv+j] = 0.5*quat4[1]; - jacdif[1*nv+j] = 0.5*quat4[2]; - jacdif[2*nv+j] = 0.5*quat4[3]; + jacdif[0 * nv + j] = 0.5 * quat4[1]; + jacdif[1 * nv + j] = 0.5 * quat4[2]; + jacdif[2 * nv + j] = 0.5 * quat4[3]; } // test that analytical and finite-differenced Jacobians match - EXPECT_THAT(AsVector(jacFD, 3*nv), - Pointwise(MjNear(eps, 1e-3), AsVector(jacdif, 3*nv))); - - mj_deleteData(data); - mj_deleteModel(model); + EXPECT_THAT(AsVector(jacFD, 3 * nv), + Pointwise(MjNear(eps, 1e-3), AsVector(jacdif, 3 * nv))); } } // namespace } // namespace mujoco - - diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 25221e0d..86a18c24 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -36,42 +36,41 @@ namespace mujoco { namespace { using ::std::vector; -using ::testing::Pointwise; using ::testing::DoubleNear; -using ::testing::Eq; using ::testing::Each; +using ::testing::Eq; using ::testing::NotNull; +using ::testing::Pointwise; using DerivativeTest = MujocoTest; // errors smaller than this are ignored #ifdef mjUSESINGLE - static const mjtNum absolute_tolerance = 1e-3; +static const mjtNum absolute_tolerance = 1e-3; #else - static const mjtNum absolute_tolerance = 1e-9; +static const mjtNum absolute_tolerance = 1e-9; #endif // corrected relative error static mjtNum RelativeError(mjtNum a, mjtNum b) { - mjtNum nominator = mjMAX(0, mju_abs(a-b) - absolute_tolerance); + mjtNum nominator = mjMAX(0, mju_abs(a - b) - absolute_tolerance); mjtNum denominator = (mju_abs(a) + mju_abs(b) + absolute_tolerance); return nominator / denominator; } // expect two 2D arrays to have elementwise relative error smaller than eps // return maximum absolute error -static mjtNum CompareMatrices(mjtNum* Actual, mjtNum* Expected, - int nrow, int ncol, mjtNum eps) { +static mjtNum CompareMatrices(mjtNum* Actual, mjtNum* Expected, int nrow, + int ncol, mjtNum eps) { mjtNum max_error = 0; - for (int i=0; i < nrow; i++) { - for (int j=0; j < ncol; j++) { - mjtNum actual = Actual[i*ncol+j]; - mjtNum expected = Expected[i*ncol+j]; + for (int i = 0; i < nrow; i++) { + for (int j = 0; j < ncol; j++) { + mjtNum actual = Actual[i * ncol + j]; + mjtNum expected = Expected[i * ncol + j]; EXPECT_LT(RelativeError(actual, expected), eps) << "error at position (" << i << ", " << j << ")" - << "\nexpected = " << expected - << "\nactual = " << actual - << "\ndiff = " << expected-actual; - max_error = mjMAX(mju_abs(actual-expected), max_error); + << "\nexpected = " << expected << "\nactual = " << actual + << "\ndiff = " << expected - actual; + max_error = mjMAX(mju_abs(actual - expected), max_error); } } return max_error; @@ -89,8 +88,7 @@ static const char* const kDamperActuatorsPath = "engine/testdata/actuation/damper.xml"; static const char* const kDampedPendulumPath = "engine/testdata/derivative/damped_pendulum.xml"; -static const char* const kLinearPath = - "engine/testdata/derivative/linear.xml"; +static const char* const kLinearPath = "engine/testdata/derivative/linear.xml"; static const char* const kDCMotorPath = "engine/testdata/derivative/dcmotor.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 TEST_F(DerivativeTest, SmoothDvel) { // run test on all models - for (const char* local_path : {kEnergyConservingPendulumPath, - kTumblingThinObjectPath, - kDampedActuatorsPath, - kDamperActuatorsPath, - kDCMotorPath}) { + for (const char* local_path : + {kEnergyConservingPendulumPath, kTumblingThinObjectPath, + kDampedActuatorsPath, kDamperActuatorsPath, kDCMotorPath}) { const std::string xml_path = GetTestDataFilePath(local_path); 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; int nD = model->nD; mjData* data = mj_makeData(model); @@ -119,7 +116,7 @@ TEST_F(DerivativeTest, SmoothDvel) { if (model->nu) { data->ctrl[0] = 0.1; } - for (int i=0; i < 100; i++) { + for (int i = 0; i < 100; i++) { mj_step(model, data); } mj_forward(model, data); @@ -171,13 +168,12 @@ TEST_F(DerivativeTest, DisabledActuators) { )"; char error[1024]; - mjModel* m1 = LoadModelFromString(xml1, error, sizeof(error)); - ASSERT_THAT(m1, NotNull()) << error; - mjData* d1 = mj_makeData(m1); + MjModelPtr m1 = LoadModelFromString(xml1, error, sizeof(error)); + ASSERT_THAT(m1.get(), NotNull()) << error; + MjDataPtr d1 = MakeData(m1); d1->ctrl[0] = 6; - while (d1->time < 1) - mj_step(m1, d1); + while (d1->time < 1) mj_step(m1.get(), d1.get()); // model with a position actuator and an intvelocity actuator static constexpr char xml2[] = R"( @@ -198,22 +194,16 @@ TEST_F(DerivativeTest, DisabledActuators) { )"; - mjModel* m2 = LoadModelFromString(xml2); - mjData* d2 = mj_makeData(m2); + MjModelPtr m2 = LoadModelFromString(xml2); + MjDataPtr d2 = MakeData(m2); d2->ctrl[0] = 6; d2->ctrl[1] = 6; - while (d2->time < 1) - mj_step(m2, d2); + while (d2->time < 1) mj_step(m2.get(), d2.get()); // expect same qvel in both models 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 @@ -242,15 +232,14 @@ TEST_F(DerivativeTest, ActuatorOrder) { )"; char error[1024]; - mjModel* m1 = LoadModelFromString(xml1, error, sizeof(error)); - ASSERT_THAT(m1, NotNull()) << "Failed to load model: " << error; - mjData* d1 = mj_makeData(m1); + MjModelPtr m1 = LoadModelFromString(xml1, error, sizeof(error)); + ASSERT_THAT(m1.get(), NotNull()) << "Failed to load model: " << error; + MjDataPtr d1 = MakeData(m1); d1->ctrl[0] = 6; d1->ctrl[1] = 6; - while (d1->time < 1) - mj_step(m1, d1); + while (d1->time < 1) mj_step(m1.get(), d1.get()); // model with stateful actuator second static constexpr char xml2[] = R"( @@ -275,38 +264,32 @@ TEST_F(DerivativeTest, ActuatorOrder) { )"; - mjModel* m2 = LoadModelFromString(xml2, error, sizeof(error)); - ASSERT_THAT(m2, NotNull()) << "Failed to load model: " << error; - mjData* d2 = mj_makeData(m2); + MjModelPtr m2 = LoadModelFromString(xml2, error, sizeof(error)); + ASSERT_THAT(m2.get(), NotNull()) << "Failed to load model: " << error; + MjDataPtr d2 = MakeData(m2); d2->ctrl[0] = 6; d2->ctrl[1] = 6; - while (d2->time < 1) - mj_step(m2, d2); + while (d2->time < 1) mj_step(m2.get(), d2.get()); // expect same qvel in both models EXPECT_EQ(d1->qvel[0], d2->qvel[0]); 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 TEST_F(DerivativeTest, PassiveDvel) { - for (const char* local_path : {kTumblingThinObjectPath, - kTumblingThinObjectEllipsoidPath}) { + for (const char* local_path : + {kTumblingThinObjectPath, kTumblingThinObjectEllipsoidPath}) { // load model const std::string xml_path = GetTestDataFilePath(local_path); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); int nD = model->nD; mjData* data = mj_makeData(model); // allocate Jacobians - mjtNum* qDerivAnalytic = (mjtNum*) mju_malloc(sizeof(mjtNum)*nD); - mjtNum* qDerivFD = (mjtNum*) mju_malloc(sizeof(mjtNum)*nD); + mjtNum* qDerivAnalytic = (mjtNum*)mju_malloc(sizeof(mjtNum) * nD); + mjtNum* qDerivFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * nD); for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) { // set sparsity @@ -314,7 +297,7 @@ TEST_F(DerivativeTest, PassiveDvel) { // take 100 steps so we have some velocities, then call forward mj_resetData(model, data); - for (int i=0; i < 100; i++) { + for (int i = 0; i < 100; i++) { mj_step(model, data); } mj_forward(model, data); @@ -355,22 +338,21 @@ TEST_F(DerivativeTest, StepSkip) { // disable warm-starts so we don't need to save qacc_warmstart model->opt.disableflags |= mjDSBL_WARMSTART; - for (const mjtIntegrator integrator : {mjINT_EULER, - mjINT_IMPLICIT, - mjINT_IMPLICITFAST}) { + for (const mjtIntegrator integrator : + {mjINT_EULER, mjINT_IMPLICIT, mjINT_IMPLICITFAST}) { model->opt.integrator = integrator; // reset, take 20 steps mj_resetData(model, data); - for (int i=0; i < 20; i++) { + for (int i = 0; i < 20; i++) { mj_step(model, data); } // denormalize the quat, just to see that it doesn't make a difference - for (int j=0; j < model->njnt; j++) { + for (int j = 0; j < model->njnt; j++) { if (model->jnt_type[j] == mjJNT_BALL) { int adr = model->jnt_qposadr[j]; - for (int k=0; k < 4; k++) { + for (int k = 0; k < 4; k++) { data->qpos[adr + k] *= 8; } } @@ -442,46 +424,46 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) { // === state-transition matrix A if (A) { - mjtNum *Ac = mj_stackAllocNum(d, 2*nv*nv); + mjtNum* Ac = mj_stackAllocNum(d, 2 * nv * nv); // Ac = H^-1 [diag(-stiffness) diag(-damping)] - mju_zero(Ac, 2*nv*nv); - for (int i=0; i < nv; i++) { - Ac[i*nv + i] = -m->jnt_stiffness[i]; - Ac[nv*nv + i*nv + i] = -m->dof_damping[i]; + mju_zero(Ac, 2 * nv * nv); + for (int i = 0; i < nv; i++) { + Ac[i * nv + i] = -m->jnt_stiffness[i]; + Ac[nv * nv + i * nv + i] = -m->dof_damping[i]; } - mj_solveLD(Ac, d->qH, d->qHDiagInv, nv, 2*nv, - m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); + mj_solveLD(Ac, d->qH, d->qHDiagInv, nv, 2 * nv, m->M_rownnz, m->M_rowadr, + m->M_colind, nullptr); // A = [dt*Ac; Ac] - mju_transpose(A, Ac, 2*nv, nv); - mju_scl(A, A, dt, nv*2*nv); - mju_transpose(A+2*nv*nv, Ac, 2*nv, nv); + mju_transpose(A, Ac, 2 * nv, nv); + mju_scl(A, A, dt, nv * 2 * nv); + mju_transpose(A + 2 * nv * nv, Ac, 2 * nv, nv); // Add eye(nv) to top right quadrant of A - for (int i=0; i < nv; i++) { - A[i*2*nv + nv + i] += 1; + for (int i = 0; i < nv; i++) { + A[i * 2 * nv + nv + i] += 1; } // A *= dt - mju_scl(A, A, dt, 2*nv*2*nv); + mju_scl(A, A, dt, 2 * nv * 2 * nv); // A += eye(2*nv) - for (int i=0; i < 2*nv; i++) { - A[i*2*nv + i] += 1; + for (int i = 0; i < 2 * nv; i++) { + A[i * 2 * nv + i] += 1; } } // === control-transition matrix B if (B) { - mjtNum *Bc = mj_stackAllocNum(d, nu*nv); - mjtNum *BcT = mj_stackAllocNum(d, nv*nu); + mjtNum* Bc = mj_stackAllocNum(d, nu * nv); + mjtNum* BcT = mj_stackAllocNum(d, nv * nu); mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz, d->moment_rowadr, d->moment_colind); - mj_solveLD(Bc, d->qH, d->qHDiagInv, nv, nu, - m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); + mj_solveLD(Bc, d->qH, d->qHDiagInv, nv, nu, m->M_rownnz, m->M_rowadr, + m->M_colind, nullptr); mju_transpose(BcT, Bc, nu, nv); - mju_scl(B, BcT, dt*dt, nu*nv); - mju_scl(B+nu*nv, BcT, dt, nu*nv); + mju_scl(B, BcT, dt * dt, nu * nv); + mju_scl(B + nu * nv, BcT, dt, nu * nv); } mj_freeStack(d); @@ -495,15 +477,15 @@ TEST_F(DerivativeTest, LinearSystem) { int nv = model->nv, nu = model->nu; // set ctrl, integrate for 20 steps - data->ctrl[0] = .1; + data->ctrl[0] = .1; data->ctrl[1] = -.1; - for (int i=0; i < 20; i++) { + for (int i = 0; i < 20; i++) { mj_step(model, data); } // analytic A and B - mjtNum* A = (mjtNum*) mju_malloc(sizeof(mjtNum)*2*nv*2*nv); - mjtNum* B = (mjtNum*) mju_malloc(sizeof(mjtNum)*2*nv*nu); + mjtNum* A = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv); + mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu); LinearSystem(model, data, A, B); @@ -513,29 +495,29 @@ TEST_F(DerivativeTest, LinearSystem) { // forward differenced A and B mjtNum eps = MjTol(1e-6, 1e-3); - mjtNum* AFD = (mjtNum*) mju_malloc(sizeof(mjtNum)*2*nv*2*nv); - mjtNum* BFD = (mjtNum*) mju_malloc(sizeof(mjtNum)*2*nv*nu); + mjtNum* AFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv); + mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu); - mjd_transitionFD(model, data, eps, /*centered=*/0, - AFD, BFD, nullptr, nullptr); + mjd_transitionFD(model, data, eps, /*centered=*/0, AFD, BFD, nullptr, + nullptr); // uncomment for debugging: // PrintMatrix(AFD, 2*nv, 2*nv); // PrintMatrix(BFD, 2*nv, nu); // expect FD and analytic derivatives to be similar to eps precision - CompareMatrices(A, AFD, 2*nv, 2*nv, eps); - CompareMatrices(B, BFD, 2*nv, nu, eps); + CompareMatrices(A, AFD, 2 * nv, 2 * nv, eps); + CompareMatrices(B, BFD, 2 * nv, nu, eps); // central differenced A and B - mjtNum* AFDc = (mjtNum*) mju_malloc(sizeof(mjtNum)*2*nv*2*nv); - mjtNum* BFDc = (mjtNum*) mju_malloc(sizeof(mjtNum)*2*nv*nu); - mjd_transitionFD(model, data, eps, /*centered=*/1, - AFDc, BFDc, nullptr, nullptr); + mjtNum* AFDc = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv); + mjtNum* BFDc = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu); + mjd_transitionFD(model, data, eps, /*centered=*/1, AFDc, BFDc, nullptr, + nullptr); // expect central derivatives to be equal to forward differences - CompareMatrices(AFD, AFDc, 2*nv, 2*nv, eps); - CompareMatrices(BFD, BFDc, 2*nv, nu, eps); + CompareMatrices(AFD, AFDc, 2 * nv, 2 * nv, eps); + CompareMatrices(BFD, BFDc, 2 * nv, nu, eps); mju_free(BFDc); mju_free(AFDc); @@ -555,52 +537,52 @@ TEST_F(DerivativeTest, ClampedCtrlDerivatives) { int nv = model->nv, nu = model->nu; // set ctrl, integrate for 20 steps - data->ctrl[0] = .1; + data->ctrl[0] = .1; data->ctrl[1] = -.1; - for (int i=0; i < 20; i++) { + for (int i = 0; i < 20; i++) { mj_step(model, data); } // analytic B - mjtNum* B = (mjtNum*) mju_malloc(sizeof(mjtNum)*2*nv*nu); + mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu); LinearSystem(model, data, nullptr, B); // forward differenced A and B mjtNum eps = MjTol(1e-6, 1e-3); - mjtNum* BFD = (mjtNum*) mju_malloc(sizeof(mjtNum)*2*nv*nu); + mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu); // set ctrl to the limits, request forward differences - data->ctrl[0] = 1; + data->ctrl[0] = 1; data->ctrl[1] = -1; - mjd_transitionFD(model, data, eps, /*centered=*/0, - nullptr, BFD, nullptr, nullptr); + mjd_transitionFD(model, data, eps, /*centered=*/0, nullptr, BFD, nullptr, + nullptr); // 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 - mjd_transitionFD(model, data, eps, /*centered=*/1, - nullptr, BFD, nullptr, nullptr); + mjd_transitionFD(model, data, eps, /*centered=*/1, nullptr, BFD, nullptr, + nullptr); // 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 - data->ctrl[0] = 2; + data->ctrl[0] = 2; data->ctrl[1] = -2; - mjd_transitionFD(model, data, eps, /*centered=*/0, - nullptr, BFD, nullptr, nullptr); + mjd_transitionFD(model, data, eps, /*centered=*/0, nullptr, BFD, nullptr, + nullptr); // 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))); // expect ctrl to remain unchanged (despite internal clamping) - EXPECT_EQ(data->ctrl[0], 2.0); + EXPECT_EQ(data->ctrl[0], 2.0); EXPECT_EQ(data->ctrl[1], -2.0); // ctrl remains beyond limits, request centered differences - mjd_transitionFD(model, data, eps, /*centered=*/1, - nullptr, BFD, nullptr, nullptr); + mjd_transitionFD(model, data, eps, /*centered=*/1, nullptr, BFD, nullptr, + nullptr); // 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))); mju_free(BFD); mju_free(B); @@ -631,38 +613,32 @@ TEST_F(DerivativeTest, SensorDerivatives) { )"; - mjModel* model = LoadModelFromString(xml); + MjModelPtr model = LoadModelFromString(xml); int nv = model->nv, nu = model->nu, ns = model->nsensordata; - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); // finite differenced C and D mjtNum eps = 1e-6; - mjtNum* CFD = (mjtNum*) mju_malloc(sizeof(mjtNum)*ns*2*nv); - mjtNum* DFD = (mjtNum*) mju_malloc(sizeof(mjtNum)*ns*nu); - mjd_transitionFD(model, data, eps, /*centered=*/0, - nullptr, nullptr, CFD, DFD); + mjtNum* CFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * 2 * nv); + mjtNum* DFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * nu); + mjd_transitionFD(model.get(), data.get(), eps, /*centered=*/0, nullptr, + nullptr, CFD, DFD); // expected analytic C and D - mjtNum C[6] = { - 1, 0, - 0, 1, - 0, 0 - }; + mjtNum C[6] = {1, 0, 0, 1, 0, 0}; mjtNum D[3] = { - 0, - 0, - 3, + 0, + 0, + 3, }; // compare expected and actual values - CompareMatrices(CFD, C, ns, 2*nv, eps); + CompareMatrices(CFD, C, ns, 2 * nv, eps); CompareMatrices(DFD, D, ns, nu, eps); mju_free(DFD); mju_free(CFD); - mj_deleteData(data); - mj_deleteModel(model); } // if sensor derivatives aren't requested, don't compute sensors @@ -686,24 +662,22 @@ TEST_F(DerivativeTest, SensorSkip) { )"; - mjModel* model = LoadModelFromString(xml); + MjModelPtr model = LoadModelFromString(xml); int nv = model->nv, nu = model->nu; - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); // set a sentinel value in the sensor data->sensordata[0] = 1337; // finite differenced B mjtNum eps = 1e-6; - mjtNum* BFD = (mjtNum*) mju_malloc(sizeof(mjtNum)*2*nv*nu); - mjd_transitionFD(model, data, eps, /*centered=*/0, - nullptr, BFD, nullptr, nullptr); + mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu); + mjd_transitionFD(model.get(), data.get(), eps, /*centered=*/0, nullptr, BFD, + nullptr, nullptr); EXPECT_EQ(data->sensordata[0], 1337) << "sensors should not be recomputed"; mju_free(BFD); - mj_deleteData(data); - mj_deleteModel(model); } // derivatives don't mutate the state @@ -718,28 +692,27 @@ TEST_F(DerivativeTest, NoStateMutation) { // set time data->time = data0->time = 0.5; - for (int i=0; i < nv; i++) { - data->qpos[i] = data0->qpos[i] = (mjtNum) i+1; - data->qvel[i] = data0->qvel[i] = (mjtNum) i+2; + for (int i = 0; i < nv; i++) { + data->qpos[i] = data0->qpos[i] = (mjtNum)i + 1; + data->qvel[i] = data0->qvel[i] = (mjtNum)i + 2; } // set ctrl - for (int i=0; i < nu; i++) { - data->ctrl[i] = data0->ctrl[i] = (mjtNum) i+1; + for (int i = 0; i < nu; i++) { + data->ctrl[i] = data0->ctrl[i] = (mjtNum)i + 1; } // set act - for (int i=0; i < na; i++) { - data->act[i] = data0->act[i] = (mjtNum) i+1; + for (int i = 0; i < na; i++) { + data->act[i] = data0->act[i] = (mjtNum)i + 1; } - // allocate Jacobians, call derivatives - int ndx = nv+nv+na; - mjtNum* A = (mjtNum*) mju_malloc(sizeof(mjtNum)*ndx*ndx); - mjtNum* B = (mjtNum*) mju_malloc(sizeof(mjtNum)*ndx*nu); - mjtNum* C = (mjtNum*) mju_malloc(sizeof(mjtNum)*ns*ndx); - mjtNum* D = (mjtNum*) mju_malloc(sizeof(mjtNum)*ns*nu); + int ndx = nv + nv + na; + mjtNum* A = (mjtNum*)mju_malloc(sizeof(mjtNum) * ndx * ndx); + mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * ndx * nu); + mjtNum* C = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * ndx); + mjtNum* D = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * nu); mjtNum eps = 1e-6; mjd_transitionFD(model, data, eps, /*centered=*/0, A, B, C, D); @@ -754,25 +727,24 @@ TEST_F(DerivativeTest, NoStateMutation) { mju_free(C); mju_free(B); mju_free(A); - mj_deleteData(data); mj_deleteData(data0); + mj_deleteData(data); mj_deleteModel(model); } // compare dense and sparse derivatives of qfrc_bias (RNE) TEST_F(DerivativeTest, DenseSparseRneEquivalent) { // run test on all models - for (const char* local_path : {kEnergyConservingPendulumPath, - kTumblingThinObjectPath, - kDampedActuatorsPath, - kDamperActuatorsPath, - kDCMotorPath}) { + for (const char* local_path : + {kEnergyConservingPendulumPath, kTumblingThinObjectPath, + kDampedActuatorsPath, kDamperActuatorsPath, kDCMotorPath}) { const std::string xml_path = GetTestDataFilePath(local_path); 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; int nD = model->nD; - mjtNum* qDeriv = (mjtNum*) mju_malloc(sizeof(mjtNum)*nD); + mjtNum* qDeriv = (mjtNum*)mju_malloc(sizeof(mjtNum) * nD); mjData* data = mj_makeData(model); // take 100 steps so we have some velocities, then call forward @@ -780,7 +752,7 @@ TEST_F(DerivativeTest, DenseSparseRneEquivalent) { if (model->nu) { data->ctrl[0] = 0.1; } - for (int i=0; i < 100; i++) { + for (int i = 0; i < 100; i++) { mj_step(model, data); } mj_forward(model, data); @@ -799,8 +771,8 @@ TEST_F(DerivativeTest, DenseSparseRneEquivalent) { EXPECT_THAT(AsVector(data->qDeriv, nD), Pointwise(MjNear(1e-12, 5e-5), AsVector(qDeriv, nD))); - mj_deleteData(data); mju_free(qDeriv); + mj_deleteData(data); mj_deleteModel(model); } } @@ -815,13 +787,13 @@ TEST_F(DerivativeTest, LinearSystemInverse) { int ns = model->nsensordata; int nM = model->nM; - vector DfDq(nv*nv); - vector DfDv(nv*nv); - vector DfDa(nv*nv); - vector DsDq(nv*ns); - vector DsDv(nv*ns); - vector DsDa(nv*ns); - vector DmDq(nv*nM); + vector DfDq(nv * nv); + vector DfDv(nv * nv); + vector DfDa(nv * nv); + vector DsDq(nv * ns); + vector DsDv(nv * ns); + vector DsDa(nv * ns); + vector DmDq(nv * nM); // call mj_forward to get accelerations at initial state mj_forward(model, data); @@ -829,54 +801,53 @@ TEST_F(DerivativeTest, LinearSystemInverse) { // get derivatives mjtNum eps = 1e-6; mjtByte flg_actuation = 0; - mjd_inverseFD(model, data, eps, flg_actuation, - DfDq.data(), DfDv.data(), DfDa.data(), - DsDq.data(), DsDv.data(), DsDa.data(), + mjd_inverseFD(model, data, eps, flg_actuation, DfDq.data(), DfDv.data(), + DfDa.data(), DsDq.data(), DsDv.data(), DsDa.data(), DmDq.data()); // expect that position derivatives are the stiffnesses - vector DfDq_expect = {model->jnt_stiffness[0], 0, 0, - 0, model->jnt_stiffness[1], 0, - 0, 0, model->jnt_stiffness[2]}; + vector DfDq_expect = {model->jnt_stiffness[0], 0, 0, 0, + model->jnt_stiffness[1], 0, 0, 0, + model->jnt_stiffness[2]}; EXPECT_THAT(DfDq, Pointwise(DoubleNear(eps), DfDq_expect)); // expect that velocity derivatives are the dampings - vector DfDv_expect = {model->dof_damping[0], 0, 0, - 0, model->dof_damping[1], 0, - 0, 0, model->dof_damping[2]}; + vector DfDv_expect = {model->dof_damping[0], 0, 0, 0, + model->dof_damping[1], 0, 0, 0, + model->dof_damping[2]}; EXPECT_THAT(DfDv, Pointwise(DoubleNear(eps), DfDv_expect)); // expect that acceleration derivatives are the mass matrix - vector DfDa_expect(nv*nv, 0); + vector DfDa_expect(nv * nv, 0); mj_fullM(model, data, DfDa_expect.data()); EXPECT_THAT(DfDa, Pointwise(DoubleNear(eps), DfDa_expect)); // expect that sensor derivatives w.r.t position only see sensor 1 at dof 0 - vector DsDq_expect(nv*ns, 0); + vector DsDq_expect(nv * ns, 0); int dof_index = 0; int sensordata_index = model->sensor_adr[1]; - DsDq_expect[dof_index*ns + sensordata_index] = 1; + DsDq_expect[dof_index * ns + sensordata_index] = 1; EXPECT_THAT(DsDq, Pointwise(DoubleNear(eps), DsDq_expect)); // expect that sensor derivatives w.r.t velocity only see sensor 0 at dof 1 - vector DsDv_expect(nv*ns, 0); + vector DsDv_expect(nv * ns, 0); dof_index = 1; sensordata_index = model->sensor_adr[0]; - DsDv_expect[dof_index*ns + sensordata_index] = 1; + DsDv_expect[dof_index * ns + sensordata_index] = 1; EXPECT_THAT(DsDv, Pointwise(DoubleNear(eps), DsDv_expect)); // expect that sensor derivatives w.r.t acceleration see the accelerometer // in the y-axis, affected by both dof 0 and dof 1 - vector DsDa_expect(nv*ns, 0); + vector DsDa_expect(nv * ns, 0); dof_index = 0; sensordata_index = model->sensor_adr[2] + 1; - DsDa_expect[dof_index*ns + sensordata_index] = 1; + DsDa_expect[dof_index * ns + sensordata_index] = 1; dof_index = 1; - DsDa_expect[dof_index*ns + sensordata_index] = 1; + DsDa_expect[dof_index * ns + sensordata_index] = 1; EXPECT_THAT(DsDa, Pointwise(DoubleNear(eps), DsDa_expect)); // expect that mass matrix derivatives are zero - vector DmDq_expect(nv*nM, 0); + vector DmDq_expect(nv * nM, 0); EXPECT_THAT(DmDq, Pointwise(DoubleNear(eps), DmDq_expect)); mj_deleteData(data); @@ -891,7 +862,7 @@ void randomQuatPair(mjtNum qa[4], mjtNum qb[4], mjtNum angle, int seed) { std::normal_distribution dist(0, 1); // sample qa = qb - for (int i=0; i < 4; i++) { + for (int i = 0; i < 4; i++) { qa[i] = qb[i] = dist(rng); } mju_normalize4(qa); @@ -899,7 +870,7 @@ void randomQuatPair(mjtNum qa[4], mjtNum qb[4], mjtNum angle, int seed) { // integrate qb in random direction by angle mjtNum dir[3]; - for (int i=0; i < 3; i++) { + for (int i = 0; i < 3; i++) { dir[i] = dist(rng); } mju_normalize3(dir); @@ -907,8 +878,8 @@ void randomQuatPair(mjtNum qa[4], mjtNum qb[4], mjtNum angle, int seed) { } // utility: finite-difference Jacobians of mju_subQuat -static void subQuatFD(mjtNum Da[9], mjtNum Db[9], - const mjtNum qa[4], const mjtNum qb[4], mjtNum eps) { +static void subQuatFD(mjtNum Da[9], mjtNum Db[9], const mjtNum qa[4], + const mjtNum qb[4], mjtNum eps) { // subQuat mjtNum y[3]; mju_subQuat(y, qa, qb); @@ -949,7 +920,8 @@ static void subQuatFD(mjtNum Da[9], mjtNum Db[9], TEST_F(DerivativeTest, SubQuat) { 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; for (int i = 0; i < nrepeats; i++) { @@ -988,22 +960,21 @@ static void randomQuatVel(mjtNum quat[4], mjtNum vel[3], int seed) { std::normal_distribution dist(0, 1); // sample quat - for (int i=0; i < 4; i++) { + for (int i = 0; i < 4; i++) { quat[i] = dist(rng); } mju_normalize4(quat); // sample vel - for (int i=0; i < 3; i++) { + for (int i = 0; i < 3; i++) { vel[i] = dist(rng); } } // utility: finite-difference Jacobians of mju_quatIntegrate -void mjd_quatIntegrateFD(mjtNum Dquat[9], mjtNum Ds[9], - mjtNum Dvel[9], mjtNum Dh[3], - const mjtNum quat[4], const mjtNum vel[3], - mjtNum h, mjtNum eps) { +void mjd_quatIntegrateFD(mjtNum Dquat[9], mjtNum Ds[9], mjtNum Dvel[9], + mjtNum Dh[3], const mjtNum quat[4], + const mjtNum vel[3], mjtNum h, mjtNum eps) { // compute y, output of mju_quatIntegrate(quat, vel, h) mjtNum y[4] = {quat[0], quat[1], quat[2], quat[3]}; mju_quatIntegrate(y, vel, h); @@ -1023,13 +994,13 @@ void mjd_quatIntegrateFD(mjtNum Dquat[9], mjtNum Ds[9], // d_y / d_quat mju_copy4(dq, quat); mju_quatIntegrate(dq, dx, eps); // nudge dq - mju_quatIntegrate(dq, vel, h); // compute nudged + mju_quatIntegrate(dq, vel, h); // compute nudged mju_subQuat(dy, dq, y); // subtract mju_scl3(DquatT + i * 3, dy, 1.0 / eps); // d_y / d_sv (scaled velocity) mju_copy4(dq, quat); - mjtNum dsv[3] = {vel[0]*h, vel[1]*h, vel[2]*h}; + mjtNum dsv[3] = {vel[0] * h, vel[1] * h, vel[2] * h}; mju_addToScl3(dsv, dx, eps); // nudge dsv mju_quatIntegrate(dq, dsv, 1.0); // compute nudged mju_subQuat(dy, dq, y); // subtract @@ -1058,7 +1029,8 @@ void mjd_quatIntegrateFD(mjtNum Dquat[9], mjtNum Ds[9], TEST_F(DerivativeTest, quatIntegrate) { 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; for (int i = 0; i < nrepeats; i++) { @@ -1114,22 +1086,22 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; mjtNum dt_small = 1e-4; mjtNum dt_large = 1e-2; mjtNum duration = 1.0; - mjData* d_gt = mj_makeData(m); - mjData* d_implicit = mj_makeData(m); - mjData* d_euler = mj_makeData(m); + MjDataPtr d_gt = MakeData(m); + MjDataPtr d_implicit = MakeData(m); + MjDataPtr d_euler = MakeData(m); - mj_resetData(m, d_gt); - mj_resetData(m, d_implicit); - mj_resetData(m, d_euler); + mj_resetData(m.get(), d_gt.get()); + mj_resetData(m.get(), d_implicit.get()); + 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_euler->ctrl[0] = 0.5; @@ -1141,8 +1113,8 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) { m->opt.timestep = dt_large; m->opt.integrator = mjINT_IMPLICITFAST; - mj_resetData(m, d_gt); - d_gt->ctrl[0] = 0.5; + mj_resetData(m.get(), d_gt.get()); + d_gt.get()->ctrl[0] = 0.5; m->opt.timestep = dt_small; m->opt.integrator = mjINT_EULER; @@ -1151,20 +1123,20 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) { m->opt.integrator = mjINT_EULER; m->opt.timestep = dt_small; for (int j = 0; j < substeps; j++) { - mj_step(m, d_gt); + mj_step(m.get(), d_gt.get()); } // euler at large timestep m->opt.timestep = dt_large; - mj_step(m, d_euler); + mj_step(m.get(), d_euler.get()); // implicitfast at large timestep m->opt.integrator = mjINT_IMPLICITFAST; - mj_step(m, d_implicit); + mj_step(m.get(), d_implicit.get()); // accumulate errors - mjtNum diff_implicit = d_gt->qpos[0] - d_implicit->qpos[0]; - mjtNum diff_euler = d_gt->qpos[0] - d_euler->qpos[0]; + mjtNum diff_implicit = d_gt.get()->qpos[0] - d_implicit->qpos[0]; + mjtNum diff_euler = d_gt.get()->qpos[0] - d_euler->qpos[0]; error_implicit += diff_implicit * diff_implicit; error_euler += diff_euler * diff_euler; } @@ -1173,11 +1145,6 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) { EXPECT_LT(error_implicit, error_euler) << "implicitfast should be more accurate than Euler at large timestep " << "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) { @@ -1197,20 +1164,20 @@ TEST_F(DerivativeTest, NonlinearDampingDerivative) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; mjtNum dt_small = 1e-4; mjtNum dt_large = 1e-2; mjtNum duration = 1.0; - mjData* d_gt = mj_makeData(m); - mjData* d_enabled = mj_makeData(m); - mjData* d_disabled = mj_makeData(m); + MjDataPtr d_gt = MakeData(m); + MjDataPtr d_enabled = MakeData(m); + MjDataPtr d_disabled = MakeData(m); - mj_resetDataKeyframe(m, d_gt, 0); - mj_resetDataKeyframe(m, d_enabled, 0); - mj_resetDataKeyframe(m, d_disabled, 0); + mj_resetDataKeyframe(m.get(), d_gt.get(), 0); + mj_resetDataKeyframe(m.get(), d_enabled.get(), 0); + mj_resetDataKeyframe(m.get(), d_disabled.get(), 0); m->opt.integrator = mjINT_EULER; mjtNum error_enabled = 0; @@ -1222,17 +1189,17 @@ TEST_F(DerivativeTest, NonlinearDampingDerivative) { m->opt.timestep = dt_small; m->opt.disableflags |= mjDSBL_EULERDAMP; // disable implicit damping for (int j = 0; j < substeps; j++) { - mj_step(m, d_gt); + mj_step(m.get(), d_gt.get()); } 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 - 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_disabled = d_gt->qvel[0] - d_disabled->qvel[0]; + mjtNum diff_enabled = d_gt.get()->qvel[0] - d_enabled.get()->qvel[0]; + mjtNum diff_disabled = d_gt.get()->qvel[0] - d_disabled.get()->qvel[0]; error_enabled += diff_enabled * diff_enabled; error_disabled += diff_disabled * diff_disabled; } @@ -1240,11 +1207,6 @@ TEST_F(DerivativeTest, NonlinearDampingDerivative) { EXPECT_LT(error_enabled, error_disabled) << "Euler with implicit damping should be more accurate than without " << "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 @@ -1274,9 +1236,9 @@ TEST_F(DerivativeTest, ActearlyDerivative) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); // set identical ctrl with zero initial activation d->ctrl[0] = 1.0; @@ -1285,7 +1247,7 @@ TEST_F(DerivativeTest, ActearlyDerivative) { d->act[1] = 0.0; // step computes derivatives during implicit integration - mj_step(m, d); + mj_step(m.get(), d.get()); // both should have same act_dot 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"; EXPECT_NEAR(d->qDeriv[diag1], 0.0, 1e-10) << "actearly=false should use current_act=0"; - - mj_deleteData(d); - mj_deleteModel(m); } - // verify stateful DC motor derivative matches analytical formula TEST_F(DerivativeTest, DCMotorStatefulDerivative) { static constexpr char xml[] = R"( @@ -1330,19 +1288,19 @@ TEST_F(DerivativeTest, DCMotorStatefulDerivative) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); // set nonzero velocity and ctrl d->qvel[0] = 1.0; d->ctrl[0] = 0.5; // forward to compute act_dot, etc. - mj_forward(m, d); + mj_forward(m.get(), d.get()); // 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 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; EXPECT_NEAR(qDeriv_diag, expected, 1e-10) << "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 TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) { // stateless DC motor with position controller @@ -1396,35 +1350,32 @@ TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) { )"; char error[1024]; - mjModel* m_sl = LoadModelFromString(xml_stateless, error, sizeof(error)); - ASSERT_THAT(m_sl, NotNull()) << error; - mjData* d_sl = mj_makeData(m_sl); + MjModelPtr m_sl = LoadModelFromString(xml_stateless, error, sizeof(error)); + ASSERT_THAT(m_sl.get(), NotNull()) << error; + MjDataPtr d_sl = MakeData(m_sl); - mjModel* m_sf = LoadModelFromString(xml_stateful, error, sizeof(error)); - ASSERT_THAT(m_sf, NotNull()) << error; - mjData* d_sf = mj_makeData(m_sf); + MjModelPtr m_sf = LoadModelFromString(xml_stateful, error, sizeof(error)); + ASSERT_THAT(m_sf.get(), NotNull()) << error; + MjDataPtr d_sf = MakeData(m_sf); // set identical state - d_sl->qvel[0] = d_sf->qvel[0] = 1.0; - d_sl->ctrl[0] = d_sf->ctrl[0] = 0.5; + d_sl.get()->qvel[0] = d_sf.get()->qvel[0] = 1.0; + d_sl.get()->ctrl[0] = d_sf.get()->ctrl[0] = 0.5; // forward and compute derivatives - mj_forward(m_sl, d_sl); - mj_forward(m_sf, d_sf); - mjd_smooth_vel(m_sl, d_sl, 1); - mjd_smooth_vel(m_sf, d_sf, 1); + mj_forward(m_sl.get(), d_sl.get()); + mj_forward(m_sf.get(), d_sf.get()); + mjd_smooth_vel(m_sl.get(), d_sl.get(), 1); + mjd_smooth_vel(m_sf.get(), d_sf.get(), 1); // extract diagonals - mjtNum diag_sl = d_sl->qDeriv[m_sl->D_rowadr[0] + m_sl->D_rownnz[0] - 1]; - mjtNum diag_sf = d_sf->qDeriv[m_sf->D_rowadr[0] + m_sf->D_rownnz[0] - 1]; + mjtNum diag_sl = + 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) << "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 @@ -1477,8 +1428,8 @@ void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name, // Builds K column-by-column using mjd_flexInterp_mul. // Result is -(h^2 + h*damping) * J'KJ (negative sign matches the old addH // convention where stiffness is subtracted from the system matrix). -static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense, - int nv, mjtNum h) { +static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense, int nv, + mjtNum h) { std::vector e_i(nv, 0); std::vector col(nv, 0); for (int i = 0; i < nv; i++) { @@ -1510,18 +1461,18 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) { )"; char error[1024]; - mjModel* model = LoadModelFromString(kXml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; int nD = model->nD; int nv = model->nv; ASSERT_EQ(model->nq, 24); // 8 corners * 3 dofs - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); // iterate over rotations for (mjtNum angle : {0.0, 0.5, 1.0, mjPI / 2, mjPI, 2.0 * mjPI}) { - RotateFlexGrid(model, data, "flex", angle); - mj_forward(model, data); + RotateFlexGrid(model.get(), data.get(), "flex", angle); + mj_forward(model.get(), data.get()); // part 1: stiffness verification { @@ -1543,7 +1494,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) { std::vector H(nv * nv, 0); // 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 model->flex_damping[0] = save_damping; @@ -1553,13 +1504,13 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) { // finite difference of mj_passive for stiffness 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 mju_addToScl(data_perturbed->qpos, vec.data(), eps, nv); // recompute geometry/passive - mj_forward(model, data_perturbed); + mj_forward(model.get(), data_perturbed); // compute FD estimate of K * vec // 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]; } } - 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 data->qvel[0] = 1.0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // get analytic derivatives (without Flex Damping currently) std::vector qDerivAnalytic(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); // finite-difference derivatives @@ -1624,7 +1576,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) { mju_zero(data->qDeriv, nD); 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); // 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 // D = 4*H(0.5) - H(1) vector 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 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 D(nv * nv); for (int i = 0; i < nv * nv; i++) { @@ -1660,9 +1612,6 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) { << "Damping Mismatch at angle: " << angle; } } - - mj_deleteData(data); - mj_deleteModel(model); } // Test Jacobian under deformation to highlight approximation error @@ -1681,14 +1630,14 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) { )"; char error[1024]; - mjModel* model = LoadModelFromString(kXml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; int nv = model->nv; - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); // 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) // qpos is initialized by RotateFlexGrid. @@ -1701,14 +1650,14 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) { } mju_addTo(data->qpos, deformation.data(), nv); - mj_forward(model, data); + mj_forward(model.get(), data.get()); // 1. Compute Analytic Jacobian (Approximate) // We use mulKD_dense to get K_approx std::vector H_approx(nv * nv, 0); // 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) // qfrc_passive = -dV/dq @@ -1717,9 +1666,9 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) { mjtNum eps = 1e-6; 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; - mj_forward(model, data_p); + mj_forward(model.get(), data_p); for (int j = 0; j < nv; j++) { // d(force_j)/d(q_i) @@ -1742,9 +1691,6 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) { // exists. EXPECT_GT(max_error, 1e-3) << "Jacobian approximation should differ from FD when deformed"; - - mj_deleteData(data); - mj_deleteModel(model); } } // namespace diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 9d2db304..ce3ab5f1 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -15,29 +15,29 @@ // Tests for engine/engine_forward.c. #include "src/engine/engine_forward.h" -#include "src/engine/engine_derivative.h" #include #include #include #include #include -#include #include +#include #include #include #include #include -#include #include +#include #include "src/engine/engine_callback.h" #include "src/engine/engine_core_util.h" +#include "src/engine/engine_derivative.h" #include "src/engine/engine_io.h" #include "test/fixture.h" #ifdef MEMORY_SANITIZER - #include +#include #endif namespace mujoco { @@ -57,7 +57,6 @@ void ExpectNear(mjtNum a, mjtNum b) { EXPECT_EQ(a, b); } - static const char* const kDampedActuatorsPath = "engine/testdata/derivative/damped_actuators.xml"; static const char* const kJointForceClamp = @@ -103,16 +102,16 @@ TEST_P(ParametrizedForwardTest, ActLimited) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); model->opt.integrator = GetParam().integrator; data->ctrl[0] = 1.0; // integrating up from 0, we will hit the clamp after 99 steps - for (int i=0; i < 200; i++) { - mj_step(model, data); + for (int i = 0; i < 200; i++) { + mj_step(model.get(), data.get()); // always greater than lower bound EXPECT_GT(data->act[0], -1); // after 99 steps we hit the upper bound @@ -122,8 +121,8 @@ TEST_P(ParametrizedForwardTest, ActLimited) { data->ctrl[0] = -1.0; // integrating down from 1, we will hit the clamp after 199 steps - for (int i=0; i < 300; i++) { - mj_step(model, data); + for (int i = 0; i < 300; i++) { + mj_step(model.get(), data.get()); // always smaller than upper bound EXPECT_LT(data->act[0], model->actuator_actrange[1]); // after 199 steps we hit the lower bound @@ -132,9 +131,6 @@ TEST_P(ParametrizedForwardTest, ActLimited) { EXPECT_NEAR(data->act[0], model->actuator_actrange[0], MjTol(0.0, 5e-6)); } } - - mj_deleteData(data); - mj_deleteModel(model); } INSTANTIATE_TEST_SUITE_P( @@ -168,25 +164,21 @@ TEST_F(ForwardTest, DamperDampens) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // move the joint data->ctrl[0] = 100.0; data->ctrl[1] = 0.0; - for (int i=0; i < 100; i++) - mj_step(model, data); + for (int i = 0; i < 100; i++) mj_step(model.get(), data.get()); // stop the joint with damping data->ctrl[0] = 0.0; data->ctrl[1] = 100.0; - for (int i=0; i < 1000; i++) - mj_step(model, data); + for (int i = 0; i < 1000; i++) mj_step(model.get(), data.get()); EXPECT_LE(data->qvel[0], std::numeric_limits::epsilon()); - mj_deleteData(data); - mj_deleteModel(model); } static const char* const kArmatureEquivalencePath = @@ -205,13 +197,13 @@ TEST_F(ForwardTest, ArmatureEquivalence) { mjtNum qpos_mse = 0; int nstep = 0; while (data->time < 4) { - data->ctrl[0] = data->ctrl[1] = mju_sin(2*data->time); + data->ctrl[0] = data->ctrl[1] = mju_sin(2 * data->time); mj_step(model, data); nstep++; mjtNum err = data->qpos[0] - data->qpos[2]; qpos_mse += err * err; } - EXPECT_LT(mju_sqrt(qpos_mse/nstep), 1e-3); + EXPECT_LT(mju_sqrt(qpos_mse / nstep), 1e-3); // no actuators model->opt.disableflags |= mjDSBL_ACTUATION; @@ -224,7 +216,7 @@ TEST_F(ForwardTest, ArmatureEquivalence) { mjtNum err = data->qpos[0] - data->qpos[2]; qpos_mse += err * err; } - EXPECT_LT(mju_sqrt(qpos_mse/nstep), 1e-3); + EXPECT_LT(mju_sqrt(qpos_mse / nstep), 1e-3); mj_deleteData(data); mj_deleteModel(model); @@ -256,45 +248,42 @@ TEST_F(ImplicitIntegratorTest, EulerDampDisable) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // step once, call mj_forward, save qvel and qacc - mj_step(model, data); - mj_forward(model, data); + mj_step(model.get(), data.get()); + mj_forward(model.get(), data.get()); std::vector qvel = AsVector(data->qvel, model->nv); std::vector qacc = AsVector(data->qacc, model->nv); // second step - mj_step(model, data); + mj_step(model.get(), data.get()); // compute finite-difference acceleration std::vector qacc_fd(model->nv); - for (int i=0; i < model->nv; i++) { + for (int i = 0; i < model->nv; i++) { qacc_fd[i] = (data->qvel[i] - qvel[i]) / model->opt.timestep; } // expect finite-differenced qacc to match to high precision EXPECT_THAT(qacc_fd, Pointwise(MjNear(1e-14, 1e-6), qacc)); // reach the same initial state - mj_resetData(model, data); - mj_step(model, data); + mj_resetData(model.get(), data.get()); + mj_step(model.get(), data.get()); // second step again, but with implicit integration of joint damping model->opt.disableflags &= ~mjDSBL_EULERDAMP; - mj_step(model, data); + mj_step(model.get(), data.get()); // compute finite-difference acceleration difference std::vector dqacc(model->nv); - for (int i=0; i < model->nv; i++) { + for (int i = 0; i < model->nv; i++) { dqacc[i] = (data->qvel[i] - qvel[i]) / model->opt.timestep; } // expect finite-differenced qacc to not match EXPECT_GT(mju_norm(dqacc.data(), model->nv), 1); - - mj_deleteData(data); - mj_deleteModel(model); } // Reducing timesteps reduces the difference between implicit/explicit @@ -315,9 +304,9 @@ TEST_F(ImplicitIntegratorTest, EulerDampLimit) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); mjtNum diff_norm_prev = -1; for (const mjtNum dt : {1e-2, 1e-3, 1e-4, 1e-5, 1e-6, 1e-7, 1e-8}) { @@ -326,33 +315,30 @@ TEST_F(ImplicitIntegratorTest, EulerDampLimit) { // step twice with implicit damping, save qvel model->opt.disableflags &= ~mjDSBL_EULERDAMP; - mj_resetData(model, data); - mj_step(model, data); - mj_step(model, data); + mj_resetData(model.get(), data.get()); + mj_step(model.get(), data.get()); + mj_step(model.get(), data.get()); std::vector qvel_imp = AsVector(data->qvel, model->nv); // step once, step again without implicit damping, save qvel - mj_resetData(model, data); - mj_step(model, data); + mj_resetData(model.get(), data.get()); + mj_step(model.get(), data.get()); model->opt.disableflags |= mjDSBL_EULERDAMP; - mj_step(model, data); + mj_step(model.get(), data.get()); std::vector qvel_exp = AsVector(data->qvel, model->nv); mjtNum diff_norm = 0; - for (int i=0; i < model->nv; i++) { + for (int i = 0; i < model->nv; i++) { diff_norm += (qvel_imp[i] - qvel_exp[i]) * (qvel_imp[i] - qvel_exp[i]); } diff_norm = mju_sqrt(diff_norm); - if (diff_norm_prev != -1){ + if (diff_norm_prev != -1) { EXPECT_LT(diff_norm, diff_norm_prev); } diff_norm_prev = diff_norm; } - - mj_deleteData(data); - mj_deleteModel(model); } // Euler and implicit should be equivalent if there is only joint damping @@ -373,21 +359,21 @@ TEST_F(ImplicitIntegratorTest, EulerImplicitEquivalent) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // step 10 times with Euler, save copy of qpos as vector - for (int i=0; i < 10; i++) { - mj_step(model, data); + for (int i = 0; i < 10; i++) { + mj_step(model.get(), data.get()); } std::vector qposEuler = AsVector(data->qpos, model->nq); // reset, step 10 times with implicit - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); model->opt.integrator = mjINT_IMPLICIT; - for (int i=0; i < 10; i++) { - mj_step(model, data); + for (int i = 0; i < 10; i++) { + mj_step(model.get(), data.get()); } // expect qpos vectors to be numerically different @@ -398,9 +384,6 @@ TEST_F(ImplicitIntegratorTest, EulerImplicitEquivalent) { // expect qpos vectors to be similar to high precision EXPECT_THAT(AsVector(data->qpos, model->nq), Pointwise(MjNear(1e-14, 1e-6), qposEuler)); - - mj_deleteData(data); - mj_deleteModel(model); } // Joint and actuator damping should integrate identically under implicit @@ -410,25 +393,25 @@ TEST_F(ImplicitIntegratorTest, JointActuatorEquivalent) { mjData* data = mj_makeData(model); // take 1000 steps with Euler - for (int i=0; i < 1000; i++) { + for (int i = 0; i < 1000; i++) { mj_step(model, data); } // expect corresponding joint values to be significantly different #ifndef mjUSESINGLE - EXPECT_GT(fabs(data->qpos[0]-data->qpos[2]), 1e-4); - EXPECT_GT(fabs(data->qpos[1]-data->qpos[3]), 1e-4); + EXPECT_GT(fabs(data->qpos[0] - data->qpos[2]), 1e-4); + EXPECT_GT(fabs(data->qpos[1] - data->qpos[3]), 1e-4); #endif // reset, take 10 steps with implicit mj_resetData(model, data); model->opt.integrator = mjINT_IMPLICIT; - for (int i=0; i < 10; i++) { + for (int i = 0; i < 10; i++) { mj_step(model, data); } // expect corresponding joint values to be insignificantly different - EXPECT_LT(fabs(data->qpos[0]-data->qpos[2]), MjTol(1e-16, 1e-6)); - EXPECT_LT(fabs(data->qpos[1]-data->qpos[3]), MjTol(1e-16, 1e-6)); + EXPECT_LT(fabs(data->qpos[0] - data->qpos[2]), MjTol(1e-16, 1e-6)); + EXPECT_LT(fabs(data->qpos[1] - data->qpos[3]), MjTol(1e-16, 1e-6)); mj_deleteData(data); mj_deleteModel(model); @@ -445,7 +428,7 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) { // take nstep steps with Euler, measure energy (potential + kinetic) model->opt.integrator = mjINT_EULER; - for (int i=0; i < nstep; i++) { + for (int i = 0; i < nstep; i++) { mj_step(model, data); } mjtNum energyEuler = data->energy[0] + data->energy[1]; @@ -453,7 +436,7 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) { // take nstep steps with implicit, measure energy model->opt.integrator = mjINT_IMPLICIT; mj_resetData(model, data); - for (int i=0; i < nstep; i++) { + for (int i = 0; i < nstep; i++) { mj_step(model, data); } mjtNum energyImplicit = data->energy[0] + data->energy[1]; @@ -461,7 +444,7 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) { // take nstep steps with 4th order Runge-Kutta, measure energy model->opt.integrator = mjINT_RK4; mj_resetData(model, data); - for (int i=0; i < nstep; i++) { + for (int i = 0; i < nstep; i++) { mj_step(model, data); } mjtNum energyRK4 = data->energy[0] + data->energy[1]; @@ -531,9 +514,9 @@ TEST_F(ImplicitIntegratorTest, ConservationMidpoint) { for (auto xml : {xml1, xml2, xml3}) { SCOPED_TRACE(testing::Message() << "XML case " << xml_idx++); char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); const int nstep = 500; mjtNum energy_drift[2], angmom_drift[2]; // [0]=midpoint, [1]=rk4 @@ -543,22 +526,22 @@ TEST_F(ImplicitIntegratorTest, ConservationMidpoint) { model->opt.integrator = integrator; // reset - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); data->qvel[3] = 1.0; data->qvel[4] = 2.0; data->qvel[5] = 3.0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); mjtNum initial_energy = data->energy[1]; mjtNum initial_angmom[3]; - mj_subtreeVel(model, data); + mj_subtreeVel(model.get(), data.get()); mju_copy3(initial_angmom, data->subtree_angmom); - for (int i=0; i < nstep; i++) { - mj_step(model, data); + for (int i = 0; i < nstep; i++) { + mj_step(model.get(), data.get()); } energy_drift[idx] = fabs(data->energy[1] - initial_energy); - mj_subtreeVel(model, data); + mj_subtreeVel(model.get(), data.get()); mjtNum angmom_err[3]; mju_sub3(angmom_err, data->subtree_angmom, initial_angmom); angmom_drift[idx] = mju_norm3(angmom_err); @@ -572,9 +555,6 @@ TEST_F(ImplicitIntegratorTest, ConservationMidpoint) { // both should conserve angular momentum well EXPECT_LT(angmom_drift[0], MjTol(1e-3, 1e-2)); EXPECT_LT(angmom_drift[1], MjTol(1e-3, 1e-2)); - - mj_deleteData(data); - mj_deleteModel(model); } } @@ -615,8 +595,8 @@ TEST_F(ImplicitIntegratorTest, MidpointConvergenceOrder) { for (auto xml : {xml1, xml2}) { SCOPED_TRACE(testing::Message() << "XML case " << xml_idx++); char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; mjtNum T = 1.0; mjtNum h_coarse = 0.02; @@ -624,7 +604,7 @@ TEST_F(ImplicitIntegratorTest, MidpointConvergenceOrder) { auto run = [&](mjtNum h, mjtNum quat_out[4]) { model->opt.timestep = h; - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); data->qvel[3] = 1.0; data->qvel[4] = 2.0; @@ -632,11 +612,10 @@ TEST_F(ImplicitIntegratorTest, MidpointConvergenceOrder) { int nstep = (int)(T / h + 0.5); for (int i = 0; i < nstep; i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); } mju_copy4(quat_out, data->qpos + 3); - mj_deleteData(data); }; run(h_coarse, quat_coarse); @@ -660,8 +639,6 @@ TEST_F(ImplicitIntegratorTest, MidpointConvergenceOrder) { mjtNum ratio = err_coarse / err_fine; EXPECT_GT(ratio, 3.5); EXPECT_LT(ratio, 4.5); - - mj_deleteModel(model); } } @@ -669,27 +646,27 @@ TEST_F(ImplicitIntegratorTest, MidpointConvergenceOrder) { TEST_F(ImplicitIntegratorTest, MidpointNewtonConvergence) { // inertia ratios: symmetric, mildly asymmetric, extremely asymmetric mjtNum inertias[][3] = { - {1.0, 1.0, 1.0}, - {1.0, 2.0, 3.0}, - {0.01, 1.0, 100.0}, - {1.0, 1.0, 1000.0}, + {1.0, 1.0, 1.0}, + {1.0, 2.0, 3.0}, + {0.01, 1.0, 100.0}, + {1.0, 1.0, 1000.0}, }; mjtNum timesteps[] = {0.001, 0.01, 0.1}; mjtNum velocities[][3] = { - {1.0, 2.0, 3.0}, - {100.0, 0.0, 0.0}, - {10.0, 10.0, 10.0}, - {0.01, 0.01, 100.0}, + {1.0, 2.0, 3.0}, + {100.0, 0.0, 0.0}, + {10.0, 10.0, 10.0}, + {0.01, 0.01, 100.0}, }; mjtNum q_identity[4] = {1, 0, 0, 0}; mjtNum torques[][3] = { - {0, 0, 0}, - {10.0, 20.0, 30.0}, - {100.0, 0.0, 0.0}, - {0.0, 0.0, 100.0}, + {0, 0, 0}, + {10.0, 20.0, 30.0}, + {100.0, 0.0, 0.0}, + {0.0, 0.0, 100.0}, }; int max_iter = 0; @@ -708,8 +685,8 @@ TEST_F(ImplicitIntegratorTest, MidpointNewtonConvergence) { tau_ext, NULL, h, v_new); EXPECT_LT(niter, 10) << "Failed for I=(" << I[0] << "," << I[1] << "," << I[2] << ")" - << " h=" << h - << " w=(" << w[0] << "," << w[1] << "," << w[2] << ")" + << " h=" << h << " w=(" << w[0] << "," << w[1] << "," << w[2] + << ")" << " tau=(" << tau[0] << "," << tau[1] << "," << tau[2] << ")"; max_iter = std::max(max_iter, niter); total_iter += niter; @@ -728,29 +705,29 @@ TEST_F(ImplicitIntegratorTest, MidpointFullNewtonConvergence) { mjtNum masses[] = {0.1, 1.0, 10.0}; mjtNum inertias[][3] = { - {1.0, 1.0, 1.0}, - {1.0, 2.0, 3.0}, - {0.01, 1.0, 100.0}, + {1.0, 1.0, 1.0}, + {1.0, 2.0, 3.0}, + {0.01, 1.0, 100.0}, }; mjtNum offsets[][3] = { - {0.1, 0.0, 0.0}, - {0.05, 0.03, 0.02}, - {0.0, 0.0, 0.5}, + {0.1, 0.0, 0.0}, + {0.05, 0.03, 0.02}, + {0.0, 0.0, 0.5}, }; mjtNum timesteps[] = {0.001, 0.01, 0.1}; mjtNum velocities[][6] = { - {1.0, 0.0, 0.0, 1.0, 2.0, 3.0}, - {0.0, 0.0, 0.0, 10.0, 10.0, 10.0}, - {5.0, 5.0, 5.0, 0.01, 0.01, 100.0}, + {1.0, 0.0, 0.0, 1.0, 2.0, 3.0}, + {0.0, 0.0, 0.0, 10.0, 10.0, 10.0}, + {5.0, 5.0, 5.0, 0.01, 0.01, 100.0}, }; mjtNum q_identity[4] = {1, 0, 0, 0}; mjtNum forces[][6] = { - {0, 0, 0, 0, 0, 0}, - {10.0, 20.0, 30.0, 1.0, 2.0, 3.0}, + {0, 0, 0, 0, 0, 0}, + {10.0, 20.0, 30.0, 1.0, 2.0, 3.0}, }; int max_iter = 0; @@ -764,11 +741,11 @@ TEST_F(ImplicitIntegratorTest, MidpointFullNewtonConvergence) { for (auto& vel : velocities) { for (auto& frc : forces) { mjtNum v_new[6]; - int niter = mj_midpoint(mass, I, r, q_identity, q_identity, - vel, frc, NULL, h, v_new); + int niter = mj_midpoint(mass, I, r, q_identity, q_identity, vel, + frc, NULL, h, v_new); EXPECT_LT(niter, 10) - << "Failed for mass=" << mass - << " I=(" << I[0] << "," << I[1] << "," << I[2] << ")" + << "Failed for mass=" << mass << " I=(" << I[0] << "," << I[1] + << "," << I[2] << ")" << " r=(" << r[0] << "," << r[1] << "," << r[2] << ")" << " h=" << h; max_iter = std::max(max_iter, niter); @@ -806,29 +783,30 @@ TEST_F(ImplicitIntegratorTest, MidpointEligibility) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d1 = mj_makeData(m); - mjData* d2 = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d1 = MakeData(m); + MjDataPtr d2 = MakeData(m); int nsteps = 50; - auto spin_and_compare = [&](const char* label, - bool expect_midpoint) { - mj_resetData(m, d1); - mj_resetData(m, d2); + auto spin_and_compare = [&](const char* label, bool expect_midpoint) { + mj_resetData(m.get(), d1.get()); + mj_resetData(m.get(), d2.get()); d1->qvel[3] = d2->qvel[3] = 5; d1->qvel[4] = d2->qvel[4] = 3; d1->qvel[5] = d2->qvel[5] = 1; // d1: midpoint enabled (default) m->opt.enableflags &= ~mjENBL_INVDISCRETE; - for (int i = 0; i < nsteps; i++) mj_step(m, d1); + for (int i = 0; i < nsteps; i++) mj_step(m.get(), d1.get()); // d2: midpoint disabled m->opt.enableflags |= mjENBL_INVDISCRETE; - mj_resetData(m, d2); - d2->qvel[3] = 5; d2->qvel[4] = 3; d2->qvel[5] = 1; - for (int i = 0; i < nsteps; i++) mj_step(m, d2); + mj_resetData(m.get(), d2.get()); + d2->qvel[3] = 5; + d2->qvel[4] = 3; + d2->qvel[5] = 1; + for (int i = 0; i < nsteps; i++) mj_step(m.get(), d2.get()); m->opt.enableflags &= ~mjENBL_INVDISCRETE; // compare angular velocities @@ -838,11 +816,9 @@ TEST_F(ImplicitIntegratorTest, MidpointEligibility) { diff += d * d; } if (expect_midpoint) { - EXPECT_GT(diff, 1e-6) - << label << ": expected midpoint to be applied"; + EXPECT_GT(diff, 1e-6) << label << ": expected midpoint to be applied"; } else { - EXPECT_LT(diff, 1e-20) - << label << ": expected midpoint to be skipped"; + EXPECT_LT(diff, 1e-20) << label << ": expected midpoint to be skipped"; } }; @@ -877,30 +853,34 @@ TEST_F(ImplicitIntegratorTest, MidpointEligibility) { m->opt.disableflags &= ~mjDSBL_ISLAND; } - mj_resetData(m, d1); - mj_resetData(m, d2); + mj_resetData(m.get(), d1.get()); + mj_resetData(m.get(), d2.get()); d1->qpos[2] = d2->qpos[2] = 0.05; d1->qvel[3] = d2->qvel[3] = 5; d1->qvel[4] = d2->qvel[4] = 3; d1->qvel[5] = d2->qvel[5] = 1; // verify contacts are active - mj_forward(m, d1); + mj_forward(m.get(), d1.get()); ASSERT_GT(d1->ncon, 0) << "body should be in contact with the plane"; // single step with midpoint enabled - mj_resetData(m, d1); + mj_resetData(m.get(), d1.get()); d1->qpos[2] = 0.05; - d1->qvel[3] = 5; d1->qvel[4] = 3; d1->qvel[5] = 1; + d1->qvel[3] = 5; + d1->qvel[4] = 3; + d1->qvel[5] = 1; m->opt.enableflags &= ~mjENBL_INVDISCRETE; - mj_step(m, d1); + mj_step(m.get(), d1.get()); // single step with midpoint disabled - mj_resetData(m, d2); + mj_resetData(m.get(), d2.get()); d2->qpos[2] = 0.05; - d2->qvel[3] = 5; d2->qvel[4] = 3; d2->qvel[5] = 1; + d2->qvel[3] = 5; + d2->qvel[4] = 3; + d2->qvel[5] = 1; m->opt.enableflags |= mjENBL_INVDISCRETE; - mj_step(m, d2); + mj_step(m.get(), d2.get()); m->opt.enableflags &= ~mjENBL_INVDISCRETE; mjtNum diff = 0; @@ -908,15 +888,11 @@ TEST_F(ImplicitIntegratorTest, MidpointEligibility) { mjtNum d = d1->qvel[k] - d2->qvel[k]; diff += d * d; } - EXPECT_LT(diff, 1e-20) - << "contact (island " << (disable_island ? "disabled" : "enabled") - << "): expected midpoint to be skipped"; + EXPECT_LT(diff, 1e-20) << "contact (island " + << (disable_island ? "disabled" : "enabled") + << "): expected midpoint to be skipped"; } m->opt.disableflags &= ~mjDSBL_ISLAND; - - mj_deleteData(d2); - mj_deleteData(d1); - mj_deleteModel(m); } // model with degenerate translational inertia @@ -939,21 +915,18 @@ TEST_F(ForwardTest, DegenerateInertia) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); for (int i = 0; i < 1000; i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_EQ(data->warning[mjWARN_BADQACC].number, 0) << "divergence at timestep " << i; if (data->warning[mjWARN_BADQACC].number != 0) { break; } } - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(ForwardTest, ControlClamping) { @@ -972,25 +945,25 @@ TEST_F(ForwardTest, ControlClamping) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // for the unclamped actuator, ctrl={1, 2} produce different accelerations data->ctrl[0] = 1; - mj_forward(model, data); + mj_forward(model.get(), data.get()); mjtNum qacc1 = data->qacc[0]; data->ctrl[0] = 2; - mj_forward(model, data); + mj_forward(model.get(), data.get()); mjtNum qacc2 = data->qacc[0]; EXPECT_NE(qacc1, qacc2); // for the clamped actuator, ctrl={1, 2} produce identical accelerations data->ctrl[1] = 1; - mj_forward(model, data); + mj_forward(model.get(), data.get()); qacc1 = data->qacc[0]; data->ctrl[1] = 2; - mj_forward(model, data); + mj_forward(model.get(), data.get()); qacc2 = data->qacc[0]; EXPECT_EQ(qacc1, qacc2); @@ -1002,31 +975,26 @@ TEST_F(ForwardTest, ControlClamping) { // for the unclamped actuator, huge raises warning warning_handler.ExpectWarnings( "Nan, Inf or huge value in CTRL at ACTUATOR 0"); - data->ctrl[0] = 10*mjMAXVAL; - mj_forward(model, data); + data->ctrl[0] = 10 * mjMAXVAL; + mj_forward(model.get(), data.get()); testing::Mock::VerifyAndClearExpectations(&warning_handler); // for the clamped actuator, huge does not raise warning EXPECT_CALL(warning_handler, Warn(_)).Times(0); - mj_resetData(model, data); - data->ctrl[1] = 10*mjMAXVAL; - mj_forward(model, data); + mj_resetData(model.get(), data.get()); + data->ctrl[1] = 10 * mjMAXVAL; + mj_forward(model.get(), data.get()); testing::Mock::VerifyAndClearExpectations(&warning_handler); // for the clamped actuator, NaN raises warning warning_handler.ExpectWarnings( "Nan, Inf or huge value in CTRL at ACTUATOR 1"); - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); data->ctrl[1] = std::numeric_limits::quiet_NaN(); - mj_forward(model, data); - - mj_deleteData(data); - mj_deleteModel(model); + mj_forward(model.get(), data.get()); } -void control_callback(const mjModel* m, mjData *d) { - d->ctrl[0] = 2; -} +void control_callback(const mjModel* m, mjData* d) { d->ctrl[0] = 2; } TEST_F(ForwardTest, MjcbControlDisabled) { static constexpr char xml[] = R"( @@ -1043,30 +1011,27 @@ TEST_F(ForwardTest, MjcbControlDisabled) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // install global control callback mjcb_control = control_callback; // call forward - mj_forward(model, data); + mj_forward(model.get(), data.get()); // expect that callback was used EXPECT_EQ(data->ctrl[0], 2.0); // reset, disable actuation, call forward - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); model->opt.disableflags |= mjDSBL_ACTUATION; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // expect that callback was not used EXPECT_EQ(data->ctrl[0], 0.0); // remove global control callback mjcb_control = nullptr; - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(ForwardTest, gravcomp) { @@ -1090,13 +1055,15 @@ TEST_F(ForwardTest, gravcomp) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; - mjData* data = mj_makeData(model); - while (data->time < 1) { mj_step(model, data); } + MjDataPtr data = MakeData(model); + while (data->time < 1) { + mj_step(model.get(), data.get()); + } - mjtNum dist = 0.5*mju_norm3(model->opt.gravity)*(data->time*data->time); + mjtNum dist = 0.5 * mju_norm3(model->opt.gravity) * (data->time * data->time); // expect that body 1 moved down, allowing some slack from our estimate EXPECT_NEAR(data->qpos[0], -dist, 0.011); @@ -1106,9 +1073,6 @@ TEST_F(ForwardTest, gravcomp) { // expect that body 3 moves up the same distance that body 0 moved down EXPECT_EQ(data->qpos[0], -data->qpos[2]); - - mj_deleteData(data); - mj_deleteModel(model); } // test disabling of equality constraints @@ -1127,14 +1091,14 @@ TEST_F(ForwardTest, eq_active) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); // simulate for 1 second while (data->time < 1) { - mj_step(model, data); + mj_step(model.get(), data.get()); } // expect that the body has barely moved @@ -1143,7 +1107,7 @@ TEST_F(ForwardTest, eq_active) { // turn the equality off, simulate for another second data->eq_active[0] = 0; while (data->time < 2) { - mj_step(model, data); + mj_step(model.get(), data.get()); } // expect that the body has fallen about 5m @@ -1153,14 +1117,11 @@ TEST_F(ForwardTest, eq_active) { // turn the equality back on, simulate for another second data->eq_active[0] = 1; while (data->time < 3) { - mj_step(model, data); + mj_step(model.get(), data.get()); } // expect that the body has snapped back EXPECT_LT(mju_abs(data->qpos[0]), 0.001); - - mj_deleteData(data); - mj_deleteModel(model); } // test that normalized and denormalized quats give the same result @@ -1190,27 +1151,27 @@ TEST_F(ForwardTest, NormalizeQuats) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; - mjData* data_u = mj_makeData(model); + MjDataPtr data_u = MakeData(model); - // we'll compare all the memory, so unpoison it first - #ifdef MEMORY_SANITIZER - __msan_unpoison(data_u->buffer, data_u->nbuffer); - __msan_unpoison(data_u->arena, data_u->narena); - #endif +// we'll compare all the memory, so unpoison it first +#ifdef MEMORY_SANITIZER + __msan_unpoison(data_u->buffer, data_u->nbuffer); + __msan_unpoison(data_u->arena, data_u->narena); +#endif // set quats to denormalized values, non-zero velocities for (int i = 3; i < model->nq; i++) data_u->qpos[i] = i; - for (int i = 0; i < model->nv; i++) data_u->qvel[i] = 0.1*i; + for (int i = 0; i < model->nv; i++) data_u->qvel[i] = 0.1 * i; // copy data and normalize quats - mjData* data_n = mj_copyData(nullptr, model, data_u); - mj_normalizeQuat(model, data_n->qpos); + mjData* data_n = mj_copyData(nullptr, model.get(), data_u.get()); + mj_normalizeQuat(model.get(), data_n->qpos); // call forward, expect quats to be untouched - mj_forward(model, data_u); + mj_forward(model.get(), data_u.get()); for (int i = 3; i < model->nq; i++) { EXPECT_EQ(data_u->qpos[i], (mjtNum)i); } @@ -1219,50 +1180,48 @@ TEST_F(ForwardTest, NormalizeQuats) { EXPECT_EQ(data_u->nl, 1); // step both models - mj_step(model, data_u); - mj_step(model, data_n); + mj_step(model.get(), data_u.get()); + mj_step(model.get(), data_n); - // expect everything to match - #define X(type, name, nr, nc) \ - for (int i = 0; i < model->nr; i++) \ - for (int j = 0; j < nc; j++) \ - ExpectNear(data_n->name[i*nc+j], data_u->name[i*nc+j]); +// expect everything to match +#define X(type, name, nr, nc) \ + for (int i = 0; i < model->nr; i++) \ + for (int j = 0; j < nc; j++) \ + ExpectNear(data_n->name[i * nc + j], data_u->name[i * nc + j]); MJDATA_POINTERS; - #undef X +#undef X // repeat the above with RK4 integrator model->opt.integrator = mjINT_RK4; // reset data, unpoison - mj_resetData(model, data_u); - #ifdef MEMORY_SANITIZER - __msan_unpoison(data_u->buffer, data_u->nbuffer); - __msan_unpoison(data_u->arena, data_u->narena); - #endif + mj_resetData(model.get(), data_u.get()); +#ifdef MEMORY_SANITIZER + __msan_unpoison(data_u->buffer, data_u->nbuffer); + __msan_unpoison(data_u->arena, data_u->narena); +#endif // set quats to un-normalized values, non-zero velocities for (int i = 3; i < model->nq; i++) data_u->qpos[i] = i; - for (int i = 0; i < model->nv; i++) data_u->qvel[i] = 0.1*i; + for (int i = 0; i < model->nv; i++) data_u->qvel[i] = 0.1 * i; // copy data and normalize quats - mj_copyData(data_n, model, data_u); - mj_normalizeQuat(model, data_n->qpos); + mj_copyData(data_n, model.get(), data_u.get()); + mj_normalizeQuat(model.get(), data_n->qpos); // step both models - mj_step(model, data_u); - mj_step(model, data_n); + mj_step(model.get(), data_u.get()); + mj_step(model.get(), data_n); - // expect everything to match - #define X(type, name, nr, nc) \ - for (int i = 0; i < model->nr; i++) \ - for (int j = 0; j < nc; j++) \ - ExpectNear(data_n->name[i*nc+j], data_u->name[i*nc+j]); +// expect everything to match +#define X(type, name, nr, nc) \ + for (int i = 0; i < model->nr; i++) \ + for (int j = 0; j < nc; j++) \ + ExpectNear(data_n->name[i * nc + j], data_u->name[i * nc + j]); MJDATA_POINTERS; - #undef X +#undef X mj_deleteData(data_n); - mj_deleteData(data_u); - mj_deleteModel(model); } // test that normalized and denormalized quats give the same result @@ -1280,38 +1239,35 @@ TEST_F(ForwardTest, MocapQuats) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; - mjData* data = mj_makeData(model); - mj_forward(model, data); + MjDataPtr data = MakeData(model); + mj_forward(model.get(), data.get()); // expect mocap_quat to be normalized (by the compiler) for (int i = 0; i < 4; i++) { EXPECT_NEAR(data->mocap_quat[i], 0.5, MjTol(0, 1e-6)); - EXPECT_NEAR(data->xquat[4+i], 0.5, MjTol(0, 1e-6)); + EXPECT_NEAR(data->xquat[4 + i], 0.5, MjTol(0, 1e-6)); } // write denormalized quats to mocap_quat, call forward again for (int i = 0; i < 4; i++) { data->mocap_quat[i] = 1; } - mj_forward(model, data); + mj_forward(model.get(), data.get()); // expect mocap_quat to remain denormalized, but xquat to be normalized for (int i = 0; i < 4; i++) { EXPECT_NEAR(data->mocap_quat[i], 1, MjTol(0, 1e-6)); - EXPECT_NEAR(data->xquat[4+i], 0.5, MjTol(0, 1e-6)); + EXPECT_NEAR(data->xquat[4 + i], 0.5, MjTol(0, 1e-6)); } - - mj_deleteData(data); - mj_deleteModel(model); } // user defined 2nd-order activation dynamics: frequency-controlled oscillator // note that scalar mjcb_act_dyn callbacks are expected to return act_dot, but // since we have a vector output we write into act_dot directly -mjtNum oscillator(const mjModel* m, const mjData *d, int id) { +mjtNum oscillator(const mjModel* m, const mjData* d, int id) { // check that actnum == 2 if (m->actuator_actnum[id] != 2) { mju_error("callback expected actnum == 2"); @@ -1322,7 +1278,7 @@ mjtNum oscillator(const mjModel* m, const mjData *d, int id) { mjtNum* act_dot = d->act_dot + m->actuator_actadr[id]; // harmonic oscillator with controlled frequency - mjtNum frequency = 2*mjPI*d->ctrl[id]; + mjtNum frequency = 2 * mjPI * d->ctrl[id]; act_dot[0] = -act[1] * frequency; act_dot[1] = act[0] * frequency; @@ -1345,9 +1301,9 @@ TEST_F(ForwardTest, MjcbActDynSecondOrderExpectsActnum) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // install global dynamics callback mjcb_act_dyn = oscillator; @@ -1355,23 +1311,20 @@ TEST_F(ForwardTest, MjcbActDynSecondOrderExpectsActnum) { // for two arbitrary frequencies, compare actuator force as output by the // user-defined oscillator and analytical sine function for (mjtNum frequency : {1.5, 0.7}) { - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); data->ctrl[0] = frequency; // set desired oscillation frequency - data->act[0] = 1; // initialise activation + data->act[0] = 1; // initialise activation // simulate and compare to sine function while (data->time < 1) { - mjtNum expected_force = mju_sin(2*mjPI*data->time*frequency); - mj_step(model, data); + mjtNum expected_force = mju_sin(2 * mjPI * data->time * frequency); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->actuator_force[0], expected_force, .01); } } // uninstall global dynamics callback mjcb_act_dyn = nullptr; - - mj_deleteData(data); - mj_deleteModel(model); } // ------------------------------ actuators ----------------------------------- @@ -1404,9 +1357,9 @@ TEST_F(ActuatorTest, ExpectedAdhesionForce) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // iterate over cone type for (mjtCone cone : {mjCONE_ELLIPTIC, mjCONE_PYRAMIDAL}) { @@ -1415,33 +1368,31 @@ TEST_F(ActuatorTest, ExpectedAdhesionForce) { // iterate over condim for (int condim : {1, 3, 4, 6}) { // set condim - for (int id=0; id < model->ngeom; id++) { + for (int id = 0; id < model->ngeom; id++) { model->geom_condim[id] = condim; } // iterate over actuators - for (int id=0; id < 2; id++) { + for (int id = 0; id < 2; id++) { // set ctrl > 1, expect free body to not fall - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); data->ctrl[id] = 1.01; for (int i = 0; i < 100; i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); } // moved down at most 10 microns EXPECT_GT(data->qpos[2], -1e-5); // set ctrl < 1, expect free body to fall below 1cm - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); data->ctrl[id] = 0.99; for (int i = 0; i < 100; i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); } // fell lower than 1cm EXPECT_LT(data->qpos[2], -0.01); } } } - mj_deleteData(data); - mj_deleteModel(model); } // Actuator force clamping at joints @@ -1493,10 +1444,10 @@ TEST_F(ActuatorTest, ActuatorGravcomp) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); - mj_forward(model, data); + mj_forward(model.get(), data.get()); // expect force clamping as specified in the model EXPECT_EQ(data->actuator_force[0], 0); @@ -1507,7 +1458,7 @@ TEST_F(ActuatorTest, ActuatorGravcomp) { // reduce gravity so gravcomp is not clamped model->opt.gravity[2] = -1; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_EQ(data->actuator_force[0], 0); EXPECT_EQ(data->qfrc_actuator[0], 1); EXPECT_EQ(data->qfrc_passive[0], 0); @@ -1516,7 +1467,7 @@ TEST_F(ActuatorTest, ActuatorGravcomp) { // add control, see that it adds up data->ctrl[0] = 0.5; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_EQ(data->actuator_force[0], 0.5); EXPECT_EQ(data->qfrc_actuator[0], 1.5); EXPECT_EQ(data->qfrc_passive[0], 0); @@ -1525,7 +1476,7 @@ TEST_F(ActuatorTest, ActuatorGravcomp) { // add larger control, expect clamping data->ctrl[0] = 1.5; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_EQ(data->actuator_force[0], 1.5); EXPECT_EQ(data->qfrc_actuator[0], 2); EXPECT_EQ(data->qfrc_passive[0], 0); @@ -1534,15 +1485,12 @@ TEST_F(ActuatorTest, ActuatorGravcomp) { // disable actgravcomp, expect gravcomp as a passive force model->jnt_actgravcomp[0] = 0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_EQ(data->actuator_force[0], 1.5); EXPECT_EQ(data->qfrc_actuator[0], 1.5); EXPECT_EQ(data->qfrc_passive[0], 1); EXPECT_EQ(data->sensordata[0], 1.5); EXPECT_EQ(data->sensordata[1], 1.5); - - mj_deleteData(data); - mj_deleteModel(model); } // Check that dampratio works as expected @@ -1569,15 +1517,15 @@ TEST_F(ActuatorTest, DampRatio) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); data->qpos[0] = data->qpos[1] = -0.1; mjtNum under_damped = data->qpos[0]; mjtNum over_damped = data->qpos[1]; while (data->time < 10) { - mj_step(model, data); + mj_step(model.get(), data.get()); under_damped = mju_max(under_damped, data->qpos[0]); over_damped = mju_max(over_damped, data->qpos[1]); } @@ -1589,12 +1537,8 @@ TEST_F(ActuatorTest, DampRatio) { // expect slightly overdamped to slightly undershoot EXPECT_LT(over_damped, 0); EXPECT_GT(over_damped, -1e-6); - - mj_deleteData(data); - mj_deleteModel(model); } - // Check dampratio for actuators with nontrivial transmission TEST_F(ActuatorTest, DampRatioTendon) { const std::string xml_path = @@ -1612,7 +1556,7 @@ TEST_F(ActuatorTest, DampRatioTendon) { } // expect first and second fingers to move together - double tol = 1e-10; + double tol = 1e-10; EXPECT_THAT(AsVector(data->qpos, 4), Pointwise(MjNear(tol, tol), AsVector(data->qpos + 4, 4))); EXPECT_THAT(AsVector(data->qvel, 4), @@ -1659,15 +1603,15 @@ TEST_F(DCMotorTest, IntVelocityEquivalence) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // Apply a time-varying velocity command while (data->time < 1.0) { data->ctrl[0] = mju_sin(20 * data->time); data->ctrl[1] = mju_sin(20 * data->time); - mj_step(model, data); + mj_step(model.get(), data.get()); // Both actuators should integrate identical states EXPECT_MJTNUM_EQ(data->act[0], data->act[1]); @@ -1681,9 +1625,6 @@ TEST_F(DCMotorTest, IntVelocityEquivalence) { EXPECT_NEAR(data->actuator_force[0], data->actuator_force[1], MjTol(1e-14, 1e-6)); } - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, StatelessSteadyState) { @@ -1701,9 +1642,9 @@ TEST_F(DCMotorTest, StatelessSteadyState) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); double K = 0.05; double R = 2.0; @@ -1712,14 +1653,11 @@ TEST_F(DCMotorTest, StatelessSteadyState) { data->ctrl[0] = V; data->qvel[0] = omega; - mj_forward(model, data); + mj_forward(model.get(), data.get()); double expected_force = K / R * (V - K * omega); EXPECT_NEAR(data->actuator_force[0], expected_force, MjTol(1e-12, 1e-5)); EXPECT_EQ(model->actuator_actnum[0], 0); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, CurrentFilterConverges) { @@ -1739,9 +1677,9 @@ TEST_F(DCMotorTest, CurrentFilterConverges) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); ASSERT_EQ(model->actuator_actnum[0], 1); @@ -1751,7 +1689,7 @@ TEST_F(DCMotorTest, CurrentFilterConverges) { data->ctrl[0] = V; for (int i = 0; i < 10000; i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); } double omega = data->qvel[0]; @@ -1760,9 +1698,6 @@ TEST_F(DCMotorTest, CurrentFilterConverges) { EXPECT_NEAR(data->act[0], i_ss, MjTol(1e-6, 1e-4)); EXPECT_NEAR(data->actuator_force[0], expected_force, MjTol(1e-6, 1e-4)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, CurrentFilterExactIntegration) { @@ -1782,16 +1717,16 @@ TEST_F(DCMotorTest, CurrentFilterExactIntegration) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); double R = 2.0; double te = 0.01 / R; double V = 12.0; data->ctrl[0] = V; - mj_step(model, data); + mj_step(model.get(), data.get()); double h = model->opt.timestep; double exact_current = V / R * (1 - mju_exp(-h / te)); @@ -1800,9 +1735,6 @@ TEST_F(DCMotorTest, CurrentFilterExactIntegration) { double euler_current = V / R * h / te; EXPECT_GT(std::abs(data->act[0] - euler_current), std::abs(data->act[0] - exact_current)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, CoggingTorque) { @@ -1821,9 +1753,9 @@ TEST_F(DCMotorTest, CoggingTorque) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); double A = 0.1, Np = 6, phi = 0; double K = 0.05, R = 2.0; @@ -1832,15 +1764,12 @@ TEST_F(DCMotorTest, CoggingTorque) { data->ctrl[0] = V; data->qpos[0] = pos; - mj_forward(model, data); + mj_forward(model.get(), data.get()); double electrical_force = K / R * V; double cogging = A * mju_sin(Np * pos + phi); EXPECT_NEAR(data->actuator_force[0], electrical_force + cogging, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, CoggingBypassesSaturation) { @@ -1859,24 +1788,21 @@ TEST_F(DCMotorTest, CoggingBypassesSaturation) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); double A = 0.1, Np = 6, phi = 0; double pos = 1.0; data->ctrl[0] = 100.0; data->qpos[0] = pos; - mj_forward(model, data); + mj_forward(model.get(), data.get()); double cogging = A * mju_sin(Np * pos + phi); EXPECT_NEAR(model->actuator_forcerange[1], 0.001, MjTol(1e-12, 1e-5)); EXPECT_GT(mju_abs(data->actuator_force[0]), 0.001); EXPECT_NEAR(data->actuator_force[0], 0.001 + cogging, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, LuGreViscousFriction) { @@ -1895,9 +1821,9 @@ TEST_F(DCMotorTest, LuGreViscousFriction) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); ASSERT_EQ(model->actuator_actnum[0], 1); @@ -1907,7 +1833,7 @@ TEST_F(DCMotorTest, LuGreViscousFriction) { data->ctrl[0] = 0; data->qvel[0] = omega; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_MJTNUM_EQ(model->actuator_damping[0], sigma2); double electrical_force = K / R * (0 - K * omega); @@ -1916,9 +1842,6 @@ TEST_F(DCMotorTest, LuGreViscousFriction) { double lugre_force = 100 * z + sigma1 * z_dot; EXPECT_NEAR(data->actuator_force[0], electrical_force - lugre_force, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, ThermalRiseAndFall) { @@ -1938,9 +1861,9 @@ TEST_F(DCMotorTest, ThermalRiseAndFall) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); int adr = model->actuator_actadr[0]; ASSERT_EQ(model->actuator_actnum[0], 1); @@ -1953,22 +1876,19 @@ TEST_F(DCMotorTest, ThermalRiseAndFall) { data->ctrl[0] = V; - mj_step(model, data); + mj_step(model.get(), data.get()); double dT1 = h * P / C; EXPECT_NEAR(data->act[adr], dT1, MjTol(1e-11, 1e-4)); - mj_step(model, data); + mj_step(model.get(), data.get()); double dT2 = dT1 + h * (P - dT1 / RT) / C; EXPECT_NEAR(data->act[adr], dT2, MjTol(1e-11, 1e-4)); data->ctrl[0] = 0; - mj_step(model, data); + mj_step(model.get(), data.get()); double dT3 = dT2 + h * (0 - dT2 / RT) / C; EXPECT_NEAR(data->act[adr], dT3, MjTol(1e-11, 1e-4)); EXPECT_LT(data->act[adr], dT2); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, ThermalSteadyState) { @@ -1988,9 +1908,9 @@ TEST_F(DCMotorTest, ThermalSteadyState) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); double R = 2.0, V = 10.0; double RT = 0.1; @@ -1998,14 +1918,11 @@ TEST_F(DCMotorTest, ThermalSteadyState) { data->ctrl[0] = V; for (int i = 0; i < 10000; i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); } int adr = model->actuator_actadr[0]; EXPECT_NEAR(data->act[adr], dT_ss, 1e-4); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, ThermalAffectsForce) { @@ -2024,9 +1941,9 @@ TEST_F(DCMotorTest, ThermalAffectsForce) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); double K = 0.05, R = 2.0, V = 10.0; double alpha = 0.004; @@ -2034,20 +1951,17 @@ TEST_F(DCMotorTest, ThermalAffectsForce) { data->ctrl[0] = V; data->act[adr] = 0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); double force_cold = data->actuator_force[0]; EXPECT_NEAR(force_cold, K / R * V, MjTol(1e-12, 1e-5)); double dT = 50; data->act[adr] = dT; - mj_forward(model, data); + mj_forward(model.get(), data.get()); double R_hot = R * (1 + alpha * dT); double force_hot = data->actuator_force[0]; EXPECT_NEAR(force_hot, K / R_hot * V, MjTol(1e-12, 1e-5)); EXPECT_LT(force_hot, force_cold); - - mj_deleteData(data); - mj_deleteModel(model); } // Temperature slot must be correctly offset past slew and integral states. @@ -2068,9 +1982,9 @@ TEST_F(DCMotorTest, ThermalAffectsForceWithController) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // slot order: slew(0), integral(1), temperature(2) ASSERT_EQ(model->actuator_actnum[0], 3); @@ -2079,11 +1993,11 @@ TEST_F(DCMotorTest, ThermalAffectsForceWithController) { double K = 0.05, R = 2.0, alpha = 0.004; double dT = 50; - data->act[adr] = 1.0; // slew state = ctrl: no rate-limiting applied - data->act[adr + 1] = 0.0; // integral state x_I = 0 - data->act[temp_adr] = dT; // temperature rise above ambient - data->ctrl[0] = 1.0; // position setpoint = 1.0, qpos = 0, error = 1.0 - mj_forward(model, data); + data->act[adr] = 1.0; // slew state = ctrl: no rate-limiting applied + data->act[adr + 1] = 0.0; // integral state x_I = 0 + data->act[temp_adr] = dT; // temperature rise above ambient + data->ctrl[0] = 1.0; // position setpoint = 1.0, qpos = 0, error = 1.0 + mj_forward(model.get(), data.get()); // u_eff = ctrl = 1.0 (no slew applied since act[slew] == ctrl) // V = kp*(u_eff - length) + ki*x_I - kd*omega = 1.0*1.0 + 1.0*0.0 - 0*0 = 1.0 @@ -2091,9 +2005,6 @@ TEST_F(DCMotorTest, ThermalAffectsForceWithController) { // stateless (no te): force = K/R(T) * V = 0.05/2.4 * 1.0 double R_hot = R * (1 + alpha * dT); EXPECT_NEAR(data->actuator_force[0], K / R_hot * 1.0, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, StatelessPositionMode) { @@ -2113,13 +2024,13 @@ TEST_F(DCMotorTest, StatelessPositionMode) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // Position target 5.0, current pos 0.0, current vel 0.0 data->ctrl[0] = 5.0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // V = Kp * (u - theta) = 2.0 * 5.0 = 10.0 // force = K / R * V + bias = (0.05 / 2.0) * 10.0 + 0 = 0.25 @@ -2127,14 +2038,11 @@ TEST_F(DCMotorTest, StatelessPositionMode) { // Velocity penalty data->qvel[0] = 2.0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // V = 10.0 - Kd * omega = 10.0 - (0.5 * 2.0) = 9.0 // bias = - K^2 / R * omega = -0.0025 / 2.0 * 2.0 = -0.0025 // force = K / R * V + bias = 0.225 - 0.0025 = 0.2225 EXPECT_NEAR(data->actuator_force[0], 0.2225, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, StatelessVelocityMode) { @@ -2154,22 +2062,19 @@ TEST_F(DCMotorTest, StatelessVelocityMode) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // Velocity target 4.0, current vel 1.0 data->ctrl[0] = 4.0; data->qvel[0] = 1.0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // V = Kp * (u - omega) = 3.0 * (4.0 - 1.0) = 9.0 // bias = - K^2 / R * omega = -0.0025 / 2.0 * 1.0 = -0.00125 // force = K / R * V + bias = (0.05 / 2.0) * 9.0 - 0.00125 = 0.22375 EXPECT_NEAR(data->actuator_force[0], 0.22375, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, StatefulPositionMode) { @@ -2189,9 +2094,9 @@ TEST_F(DCMotorTest, StatefulPositionMode) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // Controller states: 1 for slew, 1 for ki -> actnum = 2 ASSERT_EQ(model->actuator_actnum[0], 2); @@ -2201,28 +2106,25 @@ TEST_F(DCMotorTest, StatefulPositionMode) { double u_prev = 1.0; double x_I = 2.0; data->act[adr] = u_prev; - data->act[adr+1] = x_I; + data->act[adr + 1] = x_I; // target 5.0 position, current 0.0 data->ctrl[0] = 5.0; data->qvel[0] = 0.5; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // slew bounding: s = 10.0, dt = 0.001. max_change = 0.01 // Target = 5.0. It is upper bounded by u_prev + 0.01 = 1.01 EXPECT_NEAR(data->act_dot[adr], 10.0, MjTol(1e-12, 1e-5)); // PI error: error = u_eff - length = 1.01 - 0.0 = 1.01 - EXPECT_NEAR(data->act_dot[adr+1], 1.01, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(data->act_dot[adr + 1], 1.01, MjTol(1e-12, 1e-5)); // V = Kp(u_eff - length) + Ki * x_I - Kd * omega // V = 2.0 * 1.01 + 0.5 * 2.0 - 0.1 * 0.5 = 2.97 // bias = - K^2/R * omega = -(0.05)^2 / 2.0 * 0.5 = -0.000625 // force = K/R * V + bias = 0.025 * 2.97 - 0.000625 = 0.073625 EXPECT_NEAR(data->actuator_force[0], 0.073625, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) { @@ -2242,9 +2144,9 @@ TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // Controller states: slew (0), ki (1), current (2). actnum = 3 ASSERT_EQ(model->actuator_actnum[0], 3); @@ -2254,19 +2156,19 @@ TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) { double x_I = 2.0; double current = 0.5; data->act[adr] = u_prev; - data->act[adr+1] = x_I; - data->act[adr+2] = current; + data->act[adr + 1] = x_I; + data->act[adr + 2] = current; // Target 5.0 position, velocity 0.5 data->ctrl[0] = 5.0; data->qvel[0] = 0.5; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // Slew bounding: max_change = 0.01, u_eff = 1.01 EXPECT_NEAR(data->act_dot[adr], 10.0, MjTol(1e-12, 1e-5)); // PI error: error = u_eff - length = 1.01 - EXPECT_NEAR(data->act_dot[adr+1], 1.01, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(data->act_dot[adr + 1], 1.01, MjTol(1e-12, 1e-5)); // Voltage computation: // V = Kp(u_eff - length) + Ki * x_I - Kd * omega @@ -2277,17 +2179,14 @@ TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) { // di/dt = (V/R - K/R * omega - i) / t_e // di/dt = (2.97/2.0 - 0.05/2.0 * 0.5 - 0.5) / 0.5 // di/dt = (1.485 - 0.0125 - 0.5) / 0.5 = 0.9725 / 0.5 = 1.945 - EXPECT_NEAR(data->act_dot[adr+2], 1.945, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(data->act_dot[adr + 2], 1.945, MjTol(1e-12, 1e-5)); // Force is K * next_activation (actearly is always on for DC motors) // Inline mj_nextActivation for te = 0.5 mjtNum te = 0.5; mjtNum h = model->opt.timestep; - mjtNum next_i = 0.5 + data->act_dot[adr+2] * te * (1 - mju_exp(-h / te)); + mjtNum next_i = 0.5 + data->act_dot[adr + 2] * te * (1 - mju_exp(-h / te)); EXPECT_NEAR(data->actuator_force[0], 0.05 * next_i, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, StatefulVelocityMode) { @@ -2307,21 +2206,21 @@ TEST_F(DCMotorTest, StatefulVelocityMode) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // Controller states: 1 for ki (no slew) ASSERT_EQ(model->actuator_actnum[0], 1); int adr = model->actuator_actadr[0]; - double x_I = 2.0; // Exactly at Imax limit (Imax = 2.0) + double x_I = 2.0; // Exactly at Imax limit (Imax = 2.0) data->act[adr] = x_I; // target vel 4.0, current vel 1.0 data->ctrl[0] = 4.0; data->qvel[0] = 1.0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // integrate command directly: error = target = 4.0 // since x_I == Imax (2.0) and error (4.0) > 0, act_dot should be clamped to 0 @@ -2335,14 +2234,11 @@ TEST_F(DCMotorTest, StatefulVelocityMode) { // repeat with non-zero joint position data->qpos[0] = 1.5; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // V = 3.0 * (4.0 - 1.0) + 1.0 * (2.0 - 1.5) = 9.0 + 0.5 = 9.5 // force = K/R * V + bias = 0.025 * 9.5 - 0.00125 = 0.2375 - 0.00125 = 0.23625 EXPECT_NEAR(data->actuator_force[0], 0.23625, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, CurrentPlusThermal) { @@ -2362,9 +2258,9 @@ TEST_F(DCMotorTest, CurrentPlusThermal) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); ASSERT_EQ(model->actuator_actnum[0], 2); int adr = model->actuator_actadr[0]; @@ -2376,14 +2272,15 @@ TEST_F(DCMotorTest, CurrentPlusThermal) { double current = 3.0; double dT = 10.0; data->act[adr] = dT; - data->act[adr+1] = current; + data->act[adr + 1] = current; data->ctrl[0] = V; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // Force uses next_activation (actearly is always on for DC motors) // Inline mj_nextActivation for te = 0.01 / R = 0.005 mjtNum h = model->opt.timestep; - mjtNum next_i = current + data->act_dot[adr+1] * te * (1 - mju_exp(-h / te)); + mjtNum next_i = + current + data->act_dot[adr + 1] * te * (1 - mju_exp(-h / te)); EXPECT_NEAR(data->actuator_force[0], K * next_i, MjTol(1e-12, 1e-5)); double R_hot = R * (1 + 0.004 * dT); @@ -2391,11 +2288,8 @@ TEST_F(DCMotorTest, CurrentPlusThermal) { EXPECT_NEAR(data->act_dot[adr], T_dot, MjTol(1e-10, 1e-4)); double omega = data->qvel[0]; - double i_dot = (V/R_hot - K/R_hot*omega - current) / te; - EXPECT_NEAR(data->act_dot[adr+1], i_dot, MjTol(1e-10, 1e-3)); - - mj_deleteData(data); - mj_deleteModel(model); + double i_dot = (V / R_hot - K / R_hot * omega - current) / te; + EXPECT_NEAR(data->act_dot[adr + 1], i_dot, MjTol(1e-10, 1e-3)); } TEST_F(DCMotorTest, CurrentRateLimit) { @@ -2416,36 +2310,32 @@ TEST_F(DCMotorTest, CurrentRateLimit) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); ASSERT_EQ(model->actuator_actnum[0], 1); int adr = model->actuator_actadr[0]; double V = 12.0; - double dimax = 100.0; // A/s rate limit + double dimax = 100.0; // A/s rate limit // unclamped: i_dot = (V/R - 0 - 0) / te = 6 / 0.005 = 1200 A/s >> dimax - data->act[adr] = 0; // current = 0 + data->act[adr] = 0; // current = 0 data->ctrl[0] = V; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // i_dot should be clipped to +dimax EXPECT_NEAR(data->act_dot[adr], dimax, MjTol(1e-12, 1e-5)); // reverse: large negative drive data->ctrl[0] = -V; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // i_dot should be clipped to -dimax EXPECT_NEAR(data->act_dot[adr], -dimax, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } - TEST_F(DCMotorTest, VoltageLimit) { // verifies that saturation:voltage clamps voltage static constexpr char xml[] = R"( @@ -2463,28 +2353,24 @@ TEST_F(DCMotorTest, VoltageLimit) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // Vmax = 10.0, ctrl = 20.0 // force = K/R * Vmax = 0.05 / 2.0 * 10.0 = 0.25 data->ctrl[0] = 20.0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_NEAR(data->actuator_force[0], 0.25, MjTol(1e-12, 1e-5)); // negative drive data->ctrl[0] = -20.0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_NEAR(data->actuator_force[0], -0.25, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } - TEST_F(DCMotorTest, IntegralClamp) { // verifies that controller Imax clamps integral state static constexpr char xml[] = R"( @@ -2503,9 +2389,9 @@ TEST_F(DCMotorTest, IntegralClamp) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // Imax = 5.0 ASSERT_EQ(model->actuator_actnum[0], 1); // only ki is stateful @@ -2518,14 +2404,14 @@ TEST_F(DCMotorTest, IntegralClamp) { data->ctrl[0] = 1.0; // target data->qpos[0] = 0.0; // length = 0 - mj_forward(model, data); + mj_forward(model.get(), data.get()); // act_dot should be clamped to 0 because act >= Imax and error > 0 EXPECT_NEAR(data->act_dot[adr], 0.0, MjTol(1e-12, 1e-5)); // set target to generate negative error data->ctrl[0] = -1.0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // act_dot should be negative (not clamped) EXPECT_NEAR(data->act_dot[adr], -1.0, MjTol(1e-12, 1e-5)); @@ -2536,13 +2422,10 @@ TEST_F(DCMotorTest, IntegralClamp) { // set target to generate negative error data->ctrl[0] = -1.0; data->qpos[0] = 0.0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // act_dot should be clamped to 0 because act <= -Imax and error < 0 EXPECT_NEAR(data->act_dot[adr], 0.0, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, LuGreExactIntegration) { @@ -2562,9 +2445,9 @@ TEST_F(DCMotorTest, LuGreExactIntegration) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); ASSERT_EQ(model->actuator_actnum[0], 1); int adr = model->actuator_actadr[0]; @@ -2578,17 +2461,14 @@ TEST_F(DCMotorTest, LuGreExactIntegration) { data->qvel[0] = v; double ratio = v / v_S; - double g_v = F_C + (F_S - F_C) * mju_exp(-ratio*ratio); + double g_v = F_C + (F_S - F_C) * mju_exp(-ratio * ratio); double a = -sigma0 * std::abs(v) / g_v; double exp_ah = mju_exp(a * h); double int_h = (exp_ah - 1) / a; double z_new = exp_ah * z0 + int_h * v; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->act[adr], z_new, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, LuGreSteadyState) { @@ -2608,9 +2488,9 @@ TEST_F(DCMotorTest, LuGreSteadyState) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); int adr = model->actuator_actadr[0]; @@ -2622,11 +2502,11 @@ TEST_F(DCMotorTest, LuGreSteadyState) { data->qvel[0] = v; data->ctrl[0] = 0; for (int i = 0; i < 10000; i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); } double ratio = v / v_S; - double g_v = F_C + (F_S - F_C) * mju_exp(-ratio*ratio); + double g_v = F_C + (F_S - F_C) * mju_exp(-ratio * ratio); double z_ss = g_v / sigma0; EXPECT_NEAR(data->act[adr], z_ss, 1e-4); @@ -2634,9 +2514,6 @@ TEST_F(DCMotorTest, LuGreSteadyState) { double back_emf = K * K / R * data->qvel[0]; double lugre_ss = g_v; EXPECT_NEAR(data->actuator_force[0], -back_emf - lugre_ss, 1e-3); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(DCMotorTest, LuGreBristleSpring) { @@ -2655,9 +2532,9 @@ TEST_F(DCMotorTest, LuGreBristleSpring) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); int adr = model->actuator_actadr[0]; double sigma0 = 100; @@ -2665,12 +2542,9 @@ TEST_F(DCMotorTest, LuGreBristleSpring) { data->act[adr] = X; data->ctrl[0] = 0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_NEAR(data->actuator_force[0], -sigma0 * X, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } // ----------------------- filterexact actuators ------------------------------- @@ -2694,48 +2568,45 @@ TEST_F(FilterExactTest, ApproximatesContinuousTime) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); const mjtNum kSimulationTime = 1.0; // compute act with a small timestep to approximate continuous integration model->opt.timestep = 0.001; - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); data->ctrl[0] = 1.0; data->act[0] = 0.0; for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); } mjtNum continuous_act = data->act[0]; // compute again with a larger timestep, introducing integration error model->opt.timestep = 0.01; - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); data->ctrl[0] = 1.0; data->act[0] = 0.0; for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); } mjtNum discrete_act = data->act[0]; // compute a third time with exact integration model->actuator_dyntype[0] = mjDYN_FILTEREXACT; - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); data->ctrl[0] = 1.0; data->act[0] = 0.0; for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); } mjtNum exactfilter_act = data->act[0]; // expect exact integration to be closer to the small-timestep result EXPECT_THAT(std::abs(continuous_act - discrete_act), - Gt(5*std::abs(continuous_act - exactfilter_act))) + Gt(5 * std::abs(continuous_act - exactfilter_act))) << "Using filterexact should make the error at least 5 times smaller"; - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(FilterExactTest, TimestepIndependent) { @@ -2755,37 +2626,34 @@ TEST_F(FilterExactTest, TimestepIndependent) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); const mjtNum kSimulationTime = 1.0; // first, compute act based on a small timestep and exact integration model->opt.timestep = 0.01; - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); data->ctrl[0] = 1.0; data->act[0] = 0.0; for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); } mjtNum small_timestep_act = data->act[0]; // now change the timestep to a much larger timestep model->opt.timestep = 0.1; - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); data->ctrl[0] = 1.0; data->act[0] = 0.0; for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); } mjtNum large_timestep_act = data->act[0]; EXPECT_NEAR(small_timestep_act, large_timestep_act, MjTol(1e-14, 1e-6)) << "exact integration should be independent of timestep to machine " "precision."; - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(FilterExactTest, ActEqualsCtrlWhenTauIsZero) { @@ -2805,16 +2673,13 @@ TEST_F(FilterExactTest, ActEqualsCtrlWhenTauIsZero) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); data->ctrl[0] = 0.5; data->act[0] = 0.0; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_EQ(data->act[0], data->ctrl[0]); - - mj_deleteData(data); - mj_deleteModel(model); } // ----------------------- actearly actuator attribute ------------------------- @@ -2835,8 +2700,8 @@ TEST_F(ActEarlyTest, RemovesOneStepDelay) { // actuators are ordered in pairs with actearly=true and actearly=false for (int i = 0; i < model->na / 2; i++) { - EXPECT_TRUE(model->actuator_actearly[2*i]); - EXPECT_FALSE(model->actuator_actearly[2*i + 1]); + EXPECT_TRUE(model->actuator_actearly[2 * i]); + EXPECT_FALSE(model->actuator_actearly[2 * i + 1]); } mjData* data = mj_makeData(model); @@ -2886,9 +2751,8 @@ TEST_F(ActEarlyTest, DoesntChangeStateInMjForward) { mj_forward(model, data); for (int i = 0; i < model->na; i++) { - EXPECT_EQ(data->act[i], 0) - << "act should not change with mj_forward." - << mj_id2name(model, mjOBJ_ACTUATOR, i); + EXPECT_EQ(data->act[i], 0) << "act should not change with mj_forward." + << mj_id2name(model, mjOBJ_ACTUATOR, i); } mj_deleteData(data); @@ -2912,26 +2776,23 @@ TEST_F(ActuatorTest, DisableActuator) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); data->ctrl[0] = 1.0; data->ctrl[1] = 1.0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_EQ(data->qfrc_actuator[0], 3.0); model->opt.disableactuator = 1 << 0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_EQ(data->qfrc_actuator[0], 1.0); model->opt.disableactuator = 1 << 1; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_EQ(data->qfrc_actuator[0], 2.0); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(ActuatorTest, DisableActuatorOutOfRange) { @@ -2952,25 +2813,22 @@ TEST_F(ActuatorTest, DisableActuatorOutOfRange) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); data->ctrl[0] = 1.0; data->ctrl[1] = 1.0; data->ctrl[2] = 1.0; // all actuators active - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_EQ(data->qfrc_actuator[0], 35.0); // set all bits of disableactuator, only group 1 is disabled model->opt.disableactuator = ~0; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_EQ(data->qfrc_actuator[0], 30.0); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(ActuatorTest, TendonActuatorForceRange) { @@ -3043,9 +2901,9 @@ TEST_F(ForwardTest, ActuatorDelay) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // delay = 0.02 seconds, timestep = 0.01, so ndelay = ceil(0.02/0.01) = 2 EXPECT_EQ(model->actuator_history[0], 2); @@ -3054,22 +2912,19 @@ TEST_F(ForwardTest, ActuatorDelay) { data->ctrl[0] = 10.0; // step once: the new ctrl is appended but won't be read for 2 timesteps - mj_step(model, data); + mj_step(model.get(), data.get()); // actuator_force should still be 0 (delayed value from buffer init) EXPECT_NEAR(data->actuator_force[0], 0.0, 1e-10); // step again - mj_step(model, data); + mj_step(model.get(), data.get()); // still reading old values EXPECT_NEAR(data->actuator_force[0], 0.0, 1e-10); // step a third time - now the delayed ctrl should arrive - mj_step(model, data); + mj_step(model.get(), data.get()); // actuator_force should now be 10.0 EXPECT_NEAR(data->actuator_force[0], 10.0, 1e-10); - - mj_deleteData(data); - mj_deleteModel(model); } // Test actuator delay with linear interpolation (interp=1) @@ -3090,9 +2945,9 @@ TEST_F(ForwardTest, ActuatorDelayLinearInterp) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // delay = 0.015 seconds = 1.5*timestep, nsample=3, interp=1 (linear) EXPECT_EQ(model->actuator_history[0], 3); @@ -3101,32 +2956,29 @@ TEST_F(ForwardTest, ActuatorDelayLinearInterp) { // Set increasing ctrl values // Buffer has samples at times: -0.02, -0.01, 0 with values 0, 0, 0 - // After step 0 at time=0.01: buffer has times -0.01, 0, 0.01 with values 0, 0, ctrl[0] - // Read at time 0.01 - 0.015 = -0.005: interpolate between t=-0.01 and t=0 - // Since both values are 0, expected actuator_force = 0 + // After step 0 at time=0.01: buffer has times -0.01, 0, 0.01 with values 0, + // 0, ctrl[0] Read at time 0.01 - 0.015 = -0.005: interpolate between t=-0.01 + // and t=0 Since both values are 0, expected actuator_force = 0 data->ctrl[0] = 10.0; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->actuator_force[0], 0.0, MjTol(1e-10, 5e-6)) << "step 0"; - // After step 1 at time=0.02: buffer has times 0, 0.01, 0.02 with values 0, 10, 20 - // Read at time 0.02 - 0.015 = 0.005: interpolate between t=0 (val=0) and t=0.01 (val=10) - // Expected: 0 * 0.5 + 10 * 0.5 = 5 + // After step 1 at time=0.02: buffer has times 0, 0.01, 0.02 with values 0, + // 10, 20 Read at time 0.02 - 0.015 = 0.005: interpolate between t=0 (val=0) + // and t=0.01 (val=10) Expected: 0 * 0.5 + 10 * 0.5 = 5 data->ctrl[0] = 20.0; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->actuator_force[0], 5.0, MjTol(1e-10, 5e-6)) << "step 1"; - // After step 2 at time=0.03: buffer has times 0.01, 0.02, 0.03 with values 10, 20, 30 - // Read at 0.03 - 0.015 = 0.015: interpolate between t=0.01 (val=10) and t=0.02 (val=20) - // Expected: 10 * 0.5 + 20 * 0.5 = 15 + // After step 2 at time=0.03: buffer has times 0.01, 0.02, 0.03 with values + // 10, 20, 30 Read at 0.03 - 0.015 = 0.015: interpolate between t=0.01 + // (val=10) and t=0.02 (val=20) Expected: 10 * 0.5 + 20 * 0.5 = 15 data->ctrl[0] = 30.0; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->actuator_force[0], 15.0, MjTol(1e-10, 5e-6)) << "step 2"; - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(ForwardTest, FlexTrilinearInstability) { @@ -3153,10 +3005,10 @@ TEST_F(ForwardTest, FlexTrilinearInstability) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); // flex stiffness sign checks // verify correct sign of flex stiffness derivatives before simulation @@ -3169,10 +3021,10 @@ TEST_F(ForwardTest, FlexTrilinearInstability) { mjtNum vnorm = mju_norm(v.data(), nv); for (int i = 0; i < nv; i++) v[i] /= vnorm; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // compute M*v and stiffness contributions - mj_mulM(model, data, Mv.data(), v.data()); + mj_mulM(model.get(), data.get(), Mv.data(), v.data()); // note: we use mjd_flexInterp_mulK here (unscaled by h^2) to check raw // stiffness logic similar to what we expect in the solver now @@ -3183,7 +3035,8 @@ TEST_F(ForwardTest, FlexTrilinearInstability) { // using mulKD for legacy check consistency, but we know it applies h^2+h*d // scaling; actually, let's stick to the high-level property checks from // FlexStiffnessSign which used mulKD - mjd_flexInterp_mul(model, data, flex_Kv.data(), v.data(), h * h, h, NULL); + mjd_flexInterp_mul(model.get(), data.get(), flex_Kv.data(), v.data(), h * h, + h, NULL); // compute v^T*M*v and v^T*scale*K*v mjtNum vMv = mju_dot(v.data(), Mv.data(), nv); @@ -3199,7 +3052,7 @@ TEST_F(ForwardTest, FlexTrilinearInstability) { // stability simulation // run for steps to catch instability for (int i = 0; i < 2000; ++i) { - mj_step(model, data); + mj_step(model.get(), data.get()); for (int j = 0; j < model->nq; ++j) { if (mju_abs(data->qpos[j]) > 1000.0) { @@ -3209,9 +3062,6 @@ TEST_F(ForwardTest, FlexTrilinearInstability) { } } } - - mj_deleteData(data); - mj_deleteModel(model); } // Verify that flex damping does not affect rigid body motion @@ -3229,9 +3079,9 @@ TEST_F(ForwardTest, FlexDampingRigidMotion) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // Set initial rigid rotation velocity about Z axis // Center of mass is roughly at 0 0 0 because pos="0 0 0" and symmetric grid. @@ -3247,15 +3097,15 @@ TEST_F(ForwardTest, FlexDampingRigidMotion) { mju_cross(vel, w, r); } - mj_forward(model, data); + mj_forward(model.get(), data.get()); mjtNum initial_energy = data->energy[0] + data->energy[1]; // Run a few steps for (int i = 0; i < 10; ++i) { - mj_step(model, data); + mj_step(model.get(), data.get()); } - mj_forward(model, data); + mj_forward(model.get(), data.get()); mjtNum final_energy = data->energy[0] + data->energy[1]; // Expect energy conservation. @@ -3263,9 +3113,6 @@ TEST_F(ForwardTest, FlexDampingRigidMotion) { EXPECT_NEAR(final_energy, initial_energy, 1e-6 * initial_energy) << "Energy decayed significantly (" << initial_energy << " -> " << final_energy << ")"; - - mj_deleteData(data); - mj_deleteModel(model); } // verify that implicit integrator respects parent-flex coupling @@ -3288,30 +3135,30 @@ TEST_F(ForwardTest, FlexParentCoupling) { )"; char error[1024]; - mjModel* model = LoadModelFromString(kXml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // set state: parent moving, flex deformed // this ensures both H_fp (coupling) and qacc_parent are non-trivial // Run with Euler (timestep 1e-6) model->opt.timestep = 1e-6; model->opt.integrator = mjINT_EULER; - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); data->qvel[0] = 1.0; data->qpos[7] += 0.01; - data->qfrc_applied[0] = 10000.0; // Apply large force to parent - mj_step(model, data); // Step integrates + data->qfrc_applied[0] = 10000.0; // Apply large force to parent + mj_step(model.get(), data.get()); // Step integrates std::vector qvel_euler(model->nv); mju_copy(qvel_euler.data(), data->qvel, model->nv); // Run with Implicit (timestep 1e-6) model->opt.integrator = mjINT_IMPLICIT; - mj_resetData(model, data); + mj_resetData(model.get(), data.get()); data->qvel[0] = 1.0; data->qpos[7] += 0.01; data->qfrc_applied[0] = 10000.0; - mj_step(model, data); // Step integrates + mj_step(model.get(), data.get()); // Step integrates std::vector qvel_implicit(model->nv); mju_copy(qvel_implicit.data(), data->qvel, model->nv); @@ -3324,12 +3171,8 @@ TEST_F(ForwardTest, FlexParentCoupling) { EXPECT_LT(max_diff, MjTol(2e-5, 1.5e-2)) << "Implicit integrator should match Euler at small timestep"; - - mj_deleteData(data); - mj_deleteModel(model); } - TEST_F(ForwardTest, TrilinearPinnedParentWithFreejoint) { static constexpr char xml[] = R"( @@ -3352,11 +3195,11 @@ TEST_F(ForwardTest, TrilinearPinnedParentWithFreejoint) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); + MjDataPtr d = MakeData(m); - int parent_id = mj_name2id(m, mjOBJ_BODY, "parent"); + int parent_id = mj_name2id(m.get(), mjOBJ_BODY, "parent"); ASSERT_GT(parent_id, 0); EXPECT_EQ(m->nflexnode, 8); @@ -3365,11 +3208,11 @@ TEST_F(ForwardTest, TrilinearPinnedParentWithFreejoint) { int freejoint_body = m->body_parentid[parent_id]; EXPECT_EQ(m->body_dofnum[freejoint_body], 6) << "freejoint body has 6 DOFs"; - mj_resetData(m, d); - mj_forward(m, d); + mj_resetData(m.get(), d.get()); + mj_forward(m.get(), d.get()); for (int i = 0; i < 500; i++) { - mj_step(m, d); + mj_step(m.get(), d.get()); ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation became unstable at step " << i; @@ -3387,9 +3230,6 @@ TEST_F(ForwardTest, TrilinearPinnedParentWithFreejoint) { << "]=" << d->qvel[j]; } } - - mj_deleteData(d); - mj_deleteModel(m); } // -------------------- actuator damping and armature -------------------------- @@ -3432,26 +3272,21 @@ TEST_F(ActuatorDampingTest, SingleActuatorJointDamping) { )"; char error[1024]; - mjModel* m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); - ASSERT_THAT(m1, NotNull()) << error; - mjData* d1 = mj_makeData(m1); + MjModelPtr m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); + ASSERT_THAT(m1.get(), NotNull()) << error; + MjDataPtr d1 = MakeData(m1); - mjModel* m2 = LoadModelFromString(xml_joint, error, sizeof(error)); - ASSERT_THAT(m2, NotNull()) << error; - mjData* d2 = mj_makeData(m2); + MjModelPtr m2 = LoadModelFromString(xml_joint, error, sizeof(error)); + ASSERT_THAT(m2.get(), NotNull()) << error; + MjDataPtr d2 = MakeData(m2); - mj_resetDataKeyframe(m1, d1, 0); - mj_forward(m1, d1); + mj_resetDataKeyframe(m1.get(), d1.get(), 0); + mj_forward(m1.get(), d1.get()); - mj_resetDataKeyframe(m2, d2, 0); - mj_forward(m2, d2); + mj_resetDataKeyframe(m2.get(), d2.get(), 0); + mj_forward(m2.get(), d2.get()); EXPECT_EQ(d1->qfrc_passive[0], d2->qfrc_passive[0]); - - mj_deleteData(d1); - mj_deleteModel(m1); - mj_deleteData(d2); - mj_deleteModel(m2); } TEST_F(ActuatorDampingTest, SingleActuatorTendonDamping) { @@ -3498,26 +3333,21 @@ TEST_F(ActuatorDampingTest, SingleActuatorTendonDamping) { )"; char error[1024]; - mjModel* m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); - ASSERT_THAT(m1, NotNull()) << error; - mjData* d1 = mj_makeData(m1); + MjModelPtr m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); + ASSERT_THAT(m1.get(), NotNull()) << error; + MjDataPtr d1 = MakeData(m1); - mjModel* m2 = LoadModelFromString(xml_tendon, error, sizeof(error)); - ASSERT_THAT(m2, NotNull()) << error; - mjData* d2 = mj_makeData(m2); + MjModelPtr m2 = LoadModelFromString(xml_tendon, error, sizeof(error)); + ASSERT_THAT(m2.get(), NotNull()) << error; + MjDataPtr d2 = MakeData(m2); - mj_resetDataKeyframe(m1, d1, 0); - mj_forward(m1, d1); + mj_resetDataKeyframe(m1.get(), d1.get(), 0); + mj_forward(m1.get(), d1.get()); - mj_resetDataKeyframe(m2, d2, 0); - mj_forward(m2, d2); + mj_resetDataKeyframe(m2.get(), d2.get(), 0); + mj_forward(m2.get(), d2.get()); EXPECT_EQ(d1->qfrc_passive[0], d2->qfrc_passive[0]); - - mj_deleteData(d1); - mj_deleteModel(m1); - mj_deleteData(d2); - mj_deleteModel(m2); } TEST_F(ActuatorDampingTest, SingleActuatorArmature) { @@ -3556,26 +3386,21 @@ TEST_F(ActuatorDampingTest, SingleActuatorArmature) { )"; char error[1024]; - mjModel* m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); - ASSERT_THAT(m1, NotNull()) << error; - mjData* d1 = mj_makeData(m1); + MjModelPtr m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); + ASSERT_THAT(m1.get(), NotNull()) << error; + MjDataPtr d1 = MakeData(m1); - mjModel* m2 = LoadModelFromString(xml_joint, error, sizeof(error)); - ASSERT_THAT(m2, NotNull()) << error; - mjData* d2 = mj_makeData(m2); + MjModelPtr m2 = LoadModelFromString(xml_joint, error, sizeof(error)); + ASSERT_THAT(m2.get(), NotNull()) << error; + MjDataPtr d2 = MakeData(m2); - mj_resetDataKeyframe(m1, d1, 0); - mj_forward(m1, d1); + mj_resetDataKeyframe(m1.get(), d1.get(), 0); + mj_forward(m1.get(), d1.get()); - mj_resetDataKeyframe(m2, d2, 0); - mj_forward(m2, d2); + mj_resetDataKeyframe(m2.get(), d2.get(), 0); + mj_forward(m2.get(), d2.get()); EXPECT_EQ(d1->qacc[0], d2->qacc[0]); - - mj_deleteData(d1); - mj_deleteModel(m1); - mj_deleteData(d2); - mj_deleteModel(m2); } TEST_F(ActuatorDampingTest, MultipleActuatorsAccumulate) { @@ -3615,26 +3440,21 @@ TEST_F(ActuatorDampingTest, MultipleActuatorsAccumulate) { )"; char error[1024]; - mjModel* m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); - ASSERT_THAT(m1, NotNull()) << error; - mjData* d1 = mj_makeData(m1); + MjModelPtr m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); + ASSERT_THAT(m1.get(), NotNull()) << error; + MjDataPtr d1 = MakeData(m1); - mjModel* m2 = LoadModelFromString(xml_joint, error, sizeof(error)); - ASSERT_THAT(m2, NotNull()) << error; - mjData* d2 = mj_makeData(m2); + MjModelPtr m2 = LoadModelFromString(xml_joint, error, sizeof(error)); + ASSERT_THAT(m2.get(), NotNull()) << error; + MjDataPtr d2 = MakeData(m2); - mj_resetDataKeyframe(m1, d1, 0); - mj_forward(m1, d1); + mj_resetDataKeyframe(m1.get(), d1.get(), 0); + mj_forward(m1.get(), d1.get()); - mj_resetDataKeyframe(m2, d2, 0); - mj_forward(m2, d2); + mj_resetDataKeyframe(m2.get(), d2.get(), 0); + mj_forward(m2.get(), d2.get()); EXPECT_EQ(d1->qfrc_passive[0], d2->qfrc_passive[0]); - - mj_deleteData(d1); - mj_deleteModel(m1); - mj_deleteData(d2); - mj_deleteModel(m2); } TEST_F(ActuatorDampingTest, DampingSimulationEquivalence) { @@ -3672,28 +3492,23 @@ TEST_F(ActuatorDampingTest, DampingSimulationEquivalence) { )"; char error[1024]; - mjModel* m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); - ASSERT_THAT(m1, NotNull()) << error; - mjData* d1 = mj_makeData(m1); + MjModelPtr m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); + ASSERT_THAT(m1.get(), NotNull()) << error; + MjDataPtr d1 = MakeData(m1); - mjModel* m2 = LoadModelFromString(xml_joint, error, sizeof(error)); - ASSERT_THAT(m2, NotNull()) << error; - mjData* d2 = mj_makeData(m2); + MjModelPtr m2 = LoadModelFromString(xml_joint, error, sizeof(error)); + ASSERT_THAT(m2.get(), NotNull()) << error; + MjDataPtr d2 = MakeData(m2); - mj_resetDataKeyframe(m1, d1, 0); - mj_resetDataKeyframe(m2, d2, 0); + mj_resetDataKeyframe(m1.get(), d1.get(), 0); + mj_resetDataKeyframe(m2.get(), d2.get(), 0); for (int i = 0; i < 100; i++) { - mj_step(m1, d1); - mj_step(m2, d2); + mj_step(m1.get(), d1.get()); + mj_step(m2.get(), d2.get()); } EXPECT_MJTNUM_EQ(d1->qpos[0], d2->qpos[0]); EXPECT_MJTNUM_EQ(d1->qvel[0], d2->qvel[0]); - - mj_deleteData(d1); - mj_deleteModel(m1); - mj_deleteData(d2); - mj_deleteModel(m2); } TEST_F(ActuatorDampingTest, ArmatureSimulationEquivalence) { @@ -3731,28 +3546,23 @@ TEST_F(ActuatorDampingTest, ArmatureSimulationEquivalence) { )"; char error[1024]; - mjModel* m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); - ASSERT_THAT(m1, NotNull()) << error; - mjData* d1 = mj_makeData(m1); + MjModelPtr m1 = LoadModelFromString(xml_actuator, error, sizeof(error)); + ASSERT_THAT(m1.get(), NotNull()) << error; + MjDataPtr d1 = MakeData(m1); - mjModel* m2 = LoadModelFromString(xml_joint, error, sizeof(error)); - ASSERT_THAT(m2, NotNull()) << error; - mjData* d2 = mj_makeData(m2); + MjModelPtr m2 = LoadModelFromString(xml_joint, error, sizeof(error)); + ASSERT_THAT(m2.get(), NotNull()) << error; + MjDataPtr d2 = MakeData(m2); - mj_resetDataKeyframe(m1, d1, 0); - mj_resetDataKeyframe(m2, d2, 0); + mj_resetDataKeyframe(m1.get(), d1.get(), 0); + mj_resetDataKeyframe(m2.get(), d2.get(), 0); for (int i = 0; i < 100; i++) { - mj_step(m1, d1); - mj_step(m2, d2); + mj_step(m1.get(), d1.get()); + mj_step(m2.get(), d2.get()); } EXPECT_MJTNUM_EQ(d1->qpos[0], d2->qpos[0]); EXPECT_MJTNUM_EQ(d1->qvel[0], d2->qvel[0]); - - mj_deleteData(d1); - mj_deleteModel(m1); - mj_deleteData(d2); - mj_deleteModel(m2); } TEST_F(ActuatorDampingTest, UtilityFunctionValues) { @@ -3770,17 +3580,14 @@ TEST_F(ActuatorDampingTest, UtilityFunctionValues) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; mjtNum poly[mjNPOLY] = {0}; - EXPECT_EQ(mj_actuatorDamping(m, mjOBJ_JOINT, 0, poly), 175); - EXPECT_EQ(mj_actuatorArmature(m, mjOBJ_JOINT, 0), 75); - - mj_deleteModel(m); + EXPECT_EQ(mj_actuatorDamping(m.get(), mjOBJ_JOINT, 0, poly), 175); + EXPECT_EQ(mj_actuatorArmature(m.get(), mjOBJ_JOINT, 0), 75); } - TEST_F(ActuatorDampingTest, NonlinearDamping) { static constexpr char xml[] = R"( @@ -3796,20 +3603,18 @@ TEST_F(ActuatorDampingTest, NonlinearDamping) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; // linear damping: 2 * gear^2 = 18 mjtNum poly0[mjNPOLY] = {0}; - EXPECT_EQ(mj_actuatorDamping(m, mjOBJ_JOINT, 0, poly0), 18); + EXPECT_EQ(mj_actuatorDamping(m.get(), mjOBJ_JOINT, 0, poly0), 18); // poly coefficients scaled by gear^2 mjtNum poly[mjNPOLY] = {0}; - mj_actuatorDamping(m, mjOBJ_JOINT, 0, poly); + mj_actuatorDamping(m.get(), mjOBJ_JOINT, 0, poly); EXPECT_MJTNUM_EQ(poly[0], 0.5 * 9); // 4.5 EXPECT_MJTNUM_EQ(poly[1], 0.1 * 9); // 0.9 - - mj_deleteModel(m); } TEST_F(ActuatorDampingTest, DampingVsKvGearScaling) { @@ -3841,13 +3646,13 @@ TEST_F(ActuatorDampingTest, DampingVsKvGearScaling) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); // check forces at initial state - mj_resetDataKeyframe(m, d, 0); - mj_forward(m, d); + mj_resetDataKeyframe(m.get(), d.get(), 0); + mj_forward(m.get(), d.get()); // kv force arrives via qfrc_actuator, damping via qfrc_passive mjtNum frc_kv = d->qfrc_actuator[0]; @@ -3858,18 +3663,15 @@ TEST_F(ActuatorDampingTest, DampingVsKvGearScaling) { EXPECT_NEAR(frc_damp, -45, MjTol(1e-12, 1e-5)); // simulate and check trajectory equivalence - mj_resetDataKeyframe(m, d, 0); + mj_resetDataKeyframe(m.get(), d.get(), 0); for (int i = 0; i < 100; i++) { - mj_step(m, d); + mj_step(m.get(), d.get()); } EXPECT_NEAR(d->qpos[0], d->qpos[1], MjTol(1e-12, 1e-5)) << "position trajectory mismatch"; EXPECT_NEAR(d->qvel[0], d->qvel[1], MjTol(1e-12, 1e-5)) << "velocity trajectory mismatch"; - - mj_deleteData(d); - mj_deleteModel(m); } // flex sheet dropping on a plane should not gain energy from implicit bending @@ -3897,14 +3699,14 @@ TEST_F(ImplicitIntegratorTest, FlexContactEnergy) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; ASSERT_EQ(m->nflex, 1); - mjData* d = mj_makeData(m); + MjDataPtr d = MakeData(m); // compute initial energy - mj_forward(m, d); + mj_forward(m.get(), d.get()); mjtNum initial_energy = d->energy[0] + d->energy[1]; ASSERT_GT(initial_energy, 0); @@ -3913,7 +3715,7 @@ TEST_F(ImplicitIntegratorTest, FlexContactEnergy) { int max_energy_step = 0; int nsteps = 500; for (int i = 0; i < nsteps; i++) { - mj_step(m, d); + mj_step(m.get(), d.get()); mjtNum total_energy = d->energy[0] + d->energy[1]; if (total_energy > max_energy) { max_energy = total_energy; @@ -3928,9 +3730,6 @@ TEST_F(ImplicitIntegratorTest, FlexContactEnergy) { << energy_ratio << " (max at step " << max_energy_step << ")" << "\n initial_energy = " << initial_energy << "\n max_energy = " << max_energy; - - mj_deleteData(d); - mj_deleteModel(m); } // bending damping on a flat flex must dissipate energy with implicit integrator @@ -3953,12 +3752,12 @@ TEST_F(ImplicitIntegratorTest, BendingDampingDecaysEnergy) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; ASSERT_EQ(m->nflex, 1); ASSERT_GT(m->flex_damping[0], 0) << "flex_damping not set"; - mjData* d = mj_makeData(m); + MjDataPtr d = MakeData(m); // perturb a central vertex with upward velocity // vertex layout is 6x6 grid; pick a central vertex (row=3, col=3 -> id=21) @@ -3968,7 +3767,7 @@ TEST_F(ImplicitIntegratorTest, BendingDampingDecaysEnergy) { d->qvel[dofadr + 2] = 1.0; // z-velocity // initial forward to compute energy - mj_forward(m, d); + mj_forward(m.get(), d.get()); mjtNum initial_energy = d->energy[0] + d->energy[1]; ASSERT_GT(initial_energy, 0) << "initial energy should be nonzero"; @@ -3976,7 +3775,7 @@ TEST_F(ImplicitIntegratorTest, BendingDampingDecaysEnergy) { mjtNum max_energy = initial_energy; int nsteps = 100; for (int i = 0; i < nsteps; i++) { - mj_step(m, d); + mj_step(m.get(), d.get()); mjtNum total_energy = d->energy[0] + d->energy[1]; max_energy = mju_max(max_energy, total_energy); } @@ -3991,9 +3790,6 @@ TEST_F(ImplicitIntegratorTest, BendingDampingDecaysEnergy) { EXPECT_LT(final_energy, 0.5 * initial_energy) << "energy did not decay by at least 50% after " << nsteps << " steps" << " (initial=" << initial_energy << ", final=" << final_energy << ")"; - - mj_deleteData(d); - mj_deleteModel(m); } // interp stretch stiffness with implicitfast must preserve energy stability @@ -4015,10 +3811,10 @@ TEST_F(ImplicitIntegratorTest, InterpStretchEnergy) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; - mjData* d = mj_makeData(m); + MjDataPtr d = MakeData(m); // perturb a central vertex with velocity int center_body = m->nbody / 2; @@ -4026,7 +3822,7 @@ TEST_F(ImplicitIntegratorTest, InterpStretchEnergy) { ASSERT_GT(m->body_dofnum[center_body], 0); d->qvel[dofadr + 2] = 1.0; // z-velocity - mj_forward(m, d); + mj_forward(m.get(), d.get()); mjtNum initial_energy = d->energy[0] + d->energy[1]; ASSERT_GT(initial_energy, 0) << "initial energy should be nonzero"; @@ -4034,7 +3830,7 @@ TEST_F(ImplicitIntegratorTest, InterpStretchEnergy) { mjtNum max_energy = initial_energy; int nsteps = 50; for (int i = 0; i < nsteps; i++) { - mj_step(m, d); + mj_step(m.get(), d.get()); mjtNum total_energy = d->energy[0] + d->energy[1]; max_energy = mju_max(max_energy, total_energy); } @@ -4043,9 +3839,6 @@ TEST_F(ImplicitIntegratorTest, InterpStretchEnergy) { EXPECT_LE(max_energy, initial_energy * 1.01) << "energy exceeded initial by more than 1%: max=" << max_energy << ", initial=" << initial_energy; - - mj_deleteData(d); - mj_deleteModel(m); } } // namespace diff --git a/test/engine/engine_io_test.cc b/test/engine/engine_io_test.cc index 6a443944..bffb77e0 100644 --- a/test/engine/engine_io_test.cc +++ b/test/engine/engine_io_test.cc @@ -15,7 +15,6 @@ // Tests for engine/{engine_io.c and engine_memory.c}. #include "src/engine/engine_io.h" -#include "src/engine/engine_memory.h" #include #include @@ -26,11 +25,12 @@ #include #include -#include #include // IWYU pragma: keep +#include #include #include #include +#include "src/engine/engine_memory.h" #include "src/engine/engine_thread.h" #include "test/fixture.h" @@ -57,19 +57,19 @@ TEST_F(EngineIoTest, VerifySizeModel) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - std::filesystem::path temp_file = ( - std::filesystem::temp_directory_path() / "model.mjb"); + std::filesystem::path temp_file = + (std::filesystem::temp_directory_path() / "model.mjb"); - mj_saveModel(model, temp_file.string().c_str(), NULL, 0); + mj_saveModel(model.get(), temp_file.string().c_str(), NULL, 0); std::uintmax_t file_size = std::filesystem::file_size(temp_file); - int model_size = mj_sizeModel(model); + int model_size = mj_sizeModel(model.get()); std::filesystem::remove(temp_file); - mj_deleteModel(model); EXPECT_EQ(file_size, model_size); } @@ -87,15 +87,13 @@ TEST_F(EngineIoTest, MakeDataLoadsQpos0) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); model->qpos0[0] = 1; - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); EXPECT_EQ(data->qpos[0], 1); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineIoTest, MakeDataLoadsMocapBodies) { @@ -110,43 +108,40 @@ TEST_F(EngineIoTest, MakeDataLoadsMocapBodies) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); EXPECT_EQ(data->mocap_pos[0], 42); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineIoTest, MakeDataReturnsNullOnFailure) { constexpr char xml[] = ""; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); // fail mj_makeData intentionally with a bad size model->nbody = -1; MockWarningHandler warning_handler; warning_handler.ExpectWarnings(); - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); EXPECT_THAT(data, IsNull()); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineIoTest, ResetVariableSizes) { constexpr char xml[] = ""; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - mjData* data = mj_makeData(model); - ASSERT_THAT(model, NotNull()) << "Failed to create mjData"; + MjDataPtr data = MakeData(model); + ASSERT_THAT(model.get(), NotNull()) << "Failed to create mjData"; // don't call mj_forward, vars should be reset EXPECT_EQ(data->ne, 0); @@ -155,25 +150,23 @@ TEST_F(EngineIoTest, ResetVariableSizes) { EXPECT_EQ(data->nJ, 0); EXPECT_EQ(data->nA, 0); EXPECT_EQ(data->ncon, 0); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineIoTest, MakeDataResetsAllArenaPointerSizes) { constexpr char xml[] = ""; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); // without calling mj_forward, all array sizes should be zero. EXPECT_EQ(data->parena, 0) << "expecting empty arena"; #define X(type, name, nr, nc) \ - EXPECT_EQ(nr*nc, 0) << "expecting (" #nr " x " #nc ") to be zero"; + EXPECT_EQ(nr* nc, 0) << "expecting (" #nr " x " #nc ") to be zero"; #undef MJ_D #undef MJ_M @@ -185,9 +178,6 @@ TEST_F(EngineIoTest, MakeDataResetsAllArenaPointerSizes) { #define MJ_D(n) n #define MJ_M(n) n #undef X - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineIoTest, MjvCopyModel) { @@ -202,28 +192,27 @@ TEST_F(EngineIoTest, MjvCopyModel) { )"; char error[1024]; - mjModel* model1 = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model1, NotNull()) << error; + MjModelPtr model1 = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model1.get(), NotNull()) << error; - mjModel* model2 = mj_copyModel(nullptr, model1); + mjModel* model2 = mj_copyModel(nullptr, model1.get()); ASSERT_THAT(model2, NotNull()) << error; model1->mesh_vert[0] = 0.1; model1->geom_rgba[0] = 0.2; - mj_copyModel(model2, model1); + mj_copyModel(model2, model1.get()); EXPECT_FLOAT_EQ(model2->mesh_vert[0], 0.1); EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.2); model1->mesh_vert[0] = 0.3; model1->geom_rgba[0] = 0.4; - mjv_copyModel(model2, model1); + mjv_copyModel(model2, model1.get()); EXPECT_FLOAT_EQ(model2->mesh_vert[0], 0.1); // unchanged EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.4); mj_deleteModel(model2); - mj_deleteModel(model1); } TEST_F(EngineIoTest, MjvCopyData) { @@ -239,23 +228,21 @@ TEST_F(EngineIoTest, MjvCopyData) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; - mjData* data1 = mj_makeData(model); - mj_forward(model, data1); + MjDataPtr data1 = MakeData(model); + mj_forward(model.get(), data1.get()); EXPECT_THAT(data1->efc_J, NotNull()); - mjData* data2 = mj_copyData(nullptr, model, data1); + mjData* data2 = mj_copyData(nullptr, model.get(), data1.get()); EXPECT_THAT(data2->efc_J, NotNull()); mj_deleteData(data2); - data2 = mjv_copyData(nullptr, model, data1); + data2 = mjv_copyData(nullptr, model.get(), data1.get()); EXPECT_THAT(data2->efc_J, IsNull()); mj_deleteData(data2); - mj_deleteData(data1); - mj_deleteModel(model); } using ValidateReferencesTest = MujocoTest; @@ -273,15 +260,14 @@ TEST_F(ValidateReferencesTest, BodyReferences) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - EXPECT_THAT(mj_validateReferences(model), IsNull()); + EXPECT_THAT(mj_validateReferences(model.get()), IsNull()); model->jnt_bodyid[0] = 2; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("jnt_bodyid")); - - mj_deleteModel(model); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("jnt_bodyid")); } TEST_F(ValidateReferencesTest, AddressRange) { @@ -298,20 +284,19 @@ TEST_F(ValidateReferencesTest, AddressRange) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - EXPECT_THAT(mj_validateReferences(model), IsNull()); + EXPECT_THAT(mj_validateReferences(model.get()), IsNull()); model->body_jntnum[1] = 3; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("body_jntadr")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("body_jntadr")); model->body_jntnum[1] = 2; // Could be more strict and test for -1, but at the moment the code is a bit // lenient. model->body_jntadr[1] = -2; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("body_jntadr")); - - mj_deleteModel(model); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("body_jntadr")); } TEST_F(ValidateReferencesTest, AddressRangeNegativeNum) { @@ -328,16 +313,16 @@ TEST_F(ValidateReferencesTest, AddressRangeNegativeNum) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - EXPECT_THAT(mj_validateReferences(model), IsNull()); + EXPECT_THAT(mj_validateReferences(model.get()), IsNull()); // jntadr + jntnum is within safe range, but jntnum is negative. model->body_jntadr[1] += 5; model->body_jntnum[1] -= 5; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("body_jntnum")); - mj_deleteModel(model); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("body_jntnum")); } TEST_F(ValidateReferencesTest, GeomCondim) { @@ -353,16 +338,15 @@ TEST_F(ValidateReferencesTest, GeomCondim) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - EXPECT_THAT(mj_validateReferences(model), IsNull()); + EXPECT_THAT(mj_validateReferences(model.get()), IsNull()); model->geom_condim[0] = 7; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("geom_condim")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("geom_condim")); model->geom_condim[0] = -1; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("geom_condim")); - - mj_deleteModel(model); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("geom_condim")); } TEST_F(ValidateReferencesTest, HField) { @@ -375,18 +359,17 @@ TEST_F(ValidateReferencesTest, HField) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - EXPECT_THAT(mj_validateReferences(model), IsNull()); + EXPECT_THAT(mj_validateReferences(model.get()), IsNull()); model->hfield_adr[0] = -2; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("hfield_adr")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("hfield_adr")); model->hfield_adr[0] = 0; model->hfield_ncol[0] = 4; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("hfield_adr")); - - mj_deleteModel(model); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("hfield_adr")); } TEST_F(ValidateReferencesTest, Texture) { @@ -399,18 +382,17 @@ TEST_F(ValidateReferencesTest, Texture) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - EXPECT_THAT(mj_validateReferences(model), IsNull()); + EXPECT_THAT(mj_validateReferences(model.get()), IsNull()); model->tex_adr[0] = -2; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("tex_adr")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("tex_adr")); model->tex_adr[0] = 0; model->tex_height[0] = 4; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("tex_adr")); - - mj_deleteModel(model); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("tex_adr")); } TEST_F(ValidateReferencesTest, GeomPairs) { @@ -433,19 +415,18 @@ TEST_F(ValidateReferencesTest, GeomPairs) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - EXPECT_THAT(mj_validateReferences(model), IsNull()); + EXPECT_THAT(mj_validateReferences(model.get()), IsNull()); // Invalid geomid=4 model->pair_signature[0] = (1 << 16) | 5; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("pair_body1")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("pair_body1")); model->pair_signature[0] = (5 << 16) | 1; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("pair_body2")); - - mj_deleteModel(model); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("pair_body2")); } TEST_F(ValidateReferencesTest, SensorsAddress) { @@ -482,12 +463,12 @@ TEST_F(ValidateReferencesTest, SensorsAddress) { for (const std::string& sensor_string : sensor_strings) { std::string xml = absl::StrFormat(xml_template, sensor_string); std::array error; - mjModel* model = + MjModelPtr model = LoadModelFromString(xml.c_str(), error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - EXPECT_THAT(mj_validateReferences(model), IsNull()); - mj_deleteModel(model); + EXPECT_THAT(mj_validateReferences(model.get()), IsNull()); } } @@ -503,11 +484,11 @@ TEST_F(ValidateReferencesTest, SensorsAddressUser) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - EXPECT_THAT(mj_validateReferences(model), IsNull()); - mj_deleteModel(model); + EXPECT_THAT(mj_validateReferences(model.get()), IsNull()); } TEST_F(ValidateReferencesTest, SensorsObj) { @@ -527,25 +508,24 @@ TEST_F(ValidateReferencesTest, SensorsObj) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); model->sensor_objtype[0] = -1; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_objtype")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("sensor_objtype")); model->sensor_objtype[0] = mjOBJ_SITE; model->sensor_objid[0] = model->nsite; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_objid")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("sensor_objid")); model->sensor_objid[0] = 0; model->sensor_reftype[0] = -1; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_reftype")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("sensor_reftype")); model->sensor_reftype[0] = mjOBJ_BODY; model->sensor_refid[0] = model->nbody; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_refid")); - - mj_deleteModel(model); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("sensor_refid")); } TEST_F(ValidateReferencesTest, MoreBodiesThanGeoms) { @@ -565,10 +545,10 @@ TEST_F(ValidateReferencesTest, MoreBodiesThanGeoms) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); - EXPECT_THAT(mj_validateReferences(model), IsNull()); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); + EXPECT_THAT(mj_validateReferences(model.get()), IsNull()); } TEST_F(ValidateReferencesTest, BodyExcludes) { @@ -585,18 +565,17 @@ TEST_F(ValidateReferencesTest, BodyExcludes) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - EXPECT_THAT(mj_validateReferences(model), IsNull()); + EXPECT_THAT(mj_validateReferences(model.get()), IsNull()); // Invalid bodyid=3 model->exclude_signature[0] = (1 << 16) | 4; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("exclude_body1")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("exclude_body1")); model->exclude_signature[0] = (4 << 16) | 2; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("exclude_body2")); - - mj_deleteModel(model); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("exclude_body2")); } TEST_F(ValidateReferencesTest, EqualityConstraints) { @@ -634,38 +613,37 @@ TEST_F(ValidateReferencesTest, EqualityConstraints) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - EXPECT_THAT(mj_validateReferences(model), IsNull()); + EXPECT_THAT(mj_validateReferences(model.get()), IsNull()); // connect constraint model->eq_obj1id[0] = -1; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj1id")); model->eq_obj1id[0] = model->nbody; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj1id")); model->eq_obj1id[0] = 1; model->eq_obj2id[0] = -2; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj2id")); model->eq_obj2id[0] = model->nbody; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj2id")); model->eq_obj2id[0] = 0; // weld constraint model->eq_obj1id[1] = -1; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj1id")); model->eq_obj1id[1] = model->nbody; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj1id")); model->eq_obj1id[1] = 1; model->eq_obj2id[1] = -2; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj2id")); model->eq_obj2id[1] = model->nbody; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj2id")); model->eq_obj2id[1] = model->nbody - 1; - - mj_deleteModel(model); } TEST_F(ValidateReferencesTest, Tuples) { @@ -692,19 +670,19 @@ TEST_F(ValidateReferencesTest, Tuples) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - EXPECT_THAT(mj_validateReferences(model), IsNull()); + EXPECT_THAT(mj_validateReferences(model.get()), IsNull()); model->tuple_objtype[0] = -1; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("tuple_objtype")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("tuple_objtype")); model->tuple_objtype[0] = mjOBJ_BODY; model->tuple_objid[0] = model->nbody; - EXPECT_THAT(mj_validateReferences(model), HasSubstr("tuple_objid")); + EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("tuple_objid")); model->tuple_objid[0] = 1; - mj_deleteModel(model); } TEST_F(EngineIoTest, CanMarkAndFreeStack) { @@ -716,20 +694,18 @@ TEST_F(EngineIoTest, CanMarkAndFreeStack) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); auto pstack_before = data->pstack; - mj_markStack(data); + mj_markStack(data.get()); EXPECT_GT(data->pstack, pstack_before); - mj_freeStack(data); + mj_freeStack(data.get()); EXPECT_EQ(data->pstack, pstack_before); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineIoTest, LargeMemory) { @@ -740,27 +716,25 @@ TEST_F(EngineIoTest, LargeMemory) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); // allocate 2.3G of mjtNums - mj_markStack(data); + mj_markStack(data.get()); size_t num = 2300000000 / sizeof(mjtNum); - mjtNum* testNum = mj_stackAllocNum(data, num); - testNum[num-1] = 1; - mj_freeStack(data); + mjtNum* testNum = mj_stackAllocNum(data.get(), num); + testNum[num - 1] = 1; + mj_freeStack(data.get()); // allocate 2.3G of bytes - mj_markStack(data); + mj_markStack(data.get()); num = 2300000000; - char* testByte = (char*) mj_stackAllocByte(data, num, alignof(char)); - testByte[num-1] = 1; - mj_freeStack(data); - - mj_deleteData(data); - mj_deleteModel(model); + char* testByte = (char*)mj_stackAllocByte(data.get(), num, alignof(char)); + testByte[num - 1] = 1; + mj_freeStack(data.get()); } TEST_F(EngineIoTest, VeryLargeMemory) { @@ -771,31 +745,29 @@ TEST_F(EngineIoTest, VeryLargeMemory) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - if (!model) { + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + if (!model.get()) { // in some test environments, 8GB is too large EXPECT_THAT(error.data(), HasSubstr("Could not allocate memory")); } else { - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); - mjData* data = mj_makeData(model); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); // allocate 7G of mjtNums - mj_markStack(data); + mj_markStack(data.get()); size_t num = 7516192768ull / sizeof(mjtNum); - mjtNum* testNum = mj_stackAllocNum(data, num); - testNum[num-1] = 1; - mj_freeStack(data); + mjtNum* testNum = mj_stackAllocNum(data.get(), num); + testNum[num - 1] = 1; + mj_freeStack(data.get()); // allocate 7G of bytes - mj_markStack(data); + mj_markStack(data.get()); num = 7516192768ull; - char* testByte = (char*) mj_stackAllocByte(data, num, alignof(char)); - testByte[num-1] = 1; - mj_freeStack(data); - - mj_deleteData(data); - mj_deleteModel(model); + char* testByte = (char*)mj_stackAllocByte(data.get(), num, alignof(char)); + testByte[num - 1] = 1; + mj_freeStack(data.get()); } } @@ -822,23 +794,22 @@ TEST_F(EngineIoTest, TestStackShardingForThreads) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); - mju_threadpool(data, 10); + mju_threadpool(data.get(), 10); constexpr int kTasks = 1000; TestFunctionArgs test_function_args; - mju_dispatch(model, data, TestFunction, &test_function_args, kTasks); + mju_dispatch(model.get(), data.get(), TestFunction, &test_function_args, + kTasks); for (int i = 0; i < kTasks; ++i) { EXPECT_EQ(i, test_function_args.stack_output[i]); } - - mj_deleteData(data); - mj_deleteModel(model); } #ifdef ADDRESS_SANITIZER @@ -858,37 +829,36 @@ TEST_F(EngineIoTest, CanDetectStackFrameLeakage) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); // MarkFreeStack correctly calls mj_freeStack, should not error. - MarkFreeStack(data, /* free= */ true); + MarkFreeStack(data.get(), /* free= */ true); // MarkFreeStack calls mj_markStack without mj_freeStack, the next call to // mj_freeStack should detect the stack frame leakage. - mj_markStack(data); - MarkFreeStack(data, /* free= */ false); + mj_markStack(data.get()); + MarkFreeStack(data.get(), /* free= */ false); EXPECT_THAT( - MjuErrorMessageFrom(mj_freeStack)(data), + MjuErrorMessageFrom(mj_freeStack)(data.get()), ContainsRegex( "mj_markStack in MarkFreeStack at .*engine_io_test\\.cc.* has no " "corresponding mj_freeStack")); // Dangling stack frames should be detected in mj_deleteData. - mj_resetData(model, data); - mj_markStack(data); + mj_resetData(model.get(), data.get()); + mj_markStack(data.get()); EXPECT_THAT( - MjuErrorMessageFrom(mj_deleteData)(data), + MjuErrorMessageFrom(mj_deleteData)(data.get()), ContainsRegex( "mj_markStack in .+EngineIoTest_CanDetectStackFrameLeakage.+ has no " "corresponding mj_freeStack")); - mj_resetData(model, data); - mj_deleteData(data); - mj_deleteModel(model); + mj_resetData(model.get(), data.get()); } TEST_F(EngineIoTest, RedZoneAlignmentTest) { @@ -900,19 +870,17 @@ TEST_F(EngineIoTest, RedZoneAlignmentTest) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); - mj_markStack(data); - mj_stackAllocByte(data, 1, 1); - mj_stackAllocByte(data, 1, 1); - mj_freeStack(data); - - mj_deleteData(data); - mj_deleteModel(model); + mj_markStack(data.get()); + mj_stackAllocByte(data.get(), 1, 1); + mj_stackAllocByte(data.get(), 1, 1); + mj_freeStack(data.get()); } #endif @@ -930,15 +898,15 @@ TEST_F(EngineIoTest, LoadModelBufferRejectsOverflowingSizes) { constexpr char xml[] = ""; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); // save model to a buffer - int bufsize = mj_sizeModel(model); + int bufsize = mj_sizeModel(model.get()); ASSERT_GT(bufsize, 0); std::vector buffer(bufsize); - mj_saveModel(model, nullptr, buffer.data(), bufsize); - mj_deleteModel(model); + mj_saveModel(model.get(), nullptr, buffer.data(), bufsize); // locate the size fields in the buffer (after 5-int header) const int header_bytes = 5 * sizeof(int); diff --git a/test/engine/engine_island_test.cc b/test/engine/engine_island_test.cc index 227b350d..274d2b42 100644 --- a/test/engine/engine_island_test.cc +++ b/test/engine/engine_island_test.cc @@ -14,6 +14,8 @@ // Tests for engine/engine_island.c. +#include "src/engine/engine_island.h" + #include #include @@ -21,7 +23,6 @@ #include #include #include -#include "src/engine/engine_island.h" #include "src/engine/engine_util_sparse.h" #include "test/fixture.h" @@ -34,16 +35,11 @@ using ::testing::NotNull; using ::testing::Pointwise; using IslandTest = MujocoTest; - TEST_F(IslandTest, FloodFillSingleton) { // adjacency matrix for the graph 0 1 2 // U U // (3 singletons, 0 and 2 have self-edges) - mjtNum mat[9] = { - 1, 0, 0, - 0, 0, 0, - 0, 0, 1 - }; + mjtNum mat[9] = {1, 0, 0, 0, 0, 0, 0, 0, 1}; constexpr int nr = 3; constexpr int nnz = 2; int rownnz[nr]; @@ -54,7 +50,7 @@ TEST_F(IslandTest, FloodFillSingleton) { // outputs / scratch int island[nr]; - int scratch[2*nr]; + int scratch[2 * nr]; // flood fill int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, scratch); @@ -63,14 +59,9 @@ TEST_F(IslandTest, FloodFillSingleton) { EXPECT_THAT(island, ElementsAre(0, -1, 1)); } - TEST_F(IslandTest, FloodFill1) { // adjacency matrix for the graph 0 - 1 - 2 - mjtNum mat[9] = { - 0, 1, 0, - 1, 0, 1, - 0, 1, 0 - }; + mjtNum mat[9] = {0, 1, 0, 1, 0, 1, 0, 1, 0}; constexpr int nr = 3; constexpr int nnz = 4; int rownnz[nr]; @@ -89,17 +80,11 @@ TEST_F(IslandTest, FloodFill1) { EXPECT_THAT(island, ElementsAre(0, 0, 0)); } - TEST_F(IslandTest, FloodFill2) { // adjacency matrix for the graph 6 – 1 – 4 0 – 3 – 5 – 2 mjtNum mat[49] = { - 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 0, 1, 0, 1, - 0, 0, 0, 0, 0, 1, 0, - 1, 0, 0, 0, 0, 1, 0, - 0, 1, 0, 0, 0, 0, 0, - 0, 0, 1, 1, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, + 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, }; constexpr int nr = 7; constexpr int nnz = 10; @@ -119,15 +104,11 @@ TEST_F(IslandTest, FloodFill2) { EXPECT_THAT(island, ElementsAre(0, 1, 0, 0, 1, 0, 1)); } - TEST_F(IslandTest, FloodFill3a) { // adjacency matrix for the graph 0 2 1 – 3 // U mjtNum mat[16] = { - 0, 0, 0, 0, - 0, 0, 0, 1, - 0, 0, 1, 0, - 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, }; constexpr int nr = 4; constexpr int nnz = 3; @@ -147,7 +128,6 @@ TEST_F(IslandTest, FloodFill3a) { EXPECT_THAT(island, ElementsAre(-1, 0, 1, 0)); } - TEST_F(IslandTest, FloodFill3b) { /* adjacency matrix for the graph 1 – 2 3 4 – 5 @@ -155,13 +135,8 @@ TEST_F(IslandTest, FloodFill3b) { 0 – 6 */ mjtNum mat[49] = { - 0, 0, 0, 0, 1, 0, 1, - 0, 1, 1, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 1, 1, - 0, 0, 0, 0, 1, 0, 1, - 1, 0, 0, 0, 1, 1, 0, + 0, 0, 0, 0, 1, 0, 1, 0, 1, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, }; constexpr int nr = 7; constexpr int nnz = 13; @@ -181,8 +156,7 @@ TEST_F(IslandTest, FloodFill3b) { EXPECT_THAT(island, ElementsAre(0, 1, 1, -1, 0, 0, 0)); } -static const char* const kAbacusPath = - "engine/testdata/island/abacus.xml"; +static const char* const kAbacusPath = "engine/testdata/island/abacus.xml"; TEST_F(IslandTest, Abacus) { const std::string xml_path = GetTestDataFilePath(kAbacusPath); @@ -207,10 +181,10 @@ TEST_F(IslandTest, Abacus) { } // sizes - int nv = model->nv; - int nefc = data->nefc; + int nv = model->nv; + int nefc = data->nefc; int nisland = data->nisland; - int nidof = data->nidof; + int nidof = data->nidof; // 4 dofs, 12 constraints, 2 islands EXPECT_EQ(nv, 4); @@ -268,9 +242,9 @@ TEST_F(IslandTest, Abacus) { } // local variables - nefc = data->nefc; - nisland = data->nisland; - nidof = data->nidof; + nefc = data->nefc; + nisland = data->nisland; + nidof = data->nidof; EXPECT_EQ(nisland, 3); EXPECT_EQ(nidof, 4); @@ -314,8 +288,8 @@ TEST_F(IslandTest, DenseSparse) { } // sizes - int nv = model->nv; - int nefc = data1->nefc; + int nv = model->nv; + int nefc = data1->nefc; int nisland = data1->nisland; // expect sparse and dense to be identical @@ -327,8 +301,7 @@ TEST_F(IslandTest, DenseSparse) { AsVector(data2->island_idofadr, nisland)); EXPECT_EQ(AsVector(data1->island_nv, nisland), AsVector(data2->island_nv, nisland)); - EXPECT_EQ(AsVector(data1->dof_island, nv), - AsVector(data2->dof_island, nv)); + EXPECT_EQ(AsVector(data1->dof_island, nv), AsVector(data2->dof_island, nv)); EXPECT_EQ(AsVector(data1->map_idof2dof, nv), AsVector(data2->map_idof2dof, nv)); EXPECT_EQ(AsVector(data1->map_dof2idof, nv), @@ -462,12 +435,10 @@ TEST_F(IslandTest, EqualityConstraintOfTendons) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); - mj_forward(model, data); - mj_deleteData(data); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mj_forward(model.get(), data.get()); } TEST_F(IslandTest, PGSIsland) { @@ -496,8 +467,7 @@ TEST_F(IslandTest, PGSIsland) { // solve without islands m->opt.disableflags |= mjDSBL_ISLAND; mj_forward(m, d); - std::vector qfrc_mono(d->qfrc_constraint, - d->qfrc_constraint + m->nv); + std::vector qfrc_mono(d->qfrc_constraint, d->qfrc_constraint + m->nv); // expect close match (inexact due to randomized constraint visitation order) EXPECT_THAT(qfrc_island, Pointwise(MjNear(1e-3, 1e-3), qfrc_mono)); diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index e4bd8d47..4655961f 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -49,28 +49,25 @@ TEST_F(PassiveTest, DisableFlags) { )"; char error[1024]; - mjModel* m = LoadModelFromString(flex_xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; - mjData* d = mj_makeData(m); - mj_resetDataKeyframe(m, d, 0); + MjModelPtr m = LoadModelFromString(flex_xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; + MjDataPtr d = MakeData(m); + mj_resetDataKeyframe(m.get(), d.get(), 0); - mj_forward(m, d); + mj_forward(m.get(), d.get()); EXPECT_MJTNUM_EQ(d->qacc[0], 11); m->opt.disableflags = mjDSBL_DAMPER; - mj_forward(m, d); + mj_forward(m.get(), d.get()); EXPECT_MJTNUM_EQ(d->qacc[0], 10); m->opt.disableflags = mjDSBL_SPRING; - mj_forward(m, d); + mj_forward(m.get(), d.get()); EXPECT_MJTNUM_EQ(d->qacc[0], 1); m->opt.disableflags = mjDSBL_SPRING | mjDSBL_DAMPER; - mj_forward(m, d); + mj_forward(m.get(), d.get()); EXPECT_EQ(d->qacc[0], -10); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(PassiveTest, GravcompNestedBody) { @@ -89,17 +86,14 @@ TEST_F(PassiveTest, GravcompNestedBody) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); - mj_forward(m, d); + mj_forward(m.get(), d.get()); EXPECT_GT(d->qacc[2], 0); EXPECT_NEAR(d->qacc[2], 2.0, 0.1); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(PassiveTest, PolyStiffnessSlide) { @@ -119,16 +113,13 @@ TEST_F(PassiveTest, PolyStiffnessSlide) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); - mj_resetDataKeyframe(m, d, 0); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); + mj_resetDataKeyframe(m.get(), d.get(), 0); - mj_forward(m, d); + mj_forward(m.get(), d.get()); EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(PassiveTest, PolyStiffnessAntiSymmetric) { @@ -148,16 +139,13 @@ TEST_F(PassiveTest, PolyStiffnessAntiSymmetric) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); - mj_resetDataKeyframe(m, d, 0); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); + mj_resetDataKeyframe(m.get(), d.get(), 0); - mj_forward(m, d); + mj_forward(m.get(), d.get()); EXPECT_MJTNUM_EQ(d->qfrc_spring[0], 8); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(PassiveTest, PolyStiffnessTendon) { @@ -183,21 +171,18 @@ TEST_F(PassiveTest, PolyStiffnessTendon) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; m->tendon_stiffness[0] = 10; m->tendon_stiffnesspoly[0] = 5; m->tendon_stiffnesspoly[1] = 1; - mjData* d = mj_makeData(m); - mj_resetDataKeyframe(m, d, 0); + MjDataPtr d = MakeData(m); + mj_resetDataKeyframe(m.get(), d.get(), 0); - mj_forward(m, d); + mj_forward(m.get(), d.get()); EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(PassiveTest, PolyStiffnessEnergy) { @@ -221,21 +206,18 @@ TEST_F(PassiveTest, PolyStiffnessEnergy) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); - mj_resetDataKeyframe(m, d, 0); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); + mj_resetDataKeyframe(m.get(), d.get(), 0); - mj_forward(m, d); + mj_forward(m.get(), d.get()); mjtNum total_energy = d->energy[0] + d->energy[1]; for (int i = 0; i < 100; i++) { - mj_step(m, d); + mj_step(m.get(), d.get()); EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.002); } - - mj_deleteData(d); - mj_deleteModel(m); } // ------------------------ ellipsoid fluid model ------------------------------ @@ -261,11 +243,11 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) { )"; char error[1024]; - mjModel* m2 = LoadModelFromString(two_bodies_xml, error, sizeof(error)); - ASSERT_THAT(m2, NotNull()) << error; - mjData* d2 = mj_makeData(m2); + MjModelPtr m2 = LoadModelFromString(two_bodies_xml, error, sizeof(error)); + ASSERT_THAT(m2.get(), NotNull()) << error; + MjDataPtr d2 = MakeData(m2); for (int i = 0; i < 6; i++) { - d2->qvel[i] = (mjtNum) i+1; + d2->qvel[i] = (mjtNum)i + 1; } d2->qpos[3] = 0.5; d2->qpos[4] = 0.5; @@ -285,11 +267,11 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) { )"; - mjModel* m1 = LoadModelFromString(one_body_xml, error, sizeof(error)); - ASSERT_THAT(m1, NotNull()) << error; - mjData* d1 = mj_makeData(m1); + MjModelPtr m1 = LoadModelFromString(one_body_xml, error, sizeof(error)); + ASSERT_THAT(m1.get(), NotNull()) << error; + MjDataPtr d1 = MakeData(m1); for (int i = 0; i < 6; i++) { - d1->qvel[i] = (mjtNum) i+1; + d1->qvel[i] = (mjtNum)i + 1; } d1->qpos[3] = 0.5; d1->qpos[4] = 0.5; @@ -301,16 +283,11 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) { EXPECT_EQ(m1->nv, m2->nv); - mj_forward(m2, d2); - mj_forward(m1, d1); + mj_forward(m2.get(), d2.get()); + mj_forward(m1.get(), d1.get()); for (int i = 0; i < m1->nv; i++) { EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol); } - - mj_deleteData(d1); - mj_deleteModel(m1); - mj_deleteData(d2); - mj_deleteModel(m2); } TEST_F(EllipsoidFluidTest, DefaultsPropagate) { @@ -334,16 +311,13 @@ TEST_F(EllipsoidFluidTest, DefaultsPropagate) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - EXPECT_THAT(AsVector(model->geom_fluid, 6), - ElementsAre(0, 0, 0, 0, 0, 0)); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + EXPECT_THAT(AsVector(model->geom_fluid, 6), ElementsAre(0, 0, 0, 0, 0, 0)); EXPECT_THAT(AsVector(model->geom_fluid + mjNFLUID, 6), ElementsAre(1, 2, 3, 4, 5, 6)); - mj_deleteModel(model); } - // ------------------------------ tendons -------------------------------------- using TendonTest = MujocoTest; @@ -359,7 +333,7 @@ TEST_F(TendonTest, SpringrangeDeadband) { // initial state outside deadband: spring is active mj_forward(model, data); mjtNum expected_force = model->tendon_stiffness[0] * - (model->tendon_lengthspring[1] - data->ten_length[0]); + (model->tendon_lengthspring[1] - data->ten_length[0]); EXPECT_EQ(expected_force, data->qfrc_passive[0]); // put body inside deadband: spring is inactive @@ -391,12 +365,10 @@ TEST_F(ElasticityTest, FlexCompatibility) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(flex_xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; + MjModelPtr m = LoadModelFromString(flex_xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; - mjData* d = mj_makeData(m); - mj_deleteData(d); - mj_deleteModel(m); + MjDataPtr d = MakeData(m); } // -------------------------------- shell ----------------------------------- @@ -413,39 +385,37 @@ TEST_F(ElasticityTest, ElasticEnergyShell) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; - mjData* d = mj_makeData(m); - mj_kinematics(m, d); - mj_flex(m, d); + MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; + MjDataPtr d = MakeData(m); + mj_kinematics(m.get(), d.get()); + mj_flex(m.get(), d.get()); // check that a plane is in the kernel of the energy for (mjtNum scale = 1; scale < 4; scale++) { for (int e = 0; e < m->flex_edgenum[0]; e++) { - int* edge = m->flex_edge + 2*(m->flex_edgeadr[0] + e); - int* flap = m->flex_edgeflap + 2*(m->flex_edgeadr[0] + e); + int* edge = m->flex_edge + 2 * (m->flex_edgeadr[0] + e); + int* flap = m->flex_edgeflap + 2 * (m->flex_edgeadr[0] + e); int v[4] = {edge[0], edge[1], flap[0], flap[1]}; - if (v[3]== -1) { + if (v[3] == -1) { continue; } mjtNum energy = 0; - mjtNum volume = 1./2.; + mjtNum volume = 1. / 2.; for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { for (int x = 0; x < 3; x++) { - mjtNum elongation1 = scale * d->flexvert_xpos[3*v[i]+x]; - mjtNum elongation2 = scale * d->flexvert_xpos[3*v[j]+x]; - energy += m->flex_bending[17*e+4*i+j] * elongation1 * elongation2; + mjtNum elongation1 = scale * d->flexvert_xpos[3 * v[i] + x]; + mjtNum elongation2 = scale * d->flexvert_xpos[3 * v[j] + x]; + energy += + m->flex_bending[17 * e + 4 * i + j] * elongation1 * elongation2; } } } - EXPECT_NEAR( - 4*energy/volume, 0, std::numeric_limits::epsilon()); + EXPECT_NEAR(4 * energy / volume, 0, + std::numeric_limits::epsilon()); } } - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(ElasticityTest, CurvedShell) { @@ -487,12 +457,12 @@ TEST_F(ElasticityTest, CurvedShell) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; - mjData* d = mj_makeData(m); - mj_kinematics(m, d); - mj_flex(m, d); - mj_passive(m, d); + MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; + MjDataPtr d = MakeData(m); + mj_kinematics(m.get(), d.get()); + mj_flex(m.get(), d.get()); + mj_passive(m.get(), d.get()); // v1 force component is in-plane along v1-v0 edge (y-axis) EXPECT_NEAR(d->qfrc_spring[3], 0, 1e-6); @@ -505,9 +475,6 @@ TEST_F(ElasticityTest, CurvedShell) { // v3 force component is in-plane along v3-v0 edge (z-axis) EXPECT_NEAR(d->qfrc_spring[9], 0, 1e-6); EXPECT_NEAR(d->qfrc_spring[10], 0, 1e-6); - - mj_deleteData(d); - mj_deleteModel(m); } // -------------------------------- membrane ----------------------------------- @@ -525,12 +492,12 @@ TEST_F(ElasticityTest, ElasticEnergyMembrane) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; + MjDataPtr d = MakeData(m); - mj_kinematics(m, d); - mj_flex(m, d); + mj_kinematics(m.get(), d.get()); + mj_flex(m.get(), d.get()); mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0]; // check that if the entire geometry is rescaled by a factor "scale", then @@ -539,26 +506,24 @@ TEST_F(ElasticityTest, ElasticEnergyMembrane) { for (mjtNum scale = 1; scale < 4; scale++) { for (int t = 0; t < m->flex_elemnum[0]; t++) { mjtNum energy = 0; - mjtNum volume = 1./2.; + mjtNum volume = 1. / 2.; int idx = 0; for (int e1 = 0; e1 < 3; e1++) { for (int e2 = e1; e2 < 3; e2++) { - int idx1 = m->flex_elemedge[3*t+e1 + m->flex_elemedgeadr[0]]; - int idx2 = m->flex_elemedge[3*t+e2 + m->flex_elemedgeadr[0]]; + int idx1 = m->flex_elemedge[3 * t + e1 + m->flex_elemedgeadr[0]]; + int idx2 = m->flex_elemedge[3 * t + e2 + m->flex_elemedgeadr[0]]; mjtNum elong1 = scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1]; mjtNum elong2 = scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2]; - energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.); + energy += + metric[21 * t + idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.); } } const mjtNum tol = MjTol(std::numeric_limits::epsilon(), 1e-5); - EXPECT_NEAR(4*energy/volume, 2*scale*scale, tol); + EXPECT_NEAR(4 * energy / volume, 2 * scale * scale, tol); } } - - mj_deleteData(d); - mj_deleteModel(m); } // -------------------------------- solid ----------------------------------- @@ -576,12 +541,12 @@ TEST_F(ElasticityTest, ElasticEnergySolid) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; + MjDataPtr d = MakeData(m); - mj_kinematics(m, d); - mj_flex(m, d); + mj_kinematics(m.get(), d.get()); + mj_flex(m.get(), d.get()); mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0]; // check that if the entire geometry is rescaled by a factor "scale", then @@ -590,26 +555,24 @@ TEST_F(ElasticityTest, ElasticEnergySolid) { for (mjtNum scale = 1; scale < 4; scale++) { for (int t = 0; t < m->flex_elemnum[0]; t++) { mjtNum energy = 0; - mjtNum volume = 1./6.; + mjtNum volume = 1. / 6.; int idx = 0; for (int e1 = 0; e1 < 6; e1++) { for (int e2 = e1; e2 < 6; e2++) { - int idx1 = m->flex_elemedge[6*t+e1 + m->flex_elemedgeadr[0]]; - int idx2 = m->flex_elemedge[6*t+e2 + m->flex_elemedgeadr[0]]; + int idx1 = m->flex_elemedge[6 * t + e1 + m->flex_elemedgeadr[0]]; + int idx2 = m->flex_elemedge[6 * t + e2 + m->flex_elemedgeadr[0]]; mjtNum elong1 = scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1]; mjtNum elong2 = scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2]; - energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.); + energy += + metric[21 * t + idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.); } } const mjtNum tol = MjTol(std::numeric_limits::epsilon(), 1e-4); - EXPECT_NEAR(energy/volume, 3*scale*scale, tol); + EXPECT_NEAR(energy / volume, 3 * scale * scale, tol); } } - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(PassiveTest, PolynomialStiffnessJoint) { @@ -626,19 +589,16 @@ TEST_F(PassiveTest, PolynomialStiffnessJoint) { )"; - mjModel* m = LoadModelFromString(xml); - ASSERT_THAT(m, NotNull()); - mjData* d = mj_makeData(m); - mj_resetDataKeyframe(m, d, 0); - mj_forward(m, d); + MjModelPtr m = LoadModelFromString(xml); + ASSERT_THAT(m.get(), NotNull()); + MjDataPtr d = MakeData(m); + mj_resetDataKeyframe(m.get(), d.get(), 0); + mj_forward(m.get(), d.get()); mjtNum x = 0.5; mjtNum a = 2, b = 3, c = 4; mjtNum expected = -(a + b * x + c * x * x) * x; EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(PassiveTest, PolynomialStiffnessNegativeDisplacement) { @@ -655,19 +615,16 @@ TEST_F(PassiveTest, PolynomialStiffnessNegativeDisplacement) { )"; - mjModel* m = LoadModelFromString(xml); - ASSERT_THAT(m, NotNull()); - mjData* d = mj_makeData(m); - mj_resetDataKeyframe(m, d, 0); - mj_forward(m, d); + MjModelPtr m = LoadModelFromString(xml); + ASSERT_THAT(m.get(), NotNull()); + MjDataPtr d = MakeData(m); + mj_resetDataKeyframe(m.get(), d.get(), 0); + mj_forward(m.get(), d.get()); mjtNum x = -0.5; mjtNum a = 2, b = 3, c = 4; mjtNum expected = -(a + b * x + c * x * x) * x; EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(PassiveTest, PolyStiffnessFixedTendon) { @@ -693,19 +650,16 @@ TEST_F(PassiveTest, PolyStiffnessFixedTendon) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); - mj_resetDataKeyframe(m, d, 0); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); + mj_resetDataKeyframe(m.get(), d.get(), 0); - mj_forward(m, d); + mj_forward(m.get(), d.get()); mjtNum x = d->ten_length[0] - m->tendon_lengthspring[1]; - mjtNum expected = -(10 + 5*x + 1*x*x) * x; + mjtNum expected = -(10 + 5 * x + 1 * x * x) * x; EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(PassiveTest, PolyStiffnessSpatialTendon) { @@ -734,23 +688,18 @@ TEST_F(PassiveTest, PolyStiffnessSpatialTendon) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); - mj_resetDataKeyframe(m, d, 0); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); + mj_resetDataKeyframe(m.get(), d.get(), 0); - mj_forward(m, d); + mj_forward(m.get(), d.get()); mjtNum x = d->ten_length[0] - m->tendon_lengthspring[1]; - mjtNum expected = -x * (10 + 5*x + 1*x*x); + mjtNum expected = -x * (10 + 5 * x + 1 * x * x); EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12); - - mj_deleteData(d); - mj_deleteModel(m); } - - TEST_F(PassiveTest, PolynomialDampingJoint) { static constexpr char xml[] = R"( @@ -765,19 +714,16 @@ TEST_F(PassiveTest, PolynomialDampingJoint) { )"; - mjModel* m = LoadModelFromString(xml); - ASSERT_THAT(m, NotNull()); - mjData* d = mj_makeData(m); - mj_resetDataKeyframe(m, d, 0); - mj_forward(m, d); + MjModelPtr m = LoadModelFromString(xml); + ASSERT_THAT(m.get(), NotNull()); + MjDataPtr d = MakeData(m); + mj_resetDataKeyframe(m.get(), d.get(), 0); + mj_forward(m.get(), d.get()); mjtNum v = 0.5; mjtNum a = 2, b = 3, c = 4; mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v); EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(PassiveTest, PolynomialDampingNegativeVelocity) { @@ -794,19 +740,16 @@ TEST_F(PassiveTest, PolynomialDampingNegativeVelocity) { )"; - mjModel* m = LoadModelFromString(xml); - ASSERT_THAT(m, NotNull()); - mjData* d = mj_makeData(m); - mj_resetDataKeyframe(m, d, 0); - mj_forward(m, d); + MjModelPtr m = LoadModelFromString(xml); + ASSERT_THAT(m.get(), NotNull()); + MjDataPtr d = MakeData(m); + mj_resetDataKeyframe(m.get(), d.get(), 0); + mj_forward(m.get(), d.get()); mjtNum v = -0.5; mjtNum a = 2, b = 3, c = 4; mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v); EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(PassiveTest, PolynomialDampingTendon) { @@ -832,19 +775,16 @@ TEST_F(PassiveTest, PolynomialDampingTendon) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); - mj_resetDataKeyframe(m, d, 0); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); + mj_resetDataKeyframe(m.get(), d.get(), 0); - mj_forward(m, d); + mj_forward(m.get(), d.get()); mjtNum v = d->ten_velocity[0]; - mjtNum expected = -(10*v + 5*v*mju_abs(v) + 1*v*v*v); + mjtNum expected = -(10 * v + 5 * v * mju_abs(v) + 1 * v * v * v); EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12); - - mj_deleteData(d); - mj_deleteModel(m); } // shell-mode (elastic2d=stretch) flexcomp must have zero passive spring forces @@ -867,20 +807,17 @@ TEST_F(ElasticityTest, ShellModeZeroForceAtRest) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; + MjDataPtr d = MakeData(m); - mj_forward(m, d); + mj_forward(m.get(), d.get()); // all spring forces should be zero at rest for (int i = 0; i < m->nv; i++) { EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10) << "nonzero spring force at DOF " << i; } - - mj_deleteData(d); - mj_deleteModel(m); } // interpolated shell bending must produce zero spring forces at rest @@ -901,16 +838,16 @@ TEST_F(ElasticityTest, InterpBendingZeroForceAtRest) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; + MjDataPtr d = MakeData(m); // verify bending data was compiled const mjtNum* bdata = m->flex_bending + m->flex_bendingadr[0]; int nedge = (int)bdata[0]; EXPECT_GT(nedge, 0) << "no bending edges compiled"; - mj_forward(m, d); + mj_forward(m.get(), d.get()); // all spring forces should be zero at rest for (int i = 0; i < m->nv; i++) { @@ -944,9 +881,6 @@ TEST_F(ElasticityTest, InterpBendingZeroForceAtRest) { EXPECT_GT(n_flat, 0) << "no intra-surface edges found"; EXPECT_GT(n_corner, 0) << "no corner edges found"; EXPECT_EQ(n_flat + n_corner, nedge); - - mj_deleteData(d); - mj_deleteModel(m); } // interpolated shell bending must produce zero forces after a rigid rotation @@ -967,21 +901,23 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; + MjDataPtr d = MakeData(m); // compute geometric center from body positions (skip world body) mjtNum center[3] = {0, 0, 0}; int nnodes = 0; for (int b = 1; b < m->nbody; b++) { - center[0] += m->body_pos[3*b + 0]; - center[1] += m->body_pos[3*b + 1]; - center[2] += m->body_pos[3*b + 2]; + center[0] += m->body_pos[3 * b + 0]; + center[1] += m->body_pos[3 * b + 1]; + center[2] += m->body_pos[3 * b + 2]; nnodes++; } ASSERT_GT(nnodes, 0); - center[0] /= nnodes; center[1] /= nnodes; center[2] /= nnodes; + center[0] /= nnodes; + center[1] /= nnodes; + center[2] /= nnodes; // rotation: 45 degrees about (1,1,1)/sqrt(3) mjtNum angle = 45 * 3.14159265358979 / 180.0; @@ -995,9 +931,9 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) { // new_pos = center + R * (body_pos - center) // qpos = new_pos - body_pos for (int b = 1; b < m->nbody; b++) { - mjtNum rel[3] = {m->body_pos[3*b+0] - center[0], - m->body_pos[3*b+1] - center[1], - m->body_pos[3*b+2] - center[2]}; + mjtNum rel[3] = {m->body_pos[3 * b + 0] - center[0], + m->body_pos[3 * b + 1] - center[1], + m->body_pos[3 * b + 2] - center[2]}; mjtNum rotated[3]; mju_rotVecQuat(rotated, rel, neg_quat); @@ -1007,7 +943,10 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) { int qadr = m->jnt_qposadr[jid]; int axis = -1; for (int a = 0; a < 3; a++) { - if (m->jnt_axis[3*jid + a] != 0) { axis = a; break; } + if (m->jnt_axis[3 * jid + a] != 0) { + axis = a; + break; + } } if (axis >= 0) { d->qpos[qadr] = @@ -1016,7 +955,7 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) { } } - mj_forward(m, d); + mj_forward(m.get(), d.get()); // spring forces should still be zero after rigid rotation const mjtNum tol = MjTol(1e-6, 1e-3); @@ -1024,9 +963,6 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) { EXPECT_NEAR(d->qfrc_spring[i], 0, tol) << "nonzero spring force at DOF " << i << " after rigid rotation"; } - - mj_deleteData(d); - mj_deleteModel(m); } } // namespace diff --git a/test/engine/engine_plugin_test.cc b/test/engine/engine_plugin_test.cc index f03b965f..4299c39a 100644 --- a/test/engine/engine_plugin_test.cc +++ b/test/engine/engine_plugin_test.cc @@ -68,13 +68,9 @@ class BaseTestPlugin { advance_count = 0; } - void Compute() { - compute_count += stride; - } + void Compute() { compute_count += stride; } - void Advance() { - advance_count += stride; - } + void Advance() { advance_count += stride; } protected: int stride; @@ -458,11 +454,12 @@ TEST_F(MujocoTest, EmptyPluginDisallowed) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, testing::IsNull()) << error.data(); - EXPECT_THAT(error.data(), HasSubstr( - "neither 'plugin' nor 'instance' is specified for body 'world'")); - mj_deleteModel(m); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), testing::IsNull()) << error.data(); + EXPECT_THAT( + error.data(), + HasSubstr( + "neither 'plugin' nor 'instance' is specified for body 'world'")); } TEST_F(MujocoTest, FirstPartyPlugins) { @@ -480,13 +477,13 @@ TEST_F(EnginePluginTest, NoAttributePlugin) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, testing::NotNull()) << error.data(); - mj_deleteModel(m); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), testing::NotNull()) << error.data(); } TEST_F(EnginePluginTest, MultiplePluginTableBlocks) { - EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins); + EXPECT_EQ(mjp_pluginCount(), + kNumTruePlugins + kNumFakePlugins + kNumTestPlugins); const mjpPlugin* last_plugin = nullptr; int table_count = 0; @@ -494,7 +491,7 @@ TEST_F(EnginePluginTest, MultiplePluginTableBlocks) { int slot; std::string name = absl::StrFormat("mujoco.test.fake%u", i); const mjpPlugin* plugin = mjp_getPlugin(name.c_str(), &slot); - EXPECT_EQ(slot, kNumTruePlugins+i); + EXPECT_EQ(slot, kNumTruePlugins + i); EXPECT_THAT(plugin, NotNull()); if (plugin - last_plugin != 1) { ++table_count; @@ -513,16 +510,17 @@ TEST_F(EnginePluginTest, RegisterIdenticalPlugin) { EXPECT_EQ(RegisterSensorPlugin(), kNumTruePlugins); EXPECT_EQ(RegisterActuatorPlugin(), kNumTruePlugins + kNumFakePlugins + 1); EXPECT_EQ(RegisterPassivePlugin(), kNumTruePlugins + kNumFakePlugins + 2); - EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins); + EXPECT_EQ(mjp_pluginCount(), + kNumTruePlugins + kNumFakePlugins + kNumTestPlugins); } TEST_F(EnginePluginTest, SaveXml) { char error[1024] = {0}; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; - std::string saved_xml = SaveAndReadXml(m); + std::string saved_xml = SaveAndReadXml(m.get()); std::string_view expected_xml(xml); const std::string extension_open = ""; @@ -561,12 +559,9 @@ TEST_F(EnginePluginTest, SaveXml) { EXPECT_THAT(saved_xml, HasSubstr(expected_sensor_section)); EXPECT_THAT(saved_xml, HasSubstr(expected_actuator_section)); - mj_deleteModel(m); - // make sure that the saved XML can still be compiled - mjModel* m2 = LoadModelFromString(saved_xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; - mj_deleteModel(m2); + MjModelPtr m2 = LoadModelFromString(saved_xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; } TEST_F(EnginePluginTest, SensorPlugin) { @@ -575,8 +570,8 @@ TEST_F(EnginePluginTest, SensorPlugin) { EXPECT_EQ(expected_init_count, expected_destroy_count); char error[1024] = {0}; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; // mj_makeModel calls mj_makeData and mj_deleteData internally expected_init_count += 3; @@ -585,10 +580,10 @@ TEST_F(EnginePluginTest, SensorPlugin) { EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count); EXPECT_EQ(m->nplugin, 7); - EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "twosensors"), 0); - EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "threesensors"), 1); + EXPECT_EQ(mj_name2id(m.get(), mjOBJ_PLUGIN, "twosensors"), 0); + EXPECT_EQ(mj_name2id(m.get(), mjOBJ_PLUGIN, "threesensors"), 1); - mjData* d = mj_makeData(m); + MjDataPtr d = MakeData(m); expected_init_count += 3; EXPECT_EQ(TestSensor::InitCount(), expected_init_count); EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count); @@ -597,32 +592,30 @@ TEST_F(EnginePluginTest, SensorPlugin) { for (int j = 0; j < 10; ++j) { EXPECT_THAT(*reinterpret_cast(d->plugin_state + m->plugin_stateadr[0]), - testing::ElementsAreArray({i+1, 2*j, j})); + testing::ElementsAreArray({i + 1, 2 * j, j})); EXPECT_THAT(*reinterpret_cast(d->plugin_state + m->plugin_stateadr[1]), - testing::ElementsAreArray({3*(i+1), 6*j, 3*j})); + testing::ElementsAreArray({3 * (i + 1), 6 * j, 3 * j})); EXPECT_THAT(*reinterpret_cast(d->plugin_state + m->plugin_stateadr[5]), - testing::ElementsAreArray({5*(i+1), 10*j, 5*j})); + testing::ElementsAreArray({5 * (i + 1), 10 * j, 5 * j})); EXPECT_THAT(*reinterpret_cast(d->sensordata), - testing::ElementsAreArray({ i+1, 2*j, j, - 5*(i+1), 10*j, 5*j, - 3*(i+1), 6*j, 3*j, - i+1, 2*j, j, - 3*(i+1), 6*j, 3*j, - 3*(i+1), 6*j, 3*j})); - mj_step(m, d); - mj_forward(m, d); + testing::ElementsAreArray( + {i + 1, 2 * j, j, 5 * (i + 1), 10 * j, 5 * j, 3 * (i + 1), + 6 * j, 3 * j, i + 1, 2 * j, j, 3 * (i + 1), 6 * j, 3 * j, + 3 * (i + 1), 6 * j, 3 * j})); + mj_step(m.get(), d.get()); + mj_forward(m.get(), d.get()); } - mj_resetData(m, d); + mj_resetData(m.get(), d.get()); } - mj_deleteData(d); + d.reset(); + m.reset(); + expected_destroy_count += 3; EXPECT_EQ(TestSensor::InitCount(), expected_init_count); EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count); - - mj_deleteModel(m); EXPECT_EQ(TestSensor::InitCount(), expected_init_count); EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count); } @@ -633,8 +626,8 @@ TEST_F(EnginePluginTest, ActuatorPlugin) { EXPECT_EQ(expected_init_count, expected_destroy_count); char error[1024] = {0}; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; // mj_makeModel calls mj_makeData and mj_deleteData internally expected_init_count += 2; @@ -643,9 +636,9 @@ TEST_F(EnginePluginTest, ActuatorPlugin) { EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count); EXPECT_EQ(m->nplugin, 7); - EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "actuator2"), 2); + EXPECT_EQ(mj_name2id(m.get(), mjOBJ_PLUGIN, "actuator2"), 2); - mjData* d = mj_makeData(m); + MjDataPtr d = MakeData(m); expected_init_count += 2; EXPECT_EQ(TestActuator::InitCount(), expected_init_count); EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count); @@ -654,33 +647,33 @@ TEST_F(EnginePluginTest, ActuatorPlugin) { for (int j = 0; j < 10; ++j) { EXPECT_THAT(*reinterpret_cast(d->plugin_state + m->plugin_stateadr[2]), - testing::ElementsAreArray({2*(i+1), 4*j, 2*j})); + testing::ElementsAreArray({2 * (i + 1), 4 * j, 2 * j})); EXPECT_THAT(*reinterpret_cast(d->plugin_state + m->plugin_stateadr[3]), - testing::ElementsAreArray({4*(i+1), 8*j, 4*j})); + testing::ElementsAreArray({4 * (i + 1), 8 * j, 4 * j})); EXPECT_THAT(*reinterpret_cast(d->actuator_force), testing::ElementsAreArray( - {(mjtNum)0.125*j, (mjtNum)0.25*j})); - mj_step(m, d); - mj_forward(m, d); + {(mjtNum)0.125 * j, (mjtNum)0.25 * j})); + mj_step(m.get(), d.get()); + mj_forward(m.get(), d.get()); } - mj_resetData(m, d); + mj_resetData(m.get(), d.get()); } - mj_deleteData(d); + d.reset(); + m.reset(); + expected_destroy_count += 2; EXPECT_EQ(TestActuator::InitCount(), expected_init_count); EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count); - - mj_deleteModel(m); EXPECT_EQ(TestActuator::InitCount(), expected_init_count); EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count); } TEST_F(EnginePluginTest, FilteredActuatorPlugin) { char error[1024] = {0}; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; // Expecting 7 actuator state variables: 3x2 from actuator2 instances, and 1 // from setting dyntype="filter" on one of the plugin actuators @@ -692,12 +685,12 @@ TEST_F(EnginePluginTest, FilteredActuatorPlugin) { EXPECT_EQ(m->actuator_actadr[1], 0); EXPECT_EQ(m->actuator_actadr[2], 3); - mjData* d = mj_makeData(m); + MjDataPtr d = MakeData(m); EXPECT_EQ(d->act[0], 0.0); mju_fill(d->ctrl, 1, m->nu); // start with nonzero act for the filter d->act[6] = 0.5; - mj_step(m, d); + mj_step(m.get(), d.get()); for (int i = 0; i < 6; ++i) { // act_dot should be computed by the plugin @@ -715,9 +708,6 @@ TEST_F(EnginePluginTest, FilteredActuatorPlugin) { EXPECT_THAT(d->act_dot[6], MjNear(expected_act_dot, 1e-6, 1e-4)); EXPECT_THAT(d->act[6], MjNear(0.5 + expected_act_dot * m->opt.timestep, 1e-6, 1e-4)); - - mj_deleteData(d); - mj_deleteModel(m); } } // namespace diff --git a/test/engine/engine_print_test.cc b/test/engine/engine_print_test.cc index 2fd4dbcc..27b0711d 100644 --- a/test/engine/engine_print_test.cc +++ b/test/engine/engine_print_test.cc @@ -24,7 +24,7 @@ #include "test/fixture.h" #ifdef MEMORY_SANITIZER - #include +#include #endif namespace mujoco { @@ -38,7 +38,6 @@ using ::testing::NotNull; using EnginePrintTest = MujocoTest; - constexpr const char* NullFile() { #ifdef _WIN32 return "NUL"; @@ -60,27 +59,26 @@ TEST_F(EnginePrintTest, PrintDataWorksWithMsan) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) + << "Failed to load model: " << error.data(); + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); // Mark qacc_smooth[0] as initialized. data->qacc_smooth[0] = 0; // This will read "uninitialized" values from mjData, but shouldn't fail. - mj_printData(model, data, NullFile()); + mj_printData(model.get(), data.get(), NullFile()); // After mj_printData, poisoned status should be restored correctly, so // qacc_smooth[0] should be marked initialzed. EXPECT_EQ(data->qacc_smooth[0], 0); #ifdef MEMORY_SANITIZER - EXPECT_THAT(__msan_test_shadow(data->buffer, data->nbuffer), Not(Eq(-1))) << - "Expecting some of data->buffer to be marked uninitialized"; + EXPECT_THAT(__msan_test_shadow(data->buffer, data->nbuffer), Not(Eq(-1))) + << "Expecting some of data->buffer to be marked uninitialized"; #endif - mj_deleteData(data); - mj_deleteModel(model); } } // namespace diff --git a/test/engine/engine_ray_test.cc b/test/engine/engine_ray_test.cc index 0078c7db..d9deea1b 100644 --- a/test/engine/engine_ray_test.cc +++ b/test/engine/engine_ray_test.cc @@ -14,6 +14,8 @@ // Tests for ray casting. +#include "src/engine/engine_ray.h" + #include #include @@ -23,7 +25,6 @@ #include #include #include -#include "src/engine/engine_ray.h" #include "test/fixture.h" namespace mujoco { @@ -85,9 +86,10 @@ using RayTest = MujocoTest; TEST_F(RayTest, NoExclusions) { char error[1024]; - mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = + LoadModelFromString(kRayCastingModel, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); mjtNum pnt[] = {0.0, 0.0, 0.0}; @@ -97,20 +99,19 @@ TEST_F(RayTest, NoExclusions) { int bodyexclude = -1; int geomid = -1; - mj_kinematics(model, data); - mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, - bodyexclude, &geomid, nullptr); - EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1"); + mj_kinematics(model.get(), data.get()); + mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, + flg_static, bodyexclude, &geomid, nullptr); + EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "static_group1"); EXPECT_MJTNUM_EQ(distance, 0.9); - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(RayTest, Exclusions) { char error[1024]; - mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = + LoadModelFromString(kRayCastingModel, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); mjtNum pnt[] = {0.0, 0.0, 0.0}; @@ -120,40 +121,38 @@ TEST_F(RayTest, Exclusions) { int bodyexclude = -1; int geomid = -1; - mj_kinematics(model, data); - mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, - bodyexclude, &geomid, nullptr); - EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1"); + mj_kinematics(model.get(), data.get()); + mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, + flg_static, bodyexclude, &geomid, nullptr); + EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "static_group1"); EXPECT_NEAR(distance, 0.9, MjTol(1e-12, 1e-5)); // Exclude nearest geom geomgroup[1] = 0; - distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude, - &geomid, nullptr); - EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0"); + distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static, + bodyexclude, &geomid, nullptr); + EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group0"); EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5)); geomgroup[0] = 0; - distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude, - &geomid, nullptr); - EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group2"); + distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static, + bodyexclude, &geomid, nullptr); + EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group2"); EXPECT_NEAR(distance, 4.9, MjTol(1e-12, 1e-5)); geomgroup[2] = 0; - distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude, - &geomid, nullptr); + distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static, + bodyexclude, &geomid, nullptr); EXPECT_EQ(geomid, -1); EXPECT_NEAR(distance, -1, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(RayTest, ExcludeStatic) { char error[1024]; - mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = + LoadModelFromString(kRayCastingModel, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); mjtNum pnt[] = {0.0, 0.0, 0.0}; @@ -163,38 +162,36 @@ TEST_F(RayTest, ExcludeStatic) { int bodyexclude = -1; int geomid = -1; - mj_kinematics(model, data); - mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, - bodyexclude, &geomid, nullptr); - EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0"); + mj_kinematics(model.get(), data.get()); + mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, + flg_static, bodyexclude, &geomid, nullptr); + EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group0"); EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5)); - mj_deleteData(data); - mj_deleteModel(model); } // ------------------------------- mj_multiRay -------------------------------- TEST_F(RayTest, MultiRayEqualsSingleRay) { char error[1024]; - mjModel* m = LoadModelFromString(kRayCastingModel, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(kRayCastingModel, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); ASSERT_THAT(d, NotNull()); - mj_forward(m, d); + mj_forward(m.get(), d.get()); // create ray array constexpr int N = 80; constexpr int M = 60; - mjtNum vec[3*N*M]; + mjtNum vec[3 * N * M]; mjtNum pnt[3] = {-1, 0, 0}; mjtNum cone[4][3] = {{1, .2, -.2}, {1, .2, .2}, {1, -.2, -.2}, {1, -.2, .2}}; - memset(vec, 0, 3*N*M*sizeof(mjtNum)); + memset(vec, 0, 3 * N * M * sizeof(mjtNum)); for (int i = 0; i < N; ++i) { for (int j = 0; j < M; ++j) { for (int k = 0; k < 3; ++k) { - vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) + - j * cone[1][1] / (M - 1) + + vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) + + j * cone[1][1] / (M - 1) + (N - i - 1) * cone[2][k] / (N - 1) + (M - j - 1) * cone[3][k] / (M - 1); } @@ -202,10 +199,10 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) { } // compute intersections with multiray functions - mjtNum dist_multiray[N*M]; - int rgeomid_multiray[N*M]; - mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, dist_multiray, - nullptr, N * M, mjMAXVAL); + mjtNum dist_multiray[N * M]; + int rgeomid_multiray[N * M]; + mj_multiRay(m.get(), d.get(), pnt, vec, NULL, 1, -1, rgeomid_multiray, + dist_multiray, nullptr, N * M, mjMAXVAL); // compare results with single ray function mjtNum dist; @@ -214,39 +211,37 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) { for (int i = 0; i < N; ++i) { for (int j = 0; j < M; ++j) { int idx = i * M + j; - dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid, nullptr); + dist = mj_ray(m.get(), d.get(), pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid, + nullptr); EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]); EXPECT_EQ(rgeomid, rgeomid_multiray[idx]); nhits += dist >= 0; } } EXPECT_GT(nhits, 10); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) { char error[1024]; - mjModel* m = LoadModelFromString(kRayCastingModel, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(kRayCastingModel, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); ASSERT_THAT(d, NotNull()); - mj_forward(m, d); + mj_forward(m.get(), d.get()); // create ray array constexpr int N = 80; constexpr int M = 60; - mjtNum vec[3*N*M]; + mjtNum vec[3 * N * M]; mjtNum pnt[3] = {-1, 0, 0}; mjtNum cone[4][3] = {{1, .2, -.2}, {1, .2, .2}, {1, -.2, -.2}, {1, -.2, .2}}; - memset(vec, 0, 3*N*M*sizeof(mjtNum)); + memset(vec, 0, 3 * N * M * sizeof(mjtNum)); for (int i = 0; i < N; ++i) { for (int j = 0; j < M; ++j) { for (int k = 0; k < 3; ++k) { - vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) + - j * cone[1][1] / (M - 1) + + vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) + + j * cone[1][1] / (M - 1) + (N - i - 1) * cone[2][k] / (N - 1) + (M - j - 1) * cone[3][k] / (M - 1); } @@ -254,10 +249,10 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) { } // compute intersections with multiray normal function - mjtNum dist_multiray[N*M]; - int rgeomid_multiray[N*M]; - mjtNum normal_multiray[3*N*M]; - mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, + mjtNum dist_multiray[N * M]; + int rgeomid_multiray[N * M]; + mjtNum normal_multiray[3 * N * M]; + mj_multiRay(m.get(), d.get(), pnt, vec, NULL, 1, -1, rgeomid_multiray, dist_multiray, normal_multiray, N * M, mjMAXVAL); // compare results with single ray normal function @@ -268,30 +263,27 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) { for (int i = 0; i < N; ++i) { for (int j = 0; j < M; ++j) { int idx = i * M + j; - dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid, + dist = mj_ray(m.get(), d.get(), pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid, normal); EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]); EXPECT_EQ(rgeomid, rgeomid_multiray[idx]); - EXPECT_MJTNUM_EQ(normal[0], normal_multiray[3*idx]); - EXPECT_MJTNUM_EQ(normal[1], normal_multiray[3*idx + 1]); - EXPECT_MJTNUM_EQ(normal[2], normal_multiray[3*idx + 2]); + EXPECT_MJTNUM_EQ(normal[0], normal_multiray[3 * idx]); + EXPECT_MJTNUM_EQ(normal[1], normal_multiray[3 * idx + 1]); + EXPECT_MJTNUM_EQ(normal[2], normal_multiray[3 * idx + 2]); nhits += dist >= 0; } } EXPECT_GT(nhits, 10); - - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(RayTest, EdgeCases) { char error[1024]; - mjModel* m = LoadModelFromString(kSingleGeomModel, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + MjModelPtr m = LoadModelFromString(kSingleGeomModel, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; ASSERT_THAT(m->nbvh, 1); - mjData* d = mj_makeData(m); + MjDataPtr d = MakeData(m); ASSERT_THAT(d, NotNull()); - mj_forward(m, d); + mj_forward(m.get(), d.get()); // spherical bounding box and result arrays mjtNum geom_ba[4]; @@ -301,51 +293,57 @@ TEST_F(RayTest, EdgeCases) { // pnt contained in bounding box mjtNum pnt1[] = {-1, 0, 0}; - mju_multiRayPrepare(m, d, pnt1, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags); + mju_multiRayPrepare(m.get(), d.get(), pnt1, NULL, NULL, 1, -1, mjMAXVAL, + geom_ba, flags); EXPECT_MJTNUM_EQ(geom_ba[0], -mjPI); - EXPECT_MJTNUM_EQ(geom_ba[1], 0); - EXPECT_MJTNUM_EQ(geom_ba[2], mjPI); - EXPECT_MJTNUM_EQ(geom_ba[3], mjPI); + EXPECT_MJTNUM_EQ(geom_ba[1], 0); + EXPECT_MJTNUM_EQ(geom_ba[2], mjPI); + EXPECT_MJTNUM_EQ(geom_ba[3], mjPI); mjtNum vec1[] = {1, 0, 0}; - mj_multiRay(m, d, pnt1, vec1, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, - mjMAXVAL); + mj_multiRay(m.get(), d.get(), pnt1, vec1, NULL, 1, -1, &rgeomid, &dist, + nullptr, 1, mjMAXVAL); EXPECT_FLOAT_EQ(dist, 0.1); // pnt at phi = Pi, -Pi mjtNum pnt2[] = {-.5, 0, 0}; - mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags); + mju_multiRayPrepare(m.get(), d.get(), pnt2, NULL, NULL, 1, -1, mjMAXVAL, + geom_ba, flags); EXPECT_FLOAT_EQ(geom_ba[0], -mjPI); // atan(y<0, x<0) - EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0) + EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0) mjtNum vec2[] = {-1, 0, 0}; - mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, - mjMAXVAL); + mj_multiRay(m.get(), d.get(), pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, + nullptr, 1, mjMAXVAL); EXPECT_FLOAT_EQ(dist, 0.4); // with cutoff mjtNum cutoff1 = 0.41, cutoff2 = 0.39; - mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, cutoff1, geom_ba, flags); + mju_multiRayPrepare(m.get(), d.get(), pnt2, NULL, NULL, 1, -1, cutoff1, + geom_ba, flags); EXPECT_EQ(flags[0], 0); - mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, cutoff2, geom_ba, flags); + mju_multiRayPrepare(m.get(), d.get(), pnt2, NULL, NULL, 1, -1, cutoff2, + geom_ba, flags); EXPECT_EQ(flags[0], 1); - mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, - cutoff2); + mj_multiRay(m.get(), d.get(), pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, + nullptr, 1, cutoff2); EXPECT_FLOAT_EQ(dist, -1); // pnt on the boundary of the box mjtNum pnt3[] = {.1, .1, .05}; - mju_multiRayPrepare(m, d, pnt3, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags); + mju_multiRayPrepare(m.get(), d.get(), pnt3, NULL, NULL, 1, -1, mjMAXVAL, + geom_ba, flags); EXPECT_FLOAT_EQ(geom_ba[1], 0); EXPECT_FLOAT_EQ(geom_ba[3], mjPI); mjtNum vec3[] = {1, 1, 0}; - mj_multiRay(m, d, pnt3, vec3, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, - mjMAXVAL); + mj_multiRay(m.get(), d.get(), pnt3, vec3, NULL, 1, -1, &rgeomid, &dist, + nullptr, 1, mjMAXVAL); EXPECT_FLOAT_EQ(dist, -1); // size 0 geom mjtNum pnt4[] = {-2, 0, 0}; m->geom_aabb[0] = m->geom_aabb[1] = m->geom_aabb[2] = 0; m->geom_aabb[3] = m->geom_aabb[4] = m->geom_aabb[5] = 0; - mju_multiRayPrepare(m, d, pnt4, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags); + mju_multiRayPrepare(m.get(), d.get(), pnt4, NULL, NULL, 1, -1, mjMAXVAL, + geom_ba, flags); // margin = atan(max_half / dist) where max_half = max(aabb[3..5]) // For a zero-size AABB: max_half = 0, so margin = 0 mjtNum dist4 = mju_dist3(pnt4, d->geom_xpos); @@ -353,16 +351,13 @@ TEST_F(RayTest, EdgeCases) { mju_max(m->geom_aabb[3], mju_max(m->geom_aabb[4], m->geom_aabb[5])); mjtNum margin4 = mju_atan2(max_half4, dist4); EXPECT_NEAR(geom_ba[0], 0 - margin4, 1e-6); - EXPECT_NEAR(geom_ba[1], mjPI/2 - margin4, 1e-6); + EXPECT_NEAR(geom_ba[1], mjPI / 2 - margin4, 1e-6); EXPECT_NEAR(geom_ba[2], 0 + margin4, 1e-6); - EXPECT_NEAR(geom_ba[3], mjPI/2 + margin4, 1e-6); + EXPECT_NEAR(geom_ba[3], mjPI / 2 + margin4, 1e-6); mjtNum vec4[] = {1, 0, 0}; - mj_multiRay(m, d, pnt4, vec4, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, - mjMAXVAL); + mj_multiRay(m.get(), d.get(), pnt4, vec4, NULL, 1, -1, &rgeomid, &dist, + nullptr, 1, mjMAXVAL); EXPECT_FLOAT_EQ(dist, 0.9); - - mj_deleteData(d); - mj_deleteModel(m); } // ------------------------------- mj_rayMesh --------------------------------- @@ -377,10 +372,10 @@ mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid, // map to local frame mjtNum lpnt[3], lvec[3]; - const mjtNum* pos = d->geom_xpos+3*geomid; - const mjtNum dif[3] = {pnt[0]-pos[0], pnt[1]-pos[1], pnt[2]-pos[2]}; - mju_mulMatTVec3(lpnt, d->geom_xmat+9*geomid, dif); - mju_mulMatTVec3(lvec, d->geom_xmat+9*geomid, vec); + const mjtNum* pos = d->geom_xpos + 3 * geomid; + const mjtNum dif[3] = {pnt[0] - pos[0], pnt[1] - pos[1], pnt[2] - pos[2]}; + mju_mulMatTVec3(lpnt, d->geom_xmat + 9 * geomid, dif); + mju_mulMatTVec3(lvec, d->geom_xmat + 9 * geomid, vec); // construct basis vectors of normal plane mjtNum b0[3] = {1, 1, 1}, b1[3]; @@ -392,7 +387,7 @@ mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid, } else { b0[2] = 0; } - mju_addScl3(b1, b0, lvec, -mju_dot3(lvec, b0)/mju_dot3(lvec, lvec)); + mju_addScl3(b1, b0, lvec, -mju_dot3(lvec, b0) / mju_dot3(lvec, lvec)); mju_normalize3(b1); mju_cross(b0, b1, lvec); mju_normalize3(b0); @@ -403,18 +398,20 @@ mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid, // process all triangles int face, meshid = m->geom_dataid[geomid]; for (face = m->mesh_faceadr[meshid]; - face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid]; - face++) { + face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid]; face++) { // get float vertices float* vf[3]; - vf[0] = m->mesh_vert + 3*(m->mesh_face[3*face] + m->mesh_vertadr[meshid]); - vf[1] = m->mesh_vert + 3*(m->mesh_face[3*face+1] + m->mesh_vertadr[meshid]); - vf[2] = m->mesh_vert + 3*(m->mesh_face[3*face+2] + m->mesh_vertadr[meshid]); + vf[0] = + m->mesh_vert + 3 * (m->mesh_face[3 * face] + m->mesh_vertadr[meshid]); + vf[1] = m->mesh_vert + + 3 * (m->mesh_face[3 * face + 1] + m->mesh_vertadr[meshid]); + vf[2] = m->mesh_vert + + 3 * (m->mesh_face[3 * face + 2] + m->mesh_vertadr[meshid]); // convert to mjtNum mjtNum v[3][3]; - for (int i=0; i < 3; i++) { - for (int j=0; j < 3; j++) { + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { v[i][j] = (mjtNum)vf[i][j]; } } @@ -440,16 +437,16 @@ void _rayMeshTest(const mjModel* m) { // create ray array constexpr int N = 80; constexpr int M = 60; - mjtNum vec[3*N*M]; + mjtNum vec[3 * N * M]; mjtNum pnt[3] = {1, .2, 0}; mjtNum cone[4][3] = {{-1, -1, -1}, {-1, -1, 1}, {1, -1, 1}, {1, -1, -1}}; - memset(vec, 0, 3*N*M*sizeof(mjtNum)); + memset(vec, 0, 3 * N * M * sizeof(mjtNum)); for (int i = 0; i < N; ++i) { for (int j = 0; j < M; ++j) { for (int k = 0; k < 3; ++k) { - vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) + - j * cone[1][1] / (M - 1) + + vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) + + j * cone[1][1] / (M - 1) + (N - i - 1) * cone[2][k] / (N - 1) + (M - j - 1) * cone[3][k] / (M - 1); } @@ -466,7 +463,6 @@ void _rayMeshTest(const mjModel* m) { EXPECT_FLOAT_EQ(dist_new, dist_old); } } - mj_deleteData(d); } @@ -478,15 +474,13 @@ TEST_F(RayTest, RayMeshPruning) { const string xml_path = GetTestDataFilePath("engine/testdata/ray/stanford_bunny.xml"); - mjModel* m = mj_loadXML(xml_path.c_str(), NULL, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - _rayMeshTest(m); - mj_deleteModel(m); + MjModelPtr m(mj_loadXML(xml_path.c_str(), NULL, error, sizeof(error))); + ASSERT_THAT(m.get(), NotNull()) << error; + _rayMeshTest(m.get()); m = LoadModelFromString(kCubeletModel, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - _rayMeshTest(m); - mj_deleteModel(m); + ASSERT_THAT(m.get(), NotNull()) << error; + _rayMeshTest(m.get()); } TEST_F(RayTest, RayHfield) { @@ -523,21 +517,17 @@ TEST_F(RayTest, RayHfield) { )"; - char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_THAT(data->sensordata[0], MjNear(1, 1e-8, 1e-5)); EXPECT_THAT(data->sensordata[1], MjNear(1, 1e-8, 1e-5)); EXPECT_THAT(data->sensordata[2], MjNear(1, 1e-8, 1e-5)); EXPECT_THAT(data->sensordata[3], MjNear(0.5, 1e-8, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } static const char* const kPlaneModel = "engine/testdata/ray/plane.xml"; @@ -603,8 +593,8 @@ TEST_F(RayTest, RayNormal) { } else { r = mj_rayFlex(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1, /*flg_edge*/ 1, /*flg_face*/ 1, - /*flg_skin*/ 1, /*flex_id*/ 0, - pnt, vec, nullptr, normal); + /*flg_skin*/ 1, /*flex_id*/ 0, pnt, vec, nullptr, + normal); // no sensor comparison: rangefinders only intersect with geoms } @@ -623,8 +613,7 @@ TEST_F(RayTest, RayNormal) { mjtNum eps = 1e-6; mjtNum ds[2][3]; for (int i = 0; i < 2; ++i) { - mjtNum nudge[3] = {d->site_xmat[0 + i], - d->site_xmat[3 + i], + mjtNum nudge[3] = {d->site_xmat[0 + i], d->site_xmat[3 + i], d->site_xmat[6 + i]}; mjtNum dr, dpnt[3]; mju_addScl3(dpnt, pnt, nudge, eps); @@ -650,8 +639,9 @@ TEST_F(RayTest, RayNormal) { mjtNum expected_neg[3] = {-expected[0], -expected[1], -expected[2]}; // compare analytic with fin-diff approximation - EXPECT_THAT(normal, AnyOf(Pointwise(MjNear(100*eps, 1e-3), expected), - Pointwise(MjNear(100*eps, 1e-3), expected_neg))) + EXPECT_THAT(normal, + AnyOf(Pointwise(MjNear(100 * eps, 1e-3), expected), + Pointwise(MjNear(100 * eps, 1e-3), expected_neg))) << path << ", time " << d->time; // increment count diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index 076f1c5f..c2c10fbf 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -35,7 +35,6 @@ namespace { using ::std::string; using ::std::vector; - using ::testing::ElementsAre; using ::testing::ElementsAreArray; using ::testing::HasSubstr; @@ -50,16 +49,16 @@ using ::testing::WhenSorted; const mjtNum tol = 1e-14; // nearness tolerance for floating point numbers // returns as a vector the measured values from sensor with index `id` -static vector GetSensor(const mjModel* model, - const mjData* data, int id) { +static vector GetSensor(const mjModel* model, const mjData* data, + int id) { return vector( data->sensordata + model->sensor_adr[id], data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]); } // returns as a vector the measured values from sensor with name `name -static vector GetSensor(const mjModel* model, - const mjData* data, const char* name) { +static vector GetSensor(const mjModel* model, const mjData* data, + const char* name) { int id = mj_name2id(model, mjOBJ_SENSOR, name); return vector( data->sensordata + model->sensor_adr[id], @@ -79,34 +78,31 @@ TEST_F(SensorTest, DisableSensors) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // call mj_forward, expect clock to report 0 - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_EQ(data->sensordata[0], 0.0); // call mj_step, mj_step1, expect clock to be incremented by timestep - mj_step(model, data); - mj_step1(model, data); + mj_step(model.get(), data.get()); + mj_step1(model.get(), data.get()); EXPECT_EQ(data->sensordata[0], model->opt.timestep); // disable sensors, call mj_step, mj_step1, expect clock to not increment model->opt.disableflags |= mjDSBL_SENSOR; - mj_step(model, data); - mj_step1(model, data); - EXPECT_EQ(data->time, 2*model->opt.timestep); + mj_step(model.get(), data.get()); + mj_step1(model.get(), data.get()); + EXPECT_EQ(data->time, 2 * model->opt.timestep); EXPECT_EQ(data->sensordata[0], model->opt.timestep); // re-enable sensors, call mj_step, mj_step1, expect clock to match time model->opt.disableflags = 0; - mj_step(model, data); - mj_step1(model, data); + mj_step(model.get(), data.get()); + mj_step1(model.get(), data.get()); EXPECT_EQ(data->time, data->sensordata[0]); - - mj_deleteData(data); - mj_deleteModel(model); } // --------------------- test relative frame sensors -------------------------- @@ -132,23 +128,20 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMat) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); - mj_forward(model, data); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mj_forward(model.get(), data.get()); // compare actual and expected values - vector pos = GetSensor(model, data, 0); + vector pos = GetSensor(model.get(), data.get(), 0); EXPECT_THAT(pos, Pointwise(MjNear(tol, 1e-6), {5, 5, 0})); - vector xaxis = GetSensor(model, data, 1); + vector xaxis = GetSensor(model.get(), data.get(), 1); EXPECT_THAT(xaxis, Pointwise(MjNear(tol, 1e-6), {0, -1, 0})); - vector yaxis = GetSensor(model, data, 2); + vector yaxis = GetSensor(model.get(), data.get(), 2); EXPECT_THAT(yaxis, Pointwise(MjNear(tol, 1e-6), {1, 0, 0})); - - mj_deleteData(data); - mj_deleteModel(model); } // orientations given by quaternion and by orientation matrix are identical @@ -171,21 +164,18 @@ TEST_F(RelativeFrameSensorTest, ReferenceQuatMat) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); // call mj_forward and convert orientation matrix to quaternion - mj_forward(model, data); + mj_forward(model.get(), data.get()); mjtNum mat[9], converted_quat[4]; mju_transpose(mat, data->sensordata, 3, 3); mju_mat2Quat(converted_quat, mat); // compare quaternion sensor and quat derived from orientation matrix - vector quat = GetSensor(model, data, 3); + vector quat = GetSensor(model.get(), data.get(), 3); EXPECT_THAT(quat, Pointwise(MjNear(tol, 1e-6), converted_quat)); - - mj_deleteData(data); - mj_deleteModel(model); } // compare global frame and initially co-located relative frame on same body @@ -218,30 +208,27 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) { )"; - mjModel* model = LoadModelFromString(xml); + MjModelPtr model = LoadModelFromString(xml); constexpr int nsensordata = 32; ASSERT_EQ(model->nsensordata, nsensordata); - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); // call mj_forward, save global sensors (colocated with reference frame) - mj_forward(model, data); - vector expected_values(data->sensordata, data->sensordata+nsensordata/2); + mj_forward(model.get(), data.get()); + vector expected_values(data->sensordata, data->sensordata + nsensordata / 2); // set qpos to arbitrary values, call mj_forward - for (int i=0; i < 7; i++) { - data->qpos[i] = i+1; + for (int i = 0; i < 7; i++) { + data->qpos[i] = i + 1; } - mj_forward(model, data); + mj_forward(model.get(), data.get()); // get values from relative sensors after moving the object - vector actual_values(data->sensordata+nsensordata/2, - data->sensordata+nsensordata); + vector actual_values(data->sensordata + nsensordata / 2, + data->sensordata + nsensordata); // object and reference have moved together, we expect values to not change EXPECT_THAT(actual_values, Pointwise(MjNear(tol, 1e-6), expected_values)); - - mj_deleteData(data); - mj_deleteModel(model); } // hand-picked velocities and orientations for simple expected values @@ -264,19 +251,16 @@ TEST_F(RelativeFrameSensorTest, FrameVelLinearFixed) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); data->qvel[0] = mju_sqrt(2); data->qvel[1] = 1; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // compare to expected values - vector linvel = GetSensor(model, data, 0); + vector linvel = GetSensor(model.get(), data.get(), 0); const mjtNum expected_linvel[3] = {-mju_sqrt(0.5), mju_sqrt(0.5), 0}; EXPECT_THAT(linvel, Pointwise(MjNear(tol, 1e-6), expected_linvel)); - - mj_deleteData(data); - mj_deleteModel(model); } // object and reference in the same body, expect angular velocities to be zero @@ -296,19 +280,16 @@ TEST_F(RelativeFrameSensorTest, FrameVelAngFixed) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); // set joint velocities and call forward dynamics data->qvel[0] = 1; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // obj and ref rotate together, relative angular velocities should be zero - vector angvel = GetSensor(model, data, 0); + vector angvel = GetSensor(model.get(), data.get(), 0); EXPECT_THAT(angvel, Pointwise(MjNear(tol, 1e-6), {0, 0, 0})); - - mj_deleteData(data); - mj_deleteModel(model); } // object and reference rotate on the same global axis @@ -331,21 +312,18 @@ TEST_F(RelativeFrameSensorTest, FrameVelAngOpposing) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); // set joint velocities and call forward dynamics data->qvel[0] = -1; data->qvel[1] = 1; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // obj and ref rotate on same axis, we can just difference the velocities - vector angvel = GetSensor(model, data, 0); - const mjtNum expected_angvel[3] = {0, data->qvel[1]-data->qvel[0], 0}; + vector angvel = GetSensor(model.get(), data.get(), 0); + const mjtNum expected_angvel[3] = {0, data->qvel[1] - data->qvel[0], 0}; EXPECT_THAT(angvel, Pointwise(MjNear(tol, 1e-6), expected_angvel)); - - mj_deleteData(data); - mj_deleteModel(model); } // two arbitrary frames, compare velocity sensors and fin-diffed positions @@ -374,37 +352,37 @@ TEST_F(RelativeFrameSensorTest, FrameVelGeneral) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); mjtNum dt = 1e-6; // timestep used for finite differencing // set (arbitrary) joint velocities and call forward dynamics data->qvel[0] = 1; data->qvel[1] = -1; - mj_forward(model, data); + mj_forward(model.get(), data.get()); // save measured linear and angular velocities as vectors - vector linvel = GetSensor(model, data, 2); - vector angvel = GetSensor(model, data, 3); + vector linvel = GetSensor(model.get(), data.get(), 2); + vector angvel = GetSensor(model.get(), data.get(), 3); // save current position, quaternion as arrays mjtNum pos0[3], quat0[4]; mju_copy3(pos0, data->sensordata); - mju_copy4(quat0, data->sensordata+3); + mju_copy4(quat0, data->sensordata + 3); // explicit Euler integration with small dt mju_addToScl(data->qpos, data->qvel, dt, 2); // call mj_forward again, save new position and quaternion - mj_forward(model, data); + mj_forward(model.get(), data.get()); mjtNum pos1[3], quat1[4]; mju_copy3(pos1, data->sensordata); - mju_copy4(quat1, data->sensordata+3); + mju_copy4(quat1, data->sensordata + 3); // compute expected linear velocities using finite differencing mjtNum linvel_findiff[3]; mju_sub3(linvel_findiff, pos1, pos0); - mju_scl3(linvel_findiff, linvel_findiff, 1/dt); + mju_scl3(linvel_findiff, linvel_findiff, 1 / dt); // compute expected angular velocities using finite differencing mjtNum dquat[4], angvel_findiff[3]; @@ -413,13 +391,8 @@ TEST_F(RelativeFrameSensorTest, FrameVelGeneral) { mju_quat2Vel(angvel_findiff, dquat, dt); // compare analytic and finite-differenced relative velocities - EXPECT_THAT(linvel, Pointwise(MjNear(10 * dt, 0.15), - linvel_findiff)); - EXPECT_THAT(angvel, - Pointwise(MjNear(10 * dt, 0.2), angvel_findiff)); - - mj_deleteData(data); - mj_deleteModel(model); + EXPECT_THAT(linvel, Pointwise(MjNear(10 * dt, 0.15), linvel_findiff)); + EXPECT_THAT(angvel, Pointwise(MjNear(10 * dt, 0.2), angvel_findiff)); } // ------------------------- general sensor tests ----------------------------- @@ -439,18 +412,15 @@ TEST_F(SensorTest, EnableEnergy) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); - mj_forward(model, data); - EXPECT_NEAR(data->energy[0], 2*3*5, MjTol(1e-12, 1e-5)); + mj_forward(model.get(), data.get()); + EXPECT_NEAR(data->energy[0], 2 * 3 * 5, MjTol(1e-12, 1e-5)); model->opt.enableflags &= ~mjENBL_ENERGY; - mj_forward(model, data); + mj_forward(model.get(), data.get()); EXPECT_NEAR(data->energy[0], 0, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SensorTest, PotentialEnergy) { @@ -468,18 +438,15 @@ TEST_F(SensorTest, PotentialEnergy) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); - mj_forward(model, data); - EXPECT_NEAR(data->sensordata[0], 2*3*5, MjTol(1e-12, 1e-5)); + mj_forward(model.get(), data.get()); + EXPECT_NEAR(data->sensordata[0], 2 * 3 * 5, MjTol(1e-12, 1e-5)); data->qpos[2] = 7; - mj_forward(model, data); - EXPECT_NEAR(data->sensordata[0], 7*3*5, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); + mj_forward(model.get(), data.get()); + EXPECT_NEAR(data->sensordata[0], 7 * 3 * 5, MjTol(1e-12, 1e-5)); } TEST_F(SensorTest, PotentialEnergyFreeJointSpring) { @@ -497,16 +464,13 @@ TEST_F(SensorTest, PotentialEnergyFreeJointSpring) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); data->qpos[0] = 1; data->qpos[1] = 2; data->qpos[2] = 3; - mj_forward(model, data); - EXPECT_NEAR(data->sensordata[0], 0.5*2*14, MjTol(1e-12, 1e-5)); - - mj_deleteData(data); - mj_deleteModel(model); + mj_forward(model.get(), data.get()); + EXPECT_NEAR(data->sensordata[0], 0.5 * 2 * 14, MjTol(1e-12, 1e-5)); } TEST_F(SensorTest, KineticEnergy) { @@ -523,21 +487,18 @@ TEST_F(SensorTest, KineticEnergy) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); while (data->time < 1.5) { - mj_step(model, data); + mj_step(model.get(), data.get()); } - mj_forward(model, data); + mj_forward(model.get(), data.get()); mjtNum mass = 3; mjtNum speed = data->time * mju_norm3(model->opt.gravity); EXPECT_NEAR(data->sensordata[0], 0.5 * mass * speed * speed, MjTol(1e-7, 1e-2)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SensorTest, PolyStiffnessEnergy) { @@ -561,21 +522,18 @@ TEST_F(SensorTest, PolyStiffnessEnergy) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); - mj_resetDataKeyframe(m, d, 0); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); + mj_resetDataKeyframe(m.get(), d.get(), 0); - mj_forward(m, d); + mj_forward(m.get(), d.get()); mjtNum total_energy = d->energy[0] + d->energy[1]; for (int i = 0; i < 100; i++) { - mj_step(m, d); + mj_step(m.get(), d.get()); EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.003); } - - mj_deleteData(d); - mj_deleteModel(m); } // test clock sensor @@ -589,25 +547,23 @@ TEST_F(SensorTest, Clock) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); // call step 4 times, checking that clock works as expected - for (int i=0; i < 5; i++) { - mj_step(model, data); - mj_step1(model, data); // update values of position-based sensors + for (int i = 0; i < 5; i++) { + mj_step(model.get(), data.get()); + mj_step1(model.get(), + data.get()); // update values of position-based sensors EXPECT_EQ(data->sensordata[0], data->time); EXPECT_EQ(data->sensordata[1], mju_min(data->time, 3e-3)); } // check names - const char* name0 = mj_id2name(model, mjOBJ_SENSOR, 0); + const char* name0 = mj_id2name(model.get(), mjOBJ_SENSOR, 0); EXPECT_EQ(name0, nullptr); - const char* name1 = mj_id2name(model, mjOBJ_SENSOR, 1); + const char* name1 = mj_id2name(model.get(), mjOBJ_SENSOR, 1); EXPECT_THAT(name1, StrEq("clampedclock")); - - mj_deleteData(data); - mj_deleteModel(model); } // test that integer parameters pass through @@ -672,8 +628,11 @@ TEST_F(SensorTest, CollisionSequential) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model_ptr = LoadModelFromString(xml); + MjDataPtr data_ptr = MakeData(model_ptr); + + mjModel* model = model_ptr.get(); + mjData* data = data_ptr.get(); mj_forward(model, data); mjtNum eps = 1e-14; @@ -688,14 +647,11 @@ TEST_F(SensorTest, CollisionSequential) { EXPECT_THAT(GetSensor(model, data, 5), Pointwise(MjNear(eps, 1e-7), vector{1, 0, 0})); EXPECT_THAT(GetSensor(model, data, 6), - Pointwise(MjNear(eps, 1e-6), - vector{0, 0, 0, 0, 0, .8})); + Pointwise(MjNear(eps, 1e-6), vector{0, 0, 0, 0, 0, .8})); EXPECT_THAT(GetSensor(model, data, 7), - Pointwise(MjNear(eps, 1e-6), - vector{1, 0, .7, 1, 0, 0})); + Pointwise(MjNear(eps, 1e-6), vector{1, 0, .7, 1, 0, 0})); EXPECT_THAT(GetSensor(model, data, 8), - Pointwise(MjNear(eps, 1e-6), - vector{.2, 0, 1, .7, 0, 1})); + Pointwise(MjNear(eps, 1e-6), vector{.2, 0, 1, .7, 0, 1})); EXPECT_THAT(GetSensor(model, data, 9), Pointwise(MjNear(eps, 1e-7), GetSensor(model, data, 0))); @@ -709,9 +665,6 @@ TEST_F(SensorTest, CollisionSequential) { Pointwise(MjNear(eps, 1e-6), GetSensor(model, data, 8))); EXPECT_THAT(GetSensor(model, data, 14), Pointwise(MjNear(eps, 1e-7), GetSensor(model, data, 2))); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SensorTest, BadContact) { @@ -757,8 +710,8 @@ TEST_F(SensorTest, BadContact) { xml.replace(pos, 8, test.bad_attr); char error[1024]; - mjModel* model = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(model, IsNull()) << "Test case: " << test.bad_attr; + MjModelPtr model = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(model.get(), IsNull()) << "Test case: " << test.bad_attr; EXPECT_THAT(error, HasSubstr(test.expected_error)) << "Test case: " << test.bad_attr; } @@ -820,8 +773,7 @@ TEST_F(SensorTest, Contact) { {3, 0, 0, 0, 0, 1, 4, 0, 0, -1, 0, 0})); vector b2r = GetSensor(model, data, "b2_reduced"); - EXPECT_THAT(b2r, - Pointwise(MjNear(1e-4, 0.02), {4, 0, 0, -1, 0, 0})); + EXPECT_THAT(b2r, Pointwise(MjNear(1e-4, 0.02), {4, 0, 0, -1, 0, 0})); } mj_deleteData(data); @@ -852,9 +804,9 @@ TEST_F(SensorTest, ContactSorted) { EXPECT_THAT(sorted_force, SizeIs(12)); vector nnorms; for (size_t i = 0; i < sorted_force.size(); i += 3) { - nnorms.push_back(-sorted_force[i]*sorted_force[i] + - -sorted_force[i+1]*sorted_force[i+1] + - -sorted_force[i+2]*sorted_force[i+2]); + nnorms.push_back(-sorted_force[i] * sorted_force[i] + + -sorted_force[i + 1] * sorted_force[i + 1] + + -sorted_force[i + 2] * sorted_force[i + 2]); } EXPECT_THAT(nnorms, WhenSorted(ElementsAreArray(nnorms))); @@ -864,9 +816,8 @@ TEST_F(SensorTest, ContactSorted) { vector largest = GetSensor(model, data, "largest force"); EXPECT_THAT(largest, SizeIs(3)); - EXPECT_THAT(largest, ElementsAre(sorted_force[0], - sorted_force[1], - sorted_force[2])); + EXPECT_THAT(largest, + ElementsAre(sorted_force[0], sorted_force[1], sorted_force[2])); mj_deleteData(data); mj_deleteModel(model); @@ -921,12 +872,12 @@ TEST_F(SensorTest, ContactSubtreePartial) { mj_step(model, data); } - EXPECT_EQ(GetSensor(model, data, "all")[0], 4); - EXPECT_EQ(GetSensor(model, data, "world")[0], 4); - EXPECT_EQ(GetSensor(model, data, "thigh")[0], 4); - EXPECT_EQ(GetSensor(model, data, "shin")[0], 2); - EXPECT_EQ(GetSensor(model, data, "foot")[0], 1); - EXPECT_EQ(GetSensor(model, data, "foot_w")[0], 0); + EXPECT_EQ(GetSensor(model, data, "all")[0], 4); + EXPECT_EQ(GetSensor(model, data, "world")[0], 4); + EXPECT_EQ(GetSensor(model, data, "thigh")[0], 4); + EXPECT_EQ(GetSensor(model, data, "shin")[0], 2); + EXPECT_EQ(GetSensor(model, data, "foot")[0], 1); + EXPECT_EQ(GetSensor(model, data, "foot_w")[0], 0); EXPECT_EQ(GetSensor(model, data, "foot_w2")[0], 1); mj_deleteData(data); @@ -1028,12 +979,12 @@ TEST_F(SensorTest, CameraProjection) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); // call step to update sensors - mj_step(model, data); - mj_step1(model, data); // update values of position-based sensors + mj_step(model.get(), data.get()); + mj_step1(model.get(), data.get()); // update values of position-based sensors EXPECT_THAT(model->cam_resolution[0], 1920); EXPECT_THAT(model->cam_resolution[1], 1200); mjtNum eps = 1e-4; @@ -1043,9 +994,6 @@ TEST_F(SensorTest, CameraProjection) { EXPECT_NEAR(data->sensordata[3], 1200, eps); EXPECT_NEAR(data->sensordata[4], 960, eps); EXPECT_NEAR(data->sensordata[5], 600, eps); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SensorTest, InsideSite) { @@ -1074,22 +1022,19 @@ TEST_F(SensorTest, InsideSite) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; ASSERT_EQ(model->nsensordata, 5); - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); mjtNum hpos[5] = {-.5, -.25, 0, .25, .5}; for (int i = 0; i < 5; i++) { data->qpos[0] = hpos[i]; - mj_forward(model, data); + mj_forward(model.get(), data.get()); vector expected(5, 0.0); expected[i] = 1.0; EXPECT_EQ(AsVector(data->sensordata, model->nsensordata), expected); } - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SensorTest, RangefinderCamera) { @@ -1111,15 +1056,15 @@ TEST_F(SensorTest, RangefinderCamera) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; // first sensor: data="dist depth" => (1+1)*9 = 18 // second sensor: data="dist dir origin point" => (1+3+3+3)*9 = 90 EXPECT_EQ(model->nsensordata, 108); - mjData* data = mj_makeData(model); - mj_forward(model, data); + MjDataPtr data = MakeData(model); + mj_forward(model.get(), data.get()); mjtNum tol = 1e-6; mjtNum height = 2.0; @@ -1134,13 +1079,13 @@ TEST_F(SensorTest, RangefinderCamera) { int idx = row * 3 + col; mjtNum dx = offsets[col] / fy; mjtNum dy = offsets[row] / fy; - mjtNum expected_dist = height * mju_sqrt(1 + dx*dx + dy*dy); - mjtNum dist = data->sensordata[adr0 + idx*stride0]; + mjtNum expected_dist = height * mju_sqrt(1 + dx * dx + dy * dy); + mjtNum dist = data->sensordata[adr0 + idx * stride0]; EXPECT_NEAR(dist, expected_dist, tol) << "perspective dist pixel (" << row << ", " << col << ")"; // depth should equal camera height (2.0) for all pixels - mjtNum depth = data->sensordata[adr0 + idx*stride0 + 1]; + mjtNum depth = data->sensordata[adr0 + idx * stride0 + 1]; EXPECT_NEAR(depth, height, tol) << "perspective depth pixel (" << row << ", " << col << ")"; } @@ -1167,14 +1112,14 @@ TEST_F(SensorTest, RangefinderCamera) { // ray hits z=0 plane: distance = origin_z / cos45 mjtNum expected_dist = origin_z / cos45; - mjtNum dist = data->sensordata[adr1 + idx*stride1]; + mjtNum dist = data->sensordata[adr1 + idx * stride1]; EXPECT_NEAR(dist, expected_dist, tol) << "orthographic dist pixel (" << row << ", " << col << ")"; // verify point = origin + dir * dist - mjtNum* dir = data->sensordata + adr1 + idx*stride1 + 1; - mjtNum* origin = data->sensordata + adr1 + idx*stride1 + 4; - mjtNum* point = data->sensordata + adr1 + idx*stride1 + 7; + mjtNum* dir = data->sensordata + adr1 + idx * stride1 + 1; + mjtNum* origin = data->sensordata + adr1 + idx * stride1 + 4; + mjtNum* point = data->sensordata + adr1 + idx * stride1 + 7; mjtNum expected_point[3]; mju_addScl3(expected_point, origin, dir, dist); EXPECT_NEAR(point[0], expected_point[0], tol) @@ -1185,9 +1130,6 @@ TEST_F(SensorTest, RangefinderCamera) { << "ortho point[2] pixel (" << row << ", " << col << ")"; } } - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SensorTest, RFCamera) { @@ -1210,9 +1152,9 @@ TEST_F(SensorTest, RFCamera) { for (int s = 0; s < 2; s++) { int adr = model->sensor_adr[s]; for (int i = 0; i < 16; i++) { - mjtNum dist = data->sensordata[adr + i*stride]; - mjtNum* point = data->sensordata + adr + i*stride + 1; - mjtNum* normal = data->sensordata + adr + i*stride + 4; + mjtNum dist = data->sensordata[adr + i * stride]; + mjtNum* point = data->sensordata + adr + i * stride + 1; + mjtNum* normal = data->sensordata + adr + i * stride + 4; EXPECT_TRUE(dist > 0 || dist == -1) << "sensor " << s << " pixel " << i; @@ -1251,9 +1193,9 @@ TEST_F(SensorTest, SensorDelay) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // delay = 0.02 seconds, timestep = 0.01 // history = 3 (more than delay/timestep=2) to ensure buffer coverage @@ -1266,31 +1208,28 @@ TEST_F(SensorTest, SensorDelay) { // step 0: qpos=10, read from initial buffer data->qpos[0] = 10.0; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step 0"; // step 1: qpos=20, still reading initial buffer data->qpos[0] = 20.0; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step 1"; // step 2: qpos=30, read value from step 0 (delay=2 steps) data->qpos[0] = 30.0; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 10.0, 1e-10) << "step 2"; // step 3: qpos=40, read value from step 1 (delay=2 steps) data->qpos[0] = 40.0; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 20.0, 1e-10) << "step 3"; // step 4: qpos=50, read value from step 2 (delay=2 steps) data->qpos[0] = 50.0; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 30.0, 1e-10) << "step 4"; - - mj_deleteData(data); - mj_deleteModel(model); } // Test sensor delay with linear interpolation (interp=1) @@ -1311,9 +1250,9 @@ TEST_F(SensorTest, SensorDelayLinearInterp) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // delay = 0.015 seconds = 1.5*timestep, nsample=3, interp=1 (linear) // With linear interpolation and 1.5*timestep delay, the read time falls @@ -1324,32 +1263,29 @@ TEST_F(SensorTest, SensorDelayLinearInterp) { // Set increasing qpos values: step i -> qpos = (i+1)*10 // Buffer has samples at times: -0.02, -0.01, 0 (initialized) - // After step 0 at time=0.01: buffer has times -0.01, 0, 0.01 with values 0, 0, 10 - // Read at time 0.01 - 0.015 = -0.005: interpolate between t=-0.01 (val=0) and t=0 (val=0) - // Expected: 0 * 0.5 + 0 * 0.5 = 0 + // After step 0 at time=0.01: buffer has times -0.01, 0, 0.01 with values 0, + // 0, 10 Read at time 0.01 - 0.015 = -0.005: interpolate between t=-0.01 + // (val=0) and t=0 (val=0) Expected: 0 * 0.5 + 0 * 0.5 = 0 data->qpos[0] = 10.0; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step 0"; - // After step 1 at time=0.02: buffer has times 0, 0.01, 0.02 with values 0, 10, 20 - // Read at time 0.02 - 0.015 = 0.005: interpolate between t=0 (val=0) and t=0.01 (val=10) - // Expected: 0 * 0.5 + 10 * 0.5 = 5 + // After step 1 at time=0.02: buffer has times 0, 0.01, 0.02 with values 0, + // 10, 20 Read at time 0.02 - 0.015 = 0.005: interpolate between t=0 (val=0) + // and t=0.01 (val=10) Expected: 0 * 0.5 + 10 * 0.5 = 5 data->qpos[0] = 20.0; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 5.0, 1e-10) << "step 1"; - // After step 2 at time=0.03: buffer has times 0.01, 0.02, 0.03 with values 10, 20, 30 - // Read at 0.03 - 0.015 = 0.015: interpolate between t=0.01 (val=10) and t=0.02 (val=20) - // Expected: 10 * 0.5 + 20 * 0.5 = 15 + // After step 2 at time=0.03: buffer has times 0.01, 0.02, 0.03 with values + // 10, 20, 30 Read at 0.03 - 0.015 = 0.015: interpolate between t=0.01 + // (val=10) and t=0.02 (val=20) Expected: 10 * 0.5 + 20 * 0.5 = 15 data->qpos[0] = 30.0; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 15.0, 1e-10) << "step 2"; - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SensorTest, SensorInterval) { @@ -1372,12 +1308,12 @@ TEST_F(SensorTest, SensorInterval) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); - int sensor0 = mj_name2id(model, mjOBJ_SENSOR, "default_phase"); - int sensor1 = mj_name2id(model, mjOBJ_SENSOR, "offset_phase"); + int sensor0 = mj_name2id(model.get(), mjOBJ_SENSOR, "default_phase"); + int sensor1 = mj_name2id(model.get(), mjOBJ_SENSOR, "offset_phase"); int adr0 = model->sensor_adr[sensor0]; int adr1 = model->sensor_adr[sensor1]; @@ -1387,8 +1323,8 @@ TEST_F(SensorTest, SensorInterval) { // rounded up to dt: -2, -5, -7, -10, -12, -15, -17, -20, -22, -25 // sensor1 (phase = -1.5): continuous times are -1.5, -4, -6.5, ... // rounded up to dt: -1, -4, -6, -9, -11, -14, -16, -19, -21, -24 - int n0 = model->sensor_history[2*sensor0]; - int n1 = model->sensor_history[2*sensor1]; + int n0 = model->sensor_history[2 * sensor0]; + int n1 = model->sensor_history[2 * sensor1]; mjtNum* buf0 = data->history + model->sensor_historyadr[sensor0]; mjtNum* buf1 = data->history + model->sensor_historyadr[sensor1]; mjtNum* times0 = buf0 + 2; @@ -1416,7 +1352,7 @@ TEST_F(SensorTest, SensorInterval) { for (int t = 0; t < 15; t++) { // set position to current time (so we can track when sensor was computed) data->qpos[0] = t; - mj_step(model, data); + mj_step(model.get(), data.get()); // update expected values based on trigger pattern if (triggers0[t]) value0 = t; @@ -1427,9 +1363,6 @@ TEST_F(SensorTest, SensorInterval) { EXPECT_NEAR(data->sensordata[adr1], value1, MjTol(1e-10, 1e-7)) << "sensor1 at t=" << t; } - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SensorTest, SensorDelayInterval) { @@ -1448,14 +1381,14 @@ TEST_F(SensorTest, SensorDelayInterval) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // Combined delay and interval EXPECT_EQ(model->sensor_history[0], 5); EXPECT_NEAR(model->sensor_delay[0], 0.02, MjTol(1e-10, 1e-7)); - EXPECT_NEAR(model->sensor_interval[2*0], 0.03, MjTol(1e-10, 1e-7)); + EXPECT_NEAR(model->sensor_interval[2 * 0], 0.03, MjTol(1e-10, 1e-7)); // Verify initial buffer timestamps (after mj_makeData/mj_resetData) // With period=0.03, dt=0.01, nsample=5, phase=0 (means -period=-0.03): @@ -1477,17 +1410,14 @@ TEST_F(SensorTest, SensorDelayInterval) { // - At t=0, interval satisfied: compute 5.0, insert at t=0 (current time) // - Reading happens at d->time - delay; at t=0.02, reads at t=0.00 (5.0) for (int i = 0; i < 2; i++) { - mj_step(model, data); + mj_step(model.get(), data.get()); // sensor reads delayed value (0.0 from initial buffer) EXPECT_NEAR(data->sensordata[0], 0.0, MjTol(1e-10, 1e-7)) << "step " << i; } // step 3 (i=2): reading at t=0.00 now returns the inserted value 5.0 - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 5.0, MjTol(1e-10, 1e-7)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SensorTest, SensorHistoryOnly) { @@ -1506,29 +1436,26 @@ TEST_F(SensorTest, SensorHistoryOnly) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // history only, no delay or interval EXPECT_EQ(model->sensor_history[0], 5); EXPECT_NEAR(model->sensor_delay[0], 0.0, MjTol(1e-10, 1e-7)); - EXPECT_NEAR(model->sensor_interval[2*0], 0.0, MjTol(1e-10, 1e-7)); + EXPECT_NEAR(model->sensor_interval[2 * 0], 0.0, MjTol(1e-10, 1e-7)); // set position data->qpos[0] = 3.0; // without delay, sensordata reflects current value immediately - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 3.0, 1e-10); // change position, check again data->qpos[0] = 7.0; - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 7.0, 1e-10); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SensorTest, SensorDelayMultiDim) { @@ -1547,9 +1474,9 @@ TEST_F(SensorTest, SensorDelayMultiDim) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // ballangvel is 3D EXPECT_EQ(model->sensor_dim[0], 3); @@ -1561,20 +1488,17 @@ TEST_F(SensorTest, SensorDelayMultiDim) { data->qvel[2] = 3.0; // step: reading delayed value (initially 0) - mj_step(model, data); + mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10); EXPECT_NEAR(data->sensordata[1], 0.0, 1e-10); EXPECT_NEAR(data->sensordata[2], 0.0, 1e-10); // after delay, values should propagate - mj_step(model, data); - mj_step(model, data); - mj_step(model, data); + mj_step(model.get(), data.get()); + mj_step(model.get(), data.get()); + mj_step(model.get(), data.get()); // angular velocity is affected by dynamics, just check the buffer works EXPECT_THAT(AsVector(data->sensordata, 3), Not(ElementsAre(0.0, 0.0, 0.0))); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SensorTest, ReadSensor) { @@ -1593,20 +1517,20 @@ TEST_F(SensorTest, ReadSensor) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // step with different qpos values to populate buffer // mj_advance inserts at current time, then time advances data->qpos[0] = 1.0; - mj_step(model, data); // inserts 1.0 at t=0, time -> 0.01 + mj_step(model.get(), data.get()); // inserts 1.0 at t=0, time -> 0.01 data->qpos[0] = 2.0; - mj_step(model, data); // inserts 2.0 at t=0.01, time -> 0.02 + mj_step(model.get(), data.get()); // inserts 2.0 at t=0.01, time -> 0.02 data->qpos[0] = 3.0; - mj_step(model, data); // inserts 3.0 at t=0.02, time -> 0.03 + mj_step(model.get(), data.get()); // inserts 3.0 at t=0.02, time -> 0.03 // now time=0.03, buffer has: [t=0: 1.0, t=0.01: 2.0, t=0.02: 3.0] @@ -1615,19 +1539,16 @@ TEST_F(SensorTest, ReadSensor) { const mjtNum* ptr; // read at t=0 -> returns 1.0 - ptr = mj_readSensor(model, data, 0, 0.0, result, /*order=*/0); + ptr = mj_readSensor(model.get(), data.get(), 0, 0.0, result, /*order=*/0); EXPECT_NEAR(*ptr, 1.0, 1e-10); // read at t=0.01 -> returns 2.0 (ZOH: exactly at insertion time) - ptr = mj_readSensor(model, data, 0, 0.01, result, /*order=*/0); + ptr = mj_readSensor(model.get(), data.get(), 0, 0.01, result, /*order=*/0); EXPECT_NEAR(*ptr, 2.0, 1e-10); // read at t=0.02 -> returns 3.0 - ptr = mj_readSensor(model, data, 0, 0.02, result, /*order=*/0); + ptr = mj_readSensor(model.get(), data.get(), 0, 0.02, result, /*order=*/0); EXPECT_NEAR(*ptr, 3.0, 1e-10); - - mj_deleteData(data); - mj_deleteModel(model); } // mj_sensorAcc returns correct accelerometer after mj_step1 (issue #3133) @@ -1647,22 +1568,19 @@ TEST_F(SensorTest, AccelerometerAfterStep1) { )"; - mjModel* model = LoadModelFromString(xml); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + MjDataPtr data = MakeData(model); // settle the simulation with split-step loop for (int i = 0; i < 100; i++) { - mj_step1(model, data); - mj_step2(model, data); + mj_step1(model.get(), data.get()); + mj_step2(model.get(), data.get()); } // call mj_step1 + mj_sensorAcc, expect gravity reading (~9.81 m/s^2) - mj_step1(model, data); - mj_sensorAcc(model, data); + mj_step1(model.get(), data.get()); + mj_sensorAcc(model.get(), data.get()); EXPECT_NEAR(data->sensordata[2], 9.81, 1e-2); - - mj_deleteData(data); - mj_deleteModel(model); } // Test tactile sensor reads non-zero values when contacts occur @@ -1690,13 +1608,13 @@ TEST_F(SensorTest, TactileSkipTangents) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; ASSERT_GT(model->nsensordata, 0) << "No sensor data allocated"; - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); // Use mj_forward to compute collisions and sensors at t=0 - mj_forward(model, data); + mj_forward(model.get(), data.get()); // Verify initial state EXPECT_EQ(data->time, 0.0); @@ -1722,12 +1640,8 @@ TEST_F(SensorTest, TactileSkipTangents) { } } EXPECT_EQ(nonzero_count, 2) << "Expected 2 taxels in contact"; - - mj_deleteData(data); - mj_deleteModel(model); } - // insidesite uses subtree_com for massless flex parent bodies TEST_F(SensorTest, InsideSiteFlexBody) { static constexpr char xml[] = R"( @@ -1755,18 +1669,18 @@ TEST_F(SensorTest, InsideSiteFlexBody) { char error[1024] = {0}; EXPECT_CALL(mock_warning_handler, Warn(testing::HasSubstr("is not rigid"))) .WillOnce(testing::Return()); - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); // flex is at origin, site is a large box at origin — should be inside - mj_forward(m, d); + mj_forward(m.get(), d.get()); EXPECT_EQ(d->sensordata[0], 1) << "flex body should be inside the container site"; // shift all vertex/node bodies far outside the site via qpos // each body has 3 slide joints (x, y, z); shift z by +10 - int parent_id = mj_name2id(m, mjOBJ_BODY, "parent"); + int parent_id = mj_name2id(m.get(), mjOBJ_BODY, "parent"); for (int b = parent_id + 1; b < m->nbody; b++) { if (m->body_parentid[b] == parent_id) { int jadr = m->body_jntadr[b]; @@ -1776,14 +1690,11 @@ TEST_F(SensorTest, InsideSiteFlexBody) { } } } - mj_forward(m, d); + mj_forward(m.get(), d.get()); // subtree_com should now be far outside; sensor should read 0 EXPECT_EQ(d->sensordata[0], 0) << "flex body should be outside the container site after displacement"; - - mj_deleteData(d); - mj_deleteModel(m); } // Test that a tactile sensor's compile-time body-collision check correctly @@ -1809,10 +1720,8 @@ TEST_F(SensorTest, TactileMeshIdMismatchedValidator) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - - mj_deleteModel(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; } // Test that contact and touch sensors work with Flex contacts. @@ -1843,32 +1752,30 @@ TEST_F(SensorTest, FlexContactSensors) { char error[1024] = {0}; EXPECT_CALL(mock_warning_handler, Warn(testing::HasSubstr("is not rigid"))) .WillOnce(testing::Return()); - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); - mj_forward(m, d); + mj_forward(m.get(), d.get()); // We expect at least one contact between the flex and the floor ASSERT_GT(d->ncon, 0) << "No contacts generated"; // Check the contact sensor "flex_contact_subtree" - vector flex_contact_subtree = GetSensor(m, d, "flex_contact_subtree"); + vector flex_contact_subtree = + GetSensor(m.get(), d.get(), "flex_contact_subtree"); EXPECT_GT(flex_contact_subtree[0], 0) << "Flex contact sensor (subtree) did not detect any contacts"; // Check the contact sensor "flex_contact_body" - vector flex_contact_body = GetSensor(m, d, "flex_contact_body"); + vector flex_contact_body = GetSensor(m.get(), d.get(), "flex_contact_body"); EXPECT_EQ(flex_contact_body[0], 0) << "Flex contact sensor (body) should not match contacts on child bodies"; // Check the touch sensor "floor_touch" - vector floor_touch = GetSensor(m, d, "floor_touch"); + vector floor_touch = GetSensor(m.get(), d.get(), "floor_touch"); EXPECT_GT(floor_touch[0], 0.0) << "Floor touch sensor did not detect any force"; - - mj_deleteData(d); - mj_deleteModel(m); } } // namespace diff --git a/test/engine/engine_setconst_test.cc b/test/engine/engine_setconst_test.cc index 6daf6bd2..b53174b6 100644 --- a/test/engine/engine_setconst_test.cc +++ b/test/engine/engine_setconst_test.cc @@ -53,7 +53,7 @@ TEST_F(SetConstTest, AwakeActuatedJoint) { )"; char error[1024]; - mjModel* m; + MjModelPtr m; string sleep[] = {"auto", "never", "allowed", "init"}; int tsp0[] = {mjSLEEP_AUTO_NEVER, mjSLEEP_NEVER, mjSLEEP_ALLOWED, @@ -69,10 +69,9 @@ TEST_F(SetConstTest, AwakeActuatedJoint) { size_t pos2 = xml_copy.find("POLICY2"); xml_copy.replace(pos2, 7, sleep[j]); m = LoadModelFromString(xml_copy.c_str(), error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + ASSERT_THAT(m.get(), NotNull()) << error; EXPECT_EQ(m->tree_sleep_policy[0], tsp0[i]); EXPECT_EQ(m->tree_sleep_policy[1], tsp1[j]); - mj_deleteModel(m); } } } @@ -97,13 +96,11 @@ TEST_F(SetConstTest, AwakeActuatedSite) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); - - mj_deleteModel(model); } TEST_F(SetConstTest, AwakeActuatedBody) { @@ -125,13 +122,11 @@ TEST_F(SetConstTest, AwakeActuatedBody) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); - - mj_deleteModel(model); } TEST_F(SetConstTest, AwakeActuatedTendon) { @@ -161,13 +156,11 @@ TEST_F(SetConstTest, AwakeActuatedTendon) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); - - mj_deleteModel(model); } TEST_F(SetConstTest, AwakeStiffTendonMultiTree) { @@ -194,13 +187,11 @@ TEST_F(SetConstTest, AwakeStiffTendonMultiTree) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_NEVER); - - mj_deleteModel(model); } TEST_F(SetConstTest, SleepyTendonSingleTree) { @@ -227,12 +218,10 @@ TEST_F(SetConstTest, SleepyTendonSingleTree) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED); - - mj_deleteModel(model); } TEST_F(SetConstTest, SleepyTendonZeroStiffness) { @@ -259,13 +248,11 @@ TEST_F(SetConstTest, SleepyTendonZeroStiffness) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); - - mj_deleteModel(model); } TEST_F(SetConstTest, TendonTreeId) { @@ -336,16 +323,16 @@ TEST_F(SetConstTest, TendonTreeId) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; - int t_static_id = mj_name2id(model, mjOBJ_TENDON, "T_static"); - int t_tree1_id = mj_name2id(model, mjOBJ_TENDON, "T_tree1"); - int t_intertree12_id = mj_name2id(model, mjOBJ_TENDON, "T_intertree12"); - int t_intertree123_id = mj_name2id(model, mjOBJ_TENDON, "T_intertree123"); + int t_static_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_static"); + int t_tree1_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_tree1"); + int t_intertree12_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_intertree12"); + int t_intertree123_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_intertree123"); - int b1_1_treeid = model->body_treeid[mj_name2id(model, mjOBJ_BODY, "B1_1")]; - int b2_1_treeid = model->body_treeid[mj_name2id(model, mjOBJ_BODY, "B2_1")]; + int b1_1_treeid = model->body_treeid[mj_name2id(model.get(), mjOBJ_BODY, "B1_1")]; + int b2_1_treeid = model->body_treeid[mj_name2id(model.get(), mjOBJ_BODY, "B2_1")]; // Tendon 1: Not associated with any tree EXPECT_EQ(model->tendon_treenum[t_static_id], 0); @@ -368,8 +355,6 @@ TEST_F(SetConstTest, TendonTreeId) { EXPECT_EQ(model->tendon_treeid[2*t_intertree123_id], b1_1_treeid); EXPECT_EQ(model->tendon_treeid[2*t_intertree123_id+1], b2_1_treeid); // The third tree ID is not stored in tendon_treeid - - mj_deleteModel(model); } TEST_F(SetConstTest, SleepingNotAllowed) { @@ -415,8 +400,8 @@ TEST_F(SetConstTest, SleepingNotAllowed) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - EXPECT_THAT(model, IsNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + EXPECT_THAT(model.get(), IsNull()) << error; EXPECT_THAT(string(error), HasSubstr( "tree 1 connected to tendon 0 which spans more than 2 trees, " "sleeping not allowed")); @@ -447,8 +432,8 @@ TEST_F(SetConstTest, DofLength) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; mjtNum tol = 1e-5; @@ -470,8 +455,6 @@ TEST_F(SetConstTest, DofLength) { EXPECT_NEAR(model->dof_length[8], 5, tol); EXPECT_NEAR(model->dof_length[9], 5, tol); EXPECT_NEAR(model->dof_length[10], 5, tol); - - mj_deleteModel(model); } } // namespace diff --git a/test/engine/engine_sleep_test.cc b/test/engine/engine_sleep_test.cc index 6cce34e0..98858a76 100644 --- a/test/engine/engine_sleep_test.cc +++ b/test/engine/engine_sleep_test.cc @@ -14,26 +14,27 @@ // Tests for engine/engine_sleep.c. +#include "src/engine/engine_sleep.h" + #include #include #include -#include #include +#include #include #include -#include "src/engine/engine_sleep.h" #include "test/fixture.h" namespace mujoco { namespace { -using ::testing::ElementsAre; -using ::testing::IsNull; -using ::testing::HasSubstr; -using ::testing::NotNull; using ::std::string; using ::std::vector; +using ::testing::ElementsAre; +using ::testing::HasSubstr; +using ::testing::IsNull; +using ::testing::NotNull; using SleepTest = MujocoTest; @@ -72,13 +73,13 @@ static constexpr char kSimple[] = R"( )"; -static constexpr int kAwake = -(1+mjMINAWAKE); +static constexpr int kAwake = -(1 + mjMINAWAKE); TEST_F(SleepTest, MjSleepUpdate) { char error[1024]; - mjModel* m = LoadModelFromString(kSimple, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(kSimple, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); // ntree = 2, nbody = 5, nv = 5, njnt = 3, ngeom = 6 // body 0: world, 1 geom @@ -93,96 +94,69 @@ TEST_F(SleepTest, MjSleepUpdate) { EXPECT_EQ(m->njnt, 3); EXPECT_EQ(m->ngeom, 6); - EXPECT_THAT(AsVector(m->body_treeid, m->nbody), - ElementsAre(-1, 0, -1, 1, 1)); - EXPECT_THAT(AsVector(m->dof_bodyid, m->nv), - ElementsAre(1, 1, 1, 3, 4)); + EXPECT_THAT(AsVector(m->body_treeid, m->nbody), ElementsAre(-1, 0, -1, 1, 1)); + EXPECT_THAT(AsVector(m->dof_bodyid, m->nv), ElementsAre(1, 1, 1, 3, 4)); EXPECT_THAT(AsVector(m->geom_bodyid, m->ngeom), ElementsAre(0, 1, 2, 3, 3, 4)); - EXPECT_THAT(AsVector(m->jnt_bodyid, m->njnt), - ElementsAre(1, 3, 4)); + EXPECT_THAT(AsVector(m->jnt_bodyid, m->njnt), ElementsAre(1, 3, 4)); // Test Case 1: Initial state - EXPECT_THAT(AsVector(d->tree_asleep, m->ntree), - ElementsAre(kAwake, kAwake)); + EXPECT_THAT(AsVector(d->tree_asleep, m->ntree), ElementsAre(kAwake, kAwake)); EXPECT_EQ(d->ntree_awake, 2); EXPECT_EQ(d->nv_awake, 5); EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre(0, 1, 2, 3, 4)); - EXPECT_THAT(AsVector(d->tree_awake, m->ntree), - ElementsAre(1, 1)); - EXPECT_THAT(AsVector(d->body_awake, m->nbody), - ElementsAre(mjS_STATIC, - mjS_AWAKE, - mjS_STATIC, - mjS_AWAKE, - mjS_AWAKE)); + EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(1, 1)); + EXPECT_THAT( + AsVector(d->body_awake, m->nbody), + ElementsAre(mjS_STATIC, mjS_AWAKE, mjS_STATIC, mjS_AWAKE, mjS_AWAKE)); // Test Case 2: Call mj_sleepUpdate, expect no changes - mj_updateSleep(m, d); + mj_updateSleep(m.get(), d.get()); EXPECT_EQ(d->ntree_awake, 2); EXPECT_EQ(d->nv_awake, 5); EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre(0, 1, 2, 3, 4)); - EXPECT_THAT(AsVector(d->body_awake, m->nbody), - ElementsAre(mjS_STATIC, - mjS_AWAKE, - mjS_STATIC, - mjS_AWAKE, - mjS_AWAKE)); - EXPECT_THAT(AsVector(d->tree_awake, m->ntree), - ElementsAre(1, 1)); + EXPECT_THAT( + AsVector(d->body_awake, m->nbody), + ElementsAre(mjS_STATIC, mjS_AWAKE, mjS_STATIC, mjS_AWAKE, mjS_AWAKE)); + EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(1, 1)); // Test Case 3: Tree 0 asleep - d->tree_asleep[0] = 0; d->tree_asleep[1] = -1; - mj_updateSleep(m, d); + d->tree_asleep[0] = 0; + d->tree_asleep[1] = -1; + mj_updateSleep(m.get(), d.get()); EXPECT_EQ(d->ntree_awake, 1); EXPECT_EQ(d->nv_awake, 2); - EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), - ElementsAre(3, 4)); - EXPECT_THAT(AsVector(d->tree_awake, m->ntree), - ElementsAre(0, 1)); - EXPECT_THAT(AsVector(d->body_awake, m->nbody), - ElementsAre(mjS_STATIC, - mjS_ASLEEP, - mjS_STATIC, - mjS_AWAKE, - mjS_AWAKE)); + EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre(3, 4)); + EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(0, 1)); + EXPECT_THAT( + AsVector(d->body_awake, m->nbody), + ElementsAre(mjS_STATIC, mjS_ASLEEP, mjS_STATIC, mjS_AWAKE, mjS_AWAKE)); // Test Case 4: Tree 1 asleep - d->tree_asleep[0] = -1; d->tree_asleep[1] = 1; - mj_updateSleep(m, d); + d->tree_asleep[0] = -1; + d->tree_asleep[1] = 1; + mj_updateSleep(m.get(), d.get()); EXPECT_EQ(d->ntree_awake, 1); EXPECT_EQ(d->nv_awake, 3); - EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), - ElementsAre(0, 1, 2)); - EXPECT_THAT(AsVector(d->tree_awake, m->ntree), - ElementsAre(1, 0)); - EXPECT_THAT(AsVector(d->body_awake, m->nbody), - ElementsAre(mjS_STATIC, - mjS_AWAKE, - mjS_STATIC, - mjS_ASLEEP, - mjS_ASLEEP)); + EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre(0, 1, 2)); + EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(1, 0)); + EXPECT_THAT( + AsVector(d->body_awake, m->nbody), + ElementsAre(mjS_STATIC, mjS_AWAKE, mjS_STATIC, mjS_ASLEEP, mjS_ASLEEP)); // Test Case 5: All trees asleep - d->tree_asleep[0] = 0; d->tree_asleep[1] = 1; - mj_updateSleep(m, d); + d->tree_asleep[0] = 0; + d->tree_asleep[1] = 1; + mj_updateSleep(m.get(), d.get()); EXPECT_EQ(d->ntree_awake, 0); EXPECT_EQ(d->nv_awake, 0); - EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), - ElementsAre()); - EXPECT_THAT(AsVector(d->tree_awake, m->ntree), - ElementsAre(0, 0)); - EXPECT_THAT(AsVector(d->body_awake, m->nbody), - ElementsAre(mjS_STATIC, - mjS_ASLEEP, - mjS_STATIC, - mjS_ASLEEP, - mjS_ASLEEP)); - - mj_deleteData(d); - mj_deleteModel(m); + EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre()); + EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(0, 0)); + EXPECT_THAT( + AsVector(d->body_awake, m->nbody), + ElementsAre(mjS_STATIC, mjS_ASLEEP, mjS_STATIC, mjS_ASLEEP, mjS_ASLEEP)); } TEST_F(SleepTest, MjWakeIsland) { @@ -191,34 +165,29 @@ TEST_F(SleepTest, MjWakeIsland) { EXPECT_EQ(mj_wakeIsland(asleep, 4, 0, kAwake, nullptr, 0), 0); EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, 2, 1, 3)); EXPECT_EQ(mj_wakeIsland(asleep, 4, 1, kAwake, nullptr, 0), 2); - EXPECT_THAT(AsVector(asleep, 4), - ElementsAre(kAwake, kAwake, kAwake, 3)); + EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, kAwake, kAwake, 3)); EXPECT_EQ(mj_wakeIsland(asleep, 4, 3, kAwake, nullptr, 0), 1); - EXPECT_THAT(AsVector(asleep, 4), - ElementsAre(kAwake, kAwake, kAwake, kAwake)); + EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, kAwake, kAwake, kAwake)); } TEST_F(SleepTest, BadWakeIsland) { - EXPECT_FATAL_FAILURE( - ([] { - int asleep_bad1[] = {-1, 0}; - mj_wakeIsland(asleep_bad1, 2, 1, kAwake, nullptr, 0); - }()), - "invalid sleep state index -1 when waking tree 1"); + EXPECT_FATAL_FAILURE(([] { + int asleep_bad1[] = {-1, 0}; + mj_wakeIsland(asleep_bad1, 2, 1, kAwake, nullptr, 0); + }()), + "invalid sleep state index -1 when waking tree 1"); - EXPECT_FATAL_FAILURE( - ([] { - int asleep_bad2[] = {-1, 2}; - mj_wakeIsland(asleep_bad2, 2, 1, kAwake, nullptr, 0); - }()), - "invalid sleep state index 2 when waking tree 1"); + EXPECT_FATAL_FAILURE(([] { + int asleep_bad2[] = {-1, 2}; + mj_wakeIsland(asleep_bad2, 2, 1, kAwake, nullptr, 0); + }()), + "invalid sleep state index 2 when waking tree 1"); - EXPECT_FATAL_FAILURE( - ([] { - int asleep_bad3[] = {1, 2, 1}; - mj_wakeIsland(asleep_bad3, 3, 0, kAwake, nullptr, 0); - }()), - "tree 0 is not in a cycle"); + EXPECT_FATAL_FAILURE(([] { + int asleep_bad3[] = {1, 2, 1}; + mj_wakeIsland(asleep_bad3, 3, 0, kAwake, nullptr, 0); + }()), + "tree 0 is not in a cycle"); } static const char* const kStaticModel = "engine/testdata/sleep/static.xml"; @@ -273,7 +242,7 @@ TEST_F(SleepTest, WakingUnaffectedBySleeping) { for (mjtJacobian jacobian : {mjJAC_DENSE, mjJAC_SPARSE}) { m->opt.jacobian = jacobian; for (mjtIntegrator integrator : // TODO: b/457674312 - Add support for RK4. - {mjINT_EULER, mjINT_IMPLICITFAST, mjINT_IMPLICIT}) { + {mjINT_EULER, mjINT_IMPLICITFAST, mjINT_IMPLICIT}) { m->opt.integrator = integrator; // make data with sleeping enabled @@ -361,7 +330,6 @@ TEST_F(SleepTest, WakingUnaffectedBySleeping) { mj_deleteModel(m); } - // Test that waking does not affect sleeping trees for pos/vel-dependent arrays. // Roll out models where some trees wake and/or sleep. At kCompare intervals, // copy the state from the mjData with sleeping enabled to another mjData and @@ -496,8 +464,8 @@ TEST_F(SleepTest, SleepingUnaffectedByWaking) { for (int i = 0; i < m->nsensor; i++) { if (m->nuser_sensor == 1 && m->sensor_user[i] == 1) { - EXPECT_TRUE(sensor_mismatch[i]) - << "contact sensor " << i << " comparison was expected to fail"; + EXPECT_TRUE(sensor_mismatch[i]) + << "contact sensor " << i << " comparison was expected to fail"; } } @@ -555,9 +523,9 @@ TEST_F(SleepTest, MidpointSleepZeroVelocity) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; + MjDataPtr d = MakeData(m); // give initial velocity (both translational and angular) d->qvel[0] = 0.5; @@ -569,7 +537,7 @@ TEST_F(SleepTest, MidpointSleepZeroVelocity) { // step until body goes to sleep for (int step = 0; step < 1000; step++) { - mj_step(m, d); + mj_step(m.get(), d.get()); if (d->ntree_awake == 0) break; } @@ -581,9 +549,6 @@ TEST_F(SleepTest, MidpointSleepZeroVelocity) { EXPECT_EQ(d->qvel[i], 0.0) << "qvel[" << i << "] not zero after sleep"; EXPECT_EQ(d->qacc[i], 0.0) << "qacc[" << i << "] not zero after sleep"; } - - mj_deleteData(d); - mj_deleteModel(m); } static const char* const kInitIslandFailModel = diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index cd753e47..c69d2074 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -38,10 +38,6 @@ using ::testing::Ne; using ::testing::NotNull; using ::testing::Pointwise; - - - - using Name2idTest = MujocoTest; static constexpr char name2idTestingModel[] = R"( @@ -94,79 +90,69 @@ static constexpr char name2idTestingModel[] = R"( )"; TEST_F(Name2idTest, FindIds) { - char error[1024]; - mjModel* model = - LoadModelFromString(name2idTestingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + char error[1024]; + MjModelPtr model = + LoadModelFromString(name2idTestingModel, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; - EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "world"), 0); - EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body1"), 1); - EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body2"), 2); - EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom1"), 0); - EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom2"), 1); - EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint2"), 1); - EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "mesh1"), 0); - EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "light1"), 0); - EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "camera1"), 0); - EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "site2"), 1); - EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "material1"), 0); - EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "texture1"), 0); - EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "tendon1"), 0); - EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "actuator1"), 0); - EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "sensor1"), 0); - - mj_deleteModel(model); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "world"), 0); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "body1"), 1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "body2"), 2); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "body1_geom1"), 0); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "body1_geom2"), 1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_JOINT, "joint2"), 1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MESH, "mesh1"), 0); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_LIGHT, "light1"), 0); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_CAMERA, "camera1"), 0); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SITE, "site2"), 1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MATERIAL, "material1"), 0); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TEXTURE, "texture1"), 0); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TENDON, "tendon1"), 0); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_ACTUATOR, "actuator1"), 0); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SENSOR, "sensor1"), 0); } -TEST_F(Name2idTest, MissingIds) { - char error[1024]; - mjModel* model = - LoadModelFromString(name2idTestingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; +TEST_F(Name2idTest, MissingIds) { + char error[1024]; + MjModelPtr model = + LoadModelFromString(name2idTestingModel, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; - EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "abody3"), -1); - EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "abody2_geom2"), -1); - EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint3"), -1); - EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "amesh2"), -1); - EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "alight2"), -1); - EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "acamera2"), -1); - EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "asite3"), -1); - EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "amaterial2"), -1); - EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "atexture2"), -1); - EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "atendon2"), -1); - EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "aactuator2"), -1); - EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "asensor2"), -1); - - mj_deleteModel(model); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "abody3"), -1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "abody2_geom2"), -1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_JOINT, "joint3"), -1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MESH, "amesh2"), -1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_LIGHT, "alight2"), -1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_CAMERA, "acamera2"), -1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SITE, "asite3"), -1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MATERIAL, "amaterial2"), -1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TEXTURE, "atexture2"), -1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TENDON, "atendon2"), -1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_ACTUATOR, "aactuator2"), -1); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SENSOR, "asensor2"), -1); } TEST_F(Name2idTest, EmptyIds) { - char error[1024]; - mjModel* model = - LoadModelFromString(name2idTestingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + char error[1024]; + MjModelPtr model = + LoadModelFromString(name2idTestingModel, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; - EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, ""), -1); - - mj_deleteModel(model); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, ""), -1); } TEST_F(Name2idTest, Namespaces) { - char error[1024]; - mjModel* model = - LoadModelFromString(name2idTestingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + char error[1024]; + MjModelPtr model = + LoadModelFromString(name2idTestingModel, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; - EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "camera1"), 3); - - mj_deleteModel(model); + EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "camera1"), 3); } using VersionTest = MujocoTest; -TEST_F(VersionTest, MjVersion) { - EXPECT_EQ(mj_version(), mjVERSION_HEADER); -} +TEST_F(VersionTest, MjVersion) { EXPECT_EQ(mj_version(), mjVERSION_HEADER); } TEST_F(VersionTest, MjVersionString) { #if GTEST_USES_SIMPLE_RE == 1 @@ -177,7 +163,6 @@ TEST_F(VersionTest, MjVersionString) { EXPECT_THAT(std::string(mj_versionString()), regex_matcher); } - using SupportTest = MujocoTest; // utility: generate two random quaternions with a given angle difference @@ -188,7 +173,7 @@ void randomQuatPair(mjtNum qa[4], mjtNum qb[4], mjtNum angle, int seed) { std::normal_distribution dist(0, 1); // sample qa = qb - for (int i=0; i < 4; i++) { + for (int i = 0; i < 4; i++) { qa[i] = qb[i] = dist(rng); } mju_normalize4(qa); @@ -196,7 +181,7 @@ void randomQuatPair(mjtNum qa[4], mjtNum qb[4], mjtNum angle, int seed) { // integrate qb in random direction by angle mjtNum dir[3]; - for (int i=0; i < 3; i++) { + for (int i = 0; i < 3; i++) { dir[i] = dist(rng); } mju_normalize3(dir); @@ -218,8 +203,8 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) { const mjtNum eps = 1e-12; // epsilon for float comparison char error[1024]; - mjModel* model = LoadModelFromString(ballJointModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(ballJointModel, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; int seed = 1; for (mjtNum angle : {0.0, 1e-5, 1e-2}) { @@ -230,7 +215,7 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) { // get velocity given timestep mjtNum qvel[3]; - mj_differentiatePos(model, qvel, dt, qpos1, qpos2); + mj_differentiatePos(model.get(), qvel, dt, qpos1, qpos2); // equivalent computation mjtNum qneg[4], qdif[4], qvel_expect[3]; @@ -242,8 +227,6 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) { EXPECT_THAT(AsVector(qvel, 3), Pointwise(MjNear(eps, 1e-3), qvel_expect)); } } - - mj_deleteModel(model); } static const char* const kDefaultModel = "testdata/model.xml"; @@ -261,9 +244,9 @@ TEST_F(StateTest, GetSetStateStepEqual) { std::normal_distribution dist(0, .01); // set controls and applied joint forces to random values - for (int i=0; i < model->nu; i++) data->ctrl[i] = dist(rng); - for (int i=0; i < model->nv; i++) data->qfrc_applied[i] = dist(rng); - for (int i=0; i < model->neq; i++) data->eq_active[i] = dist(rng) > 0; + for (int i = 0; i < model->nu; i++) data->ctrl[i] = dist(rng); + for (int i = 0; i < model->nv; i++) data->qfrc_applied[i] = dist(rng); + for (int i = 0; i < model->neq; i++) data->eq_active[i] = dist(rng) > 0; // take one step mj_step(model, data); @@ -320,38 +303,35 @@ TEST_F(StateTest, GetSetStateDelay) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // verify history buffer exists: nhistory = 2 + 2*5 = 12 EXPECT_EQ(model->nhistory, 12); // [user, cursor, times(5), values(5)] // state size should include history buffer - int size = mj_stateSize(model, mjSTATE_HISTORY); + int size = mj_stateSize(model.get(), mjSTATE_HISTORY); EXPECT_EQ(size, model->nhistory); // step to populate history buffer data->ctrl[0] = 1.0; - mj_step(model, data); + mj_step(model.get(), data.get()); data->ctrl[0] = 2.0; - mj_step(model, data); + mj_step(model.get(), data.get()); // get history state vector history_state(size); - mj_getState(model, data, history_state.data(), mjSTATE_HISTORY); + mj_getState(model.get(), data.get(), history_state.data(), mjSTATE_HISTORY); // modify the history buffer manually (value at index 7 = 2+5 = after times) data->history[7] = 99.0; // first value // set history state back - should restore original - mj_setState(model, data, history_state.data(), mjSTATE_HISTORY); + mj_setState(model.get(), data.get(), history_state.data(), mjSTATE_HISTORY); // verify restoration EXPECT_NE(data->history[7], 99.0); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(StateTest, CopyState) { @@ -367,13 +347,13 @@ TEST_F(StateTest, CopyState) { // modify d_src src->time = 1.23; - for (int i=0; i < m->nq; ++i) src->qpos[i] = i*0.1; - for (int i=0; i < m->nv; ++i) src->qvel[i] = i*0.2; - for (int i=0; i < m->na; ++i) src->act[i] = i*0.3; - for (int i=0; i < m->nu; ++i) src->ctrl[i] = i*0.4; - for (int i=0; i < m->nhistory; ++i) src->history[i] = i*0.5; + for (int i = 0; i < m->nq; ++i) src->qpos[i] = i * 0.1; + for (int i = 0; i < m->nv; ++i) src->qvel[i] = i * 0.2; + for (int i = 0; i < m->na; ++i) src->act[i] = i * 0.3; + for (int i = 0; i < m->nu; ++i) src->ctrl[i] = i * 0.4; + for (int i = 0; i < m->nhistory; ++i) src->history[i] = i * 0.5; - for (int i=0; i < m->neq; ++i) src->eq_active[i] = 1 - m->eq_active0[i]; + for (int i = 0; i < m->neq; ++i) src->eq_active[i] = 1 - m->eq_active0[i]; // check that states differ EXPECT_NE(src->time, dst->time); @@ -413,9 +393,9 @@ TEST_F(StateTest, ExtractState) { std::normal_distribution dist(0, .01); // set controls and applied joint forces to random values - for (int i=0; i < model->nu; i++) data->ctrl[i] = dist(rng); - for (int i=0; i < model->nv; i++) data->qfrc_applied[i] = dist(rng); - for (int i=0; i < model->neq; i++) data->eq_active[i] = dist(rng) > 0; + for (int i = 0; i < model->nu; i++) data->ctrl[i] = dist(rng); + for (int i = 0; i < model->nv; i++) data->qfrc_applied[i] = dist(rng); + for (int i = 0; i < model->neq; i++) data->eq_active[i] = dist(rng) > 0; // take one step mj_step(model, data); @@ -518,16 +498,15 @@ TEST_F(InertiaTest, AddMdenseSameAsSparse) { } // sparse addM - mj_addM(m, d, dst_sparse.data(), rownnz.data(), - rowadr.data(), colind.data()); + mj_addM(m, d, dst_sparse.data(), rownnz.data(), rowadr.data(), colind.data()); // dense addM mj_addM(m, d, dst_dense.data(), nullptr, nullptr, nullptr); // dense comparison (lower triangle) - for (int i=0; i < nv; i++) { - for (int j=0; j < nv; j++) { - EXPECT_EQ(dst_dense[i*nv+j], dst_sparse[i*nv+j]); + for (int i = 0; i < nv; i++) { + for (int j = 0; j < nv; j++) { + EXPECT_EQ(dst_dense[i * nv + j], dst_sparse[i * nv + j]); } } @@ -547,13 +526,13 @@ TEST_F(InertiaTest, mulM) { mj_forward(model, data); // dense M matrix - vector Mdense(nv*nv); - mju_sym2dense(Mdense.data(), data->M, nv, - model->M_rownnz, model->M_rowadr, model->M_colind); + vector Mdense(nv * nv); + mju_sym2dense(Mdense.data(), data->M, nv, model->M_rownnz, model->M_rowadr, + model->M_colind); // arbitrary RHS vector vector vec(nv); - for (int i=0; i < nv; i++) vec[i] = vec[i] = 20 + 30*i; + for (int i = 0; i < nv; i++) vec[i] = vec[i] = 20 + 30 * i; // multiply directly vector res1(nv, 0); @@ -582,7 +561,7 @@ TEST_F(InertiaTest, mulM2) { // arbitrary RHS vector vector vec(nv); - for (int i=0; i < nv; i++) vec[i] = .2 + .3*i; + for (int i = 0; i < nv; i++) vec[i] = .2 + .3 * i; // multiply sqrtMvec = M^1/2 * vec vector sqrtMvec(nv); @@ -618,8 +597,8 @@ TEST_F(InertiaTest, FullM) { // get dense mass matrix from M using mju_sparse2dense vector M_CSR(nv * nv); - mju_sparse2dense(M_CSR.data(), d->M, nv, nv, - m->M_rownnz, m->M_rowadr, m->M_colind); + mju_sparse2dense(M_CSR.data(), d->M, nv, nv, m->M_rownnz, m->M_rowadr, + m->M_colind); // expect lower triangles to match exactly for (int i = 0; i < nv; ++i) { @@ -630,8 +609,8 @@ TEST_F(InertiaTest, FullM) { // get dense LTDL factor (D on the diagonal) vector LD(nv * nv); - mju_sparse2dense(LD.data(), d->qLD, nv, nv, - m->M_rownnz, m->M_rowadr, m->M_colind); + mju_sparse2dense(LD.data(), d->qLD, nv, nv, m->M_rownnz, m->M_rowadr, + m->M_colind); // extract L and D from LD vector L = LD; @@ -686,88 +665,85 @@ static constexpr char GeomDistanceTestingModel2[] = R"( TEST_F(SupportTest, GeomDistance) { char error[1024]; - mjModel* model = + MjModelPtr model = LoadModelFromString(GeomDistanceTestingModel1, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); - mj_kinematics(model, data); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mj_kinematics(model.get(), data.get()); // plane-sphere, distmax too small mjtNum distmax = 0.5; - EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, nullptr), 0.5); + EXPECT_EQ(mj_geomDistance(model.get(), data.get(), 0, 1, distmax, nullptr), + 0.5); mjtNum fromto[6]; - EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, fromto), 0.5); + EXPECT_EQ(mj_geomDistance(model.get(), data.get(), 0, 1, distmax, fromto), + 0.5); EXPECT_THAT(fromto, Pointwise(Eq(), vector{0, 0, 0, 0, 0, 0})); // plane-sphere distmax = 1.0; - EXPECT_THAT(mj_geomDistance(model, data, 0, 1, 1.0, fromto), + EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 0, 1, 1.0, fromto), MjNear(0.8, 1e-12, 1e-5)); mjtNum eps = 1e-12; - EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5), - vector{0, 0, 0, 0, 0, 0.8})); + EXPECT_THAT(fromto, + Pointwise(MjNear(eps, 1e-5), vector{0, 0, 0, 0, 0, 0.8})); // sphere-plane - EXPECT_THAT(mj_geomDistance(model, data, 1, 0, 1.0, fromto), + EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 1, 0, 1.0, fromto), MjNear(0.8, 1e-12, 1e-5)); - EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5), - vector{0, 0, 0.8, 0, 0, 0})); + EXPECT_THAT(fromto, + Pointwise(MjNear(eps, 1e-5), vector{0, 0, 0.8, 0, 0, 0})); // sphere-sphere - EXPECT_THAT(mj_geomDistance(model, data, 1, 2, 1.0, fromto), + EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 1, 2, 1.0, fromto), MjNear(0.5, 1e-12, 1e-5)); - EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5), - vector{.2, 0, 1, .7, 0, 1})); + EXPECT_THAT(fromto, + Pointwise(MjNear(eps, 1e-5), vector{.2, 0, 1, .7, 0, 1})); // sphere-sphere, flipped order - EXPECT_THAT(mj_geomDistance(model, data, 2, 1, 1.0, fromto), + EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 2, 1, 1.0, fromto), MjNear(0.5, 1e-12, 1e-5)); - EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5), - vector{.7, 0, 1, .2, 0, 1})); + EXPECT_THAT(fromto, + Pointwise(MjNear(eps, 1e-5), vector{.7, 0, 1, .2, 0, 1})); // mesh-sphere (close distmax) distmax = 0.701; eps = model->opt.ccd_tolerance; - EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto), - MjNear(0.7, eps, eps*100)); - EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps*100), + EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 3, 1, distmax, fromto), + MjNear(0.7, eps, eps * 100)); + EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps * 100), vector{0, 0, .1, 0, 0, .8})); // mesh-sphere (far distmax) distmax = 1.0; - EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto), - MjNear(0.7, eps, eps*100)); - EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps*100), + EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 3, 1, distmax, fromto), + MjNear(0.7, eps, eps * 100)); + EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps * 100), vector{0, 0, .1, 0, 0, .8})); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SupportTest, GeomDistanceFromToFlipped) { mjtNum distmax = 10.0; char error[1024]; - mjModel* model = + MjModelPtr model = LoadModelFromString(GeomDistanceTestingModel2, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); - mj_kinematics(model, data); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); + mj_kinematics(model.get(), data.get()); mjtNum fromto01[6]; mjtNum fromto10[6]; for (int flag : {0, (int)mjDSBL_NATIVECCD}) { model->opt.disableflags = flag; - mj_geomDistance(model, data, 0, 1, distmax, fromto01); - mj_geomDistance(model, data, 1, 0, distmax, fromto10); + mj_geomDistance(model.get(), data.get(), 0, 1, distmax, fromto01); + mj_geomDistance(model.get(), data.get(), 1, 0, distmax, fromto10); mjtNum fromto10flipped[6] = {fromto10[3], fromto10[4], fromto10[5], fromto10[0], fromto10[1], fromto10[2]}; EXPECT_THAT(AsVector(fromto10flipped, 6), Pointwise(MjNear(1.0e-12, 1e-5), fromto01)); } - mj_deleteData(data); - mj_deleteModel(model); } static constexpr char kSetKeyframeTestingModel[] = R"( @@ -789,43 +765,38 @@ static constexpr char kSetKeyframeTestingModel[] = R"( TEST_F(SupportTest, SetKeyframe) { char error[1024]; - mjModel* model = + MjModelPtr model = LoadModelFromString(kSetKeyframeTestingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); data->ctrl[0] = 1; while (data->time < 1) { - mj_step(model, data); + mj_step(model.get(), data.get()); } - mj_setKeyframe(model, data, 1); + mj_setKeyframe(model.get(), data.get(), 1); EXPECT_EQ(data->time, model->key_time[1]); EXPECT_EQ(data->ctrl[0], model->key_ctrl[model->nu * 1]); EXPECT_EQ(data->qpos[0], model->key_qpos[model->nq * 1]); EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 1]); EXPECT_EQ(data->act[0], model->key_act[model->na * 1]); - mj_step(model, data); - mj_setKeyframe(model, data, 0); + mj_step(model.get(), data.get()); + mj_setKeyframe(model.get(), data.get(), 0); EXPECT_EQ(data->time, model->key_time[0]); EXPECT_EQ(data->ctrl[0], model->key_ctrl[model->nu * 0]); EXPECT_EQ(data->qpos[0], model->key_qpos[model->nq * 0]); EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 0]); EXPECT_EQ(data->act[0], model->key_act[model->na * 0]); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SupportTest, ContactSensorDim) { - int dataSpec = 1 << mjCONDATA_FOUND | - 1 << mjCONDATA_FORCE | - 1 << mjCONDATA_DIST | - 1 << mjCONDATA_POS | + int dataSpec = 1 << mjCONDATA_FOUND | 1 << mjCONDATA_FORCE | + 1 << mjCONDATA_DIST | 1 << mjCONDATA_POS | 1 << mjCONDATA_TANGENT; - EXPECT_EQ(mju_condataSize(dataSpec), 1+3+1+3+3); + EXPECT_EQ(mju_condataSize(dataSpec), 1 + 3 + 1 + 3 + 3); } // ------------------------------ ctrl delays -------------------------------- @@ -844,16 +815,14 @@ TEST_F(SupportTest, ReadCtrlNoDelay) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + MjDataPtr data = MakeData(model); // no delay: should return current ctrl value data->ctrl[0] = 42.0; - EXPECT_EQ(mj_readCtrl(model, data, 0, data->time, /*order=*/0), 42.0); - - mj_deleteData(data); - mj_deleteModel(model); + EXPECT_EQ(mj_readCtrl(model.get(), data.get(), 0, data->time, /*order=*/0), + 42.0); } TEST_F(SupportTest, ReadCtrlWithDelay) { @@ -872,9 +841,9 @@ TEST_F(SupportTest, ReadCtrlWithDelay) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // model should have delay configured // delay = 0.03 seconds, timestep = 0.01, so ndelay = ceil(0.03/0.01) = 3 @@ -884,11 +853,8 @@ TEST_F(SupportTest, ReadCtrlWithDelay) { // initially, buffer should be filled with constant value (from init) // reading at current time should return the init value - mjtNum val = mj_readCtrl(model, data, 0, data->time, /*order=*/0); + mjtNum val = mj_readCtrl(model.get(), data.get(), 0, data->time, /*order=*/0); EXPECT_EQ(val, data->ctrl[0]); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SupportTest, InitCtrlDelay) { @@ -907,35 +873,33 @@ TEST_F(SupportTest, InitCtrlDelay) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // verify nhistory EXPECT_EQ(model->actuator_history[0], 3); // initialize with custom times and values - // buffer stores: time 0.0 -> value 1.0, time 0.01 -> value 2.0, time 0.02 -> value 3.0 + // buffer stores: time 0.0 -> value 1.0, time 0.01 -> value 2.0, time 0.02 -> + // value 3.0 mjtNum times[3] = {0.0, 0.01, 0.02}; mjtNum values[3] = {1.0, 2.0, 3.0}; - mj_initCtrlHistory(model, data, 0, times, values); + mj_initCtrlHistory(model.get(), data.get(), 0, times, values); // mj_readCtrl now auto-subtracts delay: lookup_time = time - delay // delay = 0.02, so: // time=0.04 -> lookup at 0.02 -> value 3.0 // time=0.03 -> lookup at 0.01 -> value 2.0 // time=0.02 -> lookup at 0.00 -> value 1.0 - mjtNum val = mj_readCtrl(model, data, 0, 0.04, /*order=*/0); + mjtNum val = mj_readCtrl(model.get(), data.get(), 0, 0.04, /*order=*/0); EXPECT_EQ(val, 3.0); - val = mj_readCtrl(model, data, 0, 0.03, /*order=*/0); + val = mj_readCtrl(model.get(), data.get(), 0, 0.03, /*order=*/0); EXPECT_EQ(val, 2.0); - val = mj_readCtrl(model, data, 0, 0.02, /*order=*/0); + val = mj_readCtrl(model.get(), data.get(), 0, 0.02, /*order=*/0); EXPECT_EQ(val, 1.0); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SupportTest, InitCtrlDelayNullTimes) { @@ -954,9 +918,9 @@ TEST_F(SupportTest, InitCtrlDelayNullTimes) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // get existing times from buffer int adr = model->actuator_historyadr[0]; @@ -965,7 +929,7 @@ TEST_F(SupportTest, InitCtrlDelayNullTimes) { // initialize with NULL times (use existing) and new values mjtNum values[3] = {10.0, 20.0, 30.0}; - mj_initCtrlHistory(model, data, 0, nullptr, values); + mj_initCtrlHistory(model.get(), data.get(), 0, nullptr, values); // verify times are unchanged EXPECT_EQ(buf[2], existing_times[0]); @@ -976,9 +940,6 @@ TEST_F(SupportTest, InitCtrlDelayNullTimes) { EXPECT_EQ(buf[5], 10.0); EXPECT_EQ(buf[6], 20.0); EXPECT_EQ(buf[7], 30.0); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(SupportTest, InitSensorDelay) { @@ -997,36 +958,35 @@ TEST_F(SupportTest, InitSensorDelay) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // verify nsample for sensor EXPECT_EQ(model->sensor_history[0], 3); // initialize with custom times and values, phase=0 - // buffer stores: time 0.0 -> value 0.5, time 0.01 -> value 0.6, time 0.02 -> value 0.7 + // buffer stores: time 0.0 -> value 0.5, time 0.01 -> value 0.6, time 0.02 -> + // value 0.7 mjtNum times[3] = {0.0, 0.01, 0.02}; mjtNum values[3] = {0.5, 0.6, 0.7}; - mj_initSensorHistory(model, data, 0, times, values, /*phase=*/0.0); + mj_initSensorHistory(model.get(), data.get(), 0, times, values, + /*phase=*/0.0); // mj_readSensor now auto-subtracts delay: lookup_time = time - delay // delay = 0.02, so: // time=0.04 -> lookup at 0.02 -> value 0.7 // time=0.03 -> lookup at 0.01 -> value 0.6 mjtNum result = 0; - const mjtNum* ptr = mj_readSensor(model, data, 0, 0.04, &result, /*order=*/0); + const mjtNum* ptr = + mj_readSensor(model.get(), data.get(), 0, 0.04, &result, /*order=*/0); mjtNum val = ptr ? *ptr : result; EXPECT_NEAR(val, 0.7, 1e-6); - ptr = mj_readSensor(model, data, 0, 0.03, &result, /*order=*/0); + ptr = mj_readSensor(model.get(), data.get(), 0, 0.03, &result, /*order=*/0); val = ptr ? *ptr : result; EXPECT_NEAR(val, 0.6, 1e-6); - - mj_deleteData(data); - mj_deleteModel(model); } } // namespace } // namespace mujoco - diff --git a/test/engine/engine_util_container_test.cc b/test/engine/engine_util_container_test.cc index 35e4bb19..8dca197d 100644 --- a/test/engine/engine_util_container_test.cc +++ b/test/engine/engine_util_container_test.cc @@ -16,9 +16,9 @@ #include #include -#include #include #include +#include #include #include "test/fixture.h" @@ -32,10 +32,9 @@ constexpr int GetExpectedStackUsageBytes() { if constexpr (N <= 0) { return prev_size; } else { - constexpr auto RoundUpToAlignment = - [](int x, int alignment) { - return alignment * (x / alignment + ((x % alignment) ? 1 : 0)); - }; + constexpr auto RoundUpToAlignment = [](int x, int alignment) { + return alignment * (x / alignment + ((x % alignment) ? 1 : 0)); + }; constexpr int size_with_arraylist = RoundUpToAlignment( prev_size + sizeof(mjArrayList), alignof(mjArrayList)); constexpr int size_with_buffer = RoundUpToAlignment( @@ -49,15 +48,15 @@ class TestMjArrayList : public MujocoTest {}; TEST_F(TestMjArrayList, TestMjArrayListSingleThreaded) { std::array error; - mjModel* m = LoadModelFromString("", error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error.data(); - mjData* d = mj_makeData(m); - mj_markStack(d); + MjModelPtr m = LoadModelFromString("", error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << "Failed to load model: " << error.data(); + MjDataPtr d = MakeData(m); + mj_markStack(d.get()); using DataType = int; constexpr int kInitialCapacity = 10; mjArrayList* array_list = - mju_arrayListCreate(d, sizeof(DataType), kInitialCapacity); + mju_arrayListCreate(d.get(), sizeof(DataType), kInitialCapacity); constexpr int kNumElements = 35; for (int i = 0; i < kNumElements; ++i) { @@ -78,19 +77,17 @@ TEST_F(TestMjArrayList, TestMjArrayListSingleThreaded) { EXPECT_EQ(mju_arrayListAt(array_list, kNumElements), nullptr); EXPECT_EQ(mju_arrayListAt(array_list, 100), nullptr); - mj_freeStack(d); - mj_deleteData(d); - mj_deleteModel(m); + mj_freeStack(d.get()); } TEST_F(TestMjArrayList, ZeroInitialCapacity) { char error[1024]; - mjModel* m = LoadModelFromString("", error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error; - mjData* d = mj_makeData(m); - mj_markStack(d); + MjModelPtr m = LoadModelFromString("", error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << "Failed to load model: " << error; + MjDataPtr d = MakeData(m); + mj_markStack(d.get()); mjArrayList* array_list = - mju_arrayListCreate(d, sizeof(double), /*initial_capacity=*/0); + mju_arrayListCreate(d.get(), sizeof(double), /*initial_capacity=*/0); EXPECT_EQ(mju_arrayListSize(array_list), 0); for (int i = 0; i < 35; ++i) { @@ -104,9 +101,7 @@ TEST_F(TestMjArrayList, ZeroInitialCapacity) { } EXPECT_EQ(mju_arrayListAt(array_list, 35), nullptr); - mj_freeStack(d); - mj_deleteData(d); - mj_deleteModel(m); + mj_freeStack(d.get()); } } // namespace diff --git a/test/engine/engine_util_misc_test.cc b/test/engine/engine_util_misc_test.cc index 8e1d742b..ec1b457f 100644 --- a/test/engine/engine_util_misc_test.cc +++ b/test/engine/engine_util_misc_test.cc @@ -14,19 +14,20 @@ // Tests for engine/engine_util_solve.c. +#include "src/engine/engine_util_misc.h" + #include -#include #include #include #include #include +#include #include -#include #include +#include #include #include -#include "src/engine/engine_util_misc.h" #include "test/fixture.h" namespace mujoco { @@ -60,11 +61,11 @@ TEST_F(UtilMiscTest, PrintsMemoryWarning) { TEST_F(UtilMiscTest, Sigmoid) { // function values - EXPECT_EQ(mju_sigmoid(-1), 0); - EXPECT_EQ(mju_sigmoid(0), 0); + EXPECT_EQ(mju_sigmoid(-1), 0); + EXPECT_EQ(mju_sigmoid(0), 0); EXPECT_EQ(mju_sigmoid(0.5), 0.5); - EXPECT_EQ(mju_sigmoid(1), 1); - EXPECT_EQ(mju_sigmoid(2), 1); + EXPECT_EQ(mju_sigmoid(1), 1); + EXPECT_EQ(mju_sigmoid(2), 1); // epsilon for finite-differencing const mjtNum dx = MjTol(1e-7, 1e-3); @@ -80,8 +81,8 @@ TEST_F(UtilMiscTest, Sigmoid) { // derivative at 0.5 const mjtNum x = 0.5; - mjtNum dy_dx_0p5 = (mju_sigmoid(x + dx) - mju_sigmoid(x - dx)) / (2*dx); - mjtNum expected = 30*x*x*x*x - 60*x*x*x + 30*x*x; + mjtNum dy_dx_0p5 = (mju_sigmoid(x + dx) - mju_sigmoid(x - dx)) / (2 * dx); + mjtNum expected = 30 * x * x * x * x - 60 * x * x * x + 30 * x * x; EXPECT_NEAR(dy_dx_0p5, expected, fd_tol); } @@ -125,29 +126,27 @@ TEST_F(UtilMiscTest, SphereWrap) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + MjDataPtr data = MakeData(model); // measure tendon length for keyframe 0 - mj_resetDataKeyframe(model, data, 0); - mj_forward(model, data); + mj_resetDataKeyframe(model.get(), data.get(), 0); + mj_forward(model.get(), data.get()); mjtNum ten_length0 = data->sensordata[0]; // measure tendon length for keyframe 1 - mj_resetDataKeyframe(model, data, 1); - mj_forward(model, data); + mj_resetDataKeyframe(model.get(), data.get(), 1); + mj_forward(model.get(), data.get()); mjtNum ten_length1 = data->sensordata[0]; // difference should be small mjtNum diff = ten_length1 - ten_length0; EXPECT_LT(mju_abs(diff), 1e-3); - - mj_deleteData(data); - mj_deleteModel(model); } -// compute time constant as in Millard et al. (2013) https://doi.org/10.1115/1.4023390 +// compute time constant as in Millard et al. (2013) +// https://doi.org/10.1115/1.4023390 mjtNum muscleDynamicsMillard(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) { // clamp control mjtNum ctrlclamp = mju_clip(ctrl, 0, 1); @@ -157,13 +156,13 @@ mjtNum muscleDynamicsMillard(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) { mjtNum tau; if (ctrlclamp > act) { - tau = prm[0] * (0.5 + 1.5*actclamp); + tau = prm[0] * (0.5 + 1.5 * actclamp); } else { - tau = prm[1] / (0.5 + 1.5*actclamp); + tau = prm[1] / (0.5 + 1.5 * actclamp); } // filter output - return (ctrlclamp-act) / mjMAX(mjMINVAL, tau); + return (ctrlclamp - act) / mjMAX(mjMINVAL, tau); } TEST_F(UtilMiscTest, SmoothMuscleDynamics) { @@ -188,7 +187,7 @@ TEST_F(UtilMiscTest, SmoothMuscleDynamics) { EXPECT_EQ(muscleDynamicsMillard(ctrl, act, prm), mju_muscleDynamics(ctrl, act, prm)); - ctrl = 0.6 + eps; // larger than act by just over 0.5*tau_smooth + ctrl = 0.6 + eps; // larger than act by just over 0.5*tau_smooth EXPECT_EQ(muscleDynamicsMillard(ctrl, act, prm), mju_muscleDynamics(ctrl, act, prm)); @@ -196,11 +195,11 @@ TEST_F(UtilMiscTest, SmoothMuscleDynamics) { mjtNum tau_act = 0.2; mjtNum tau_deact = 0.3; for (mjtNum dctrl : {0.0, 0.1, 0.2, 1.0, 1.1}) { - mjtNum lower = mju_muscleDynamicsTimescale(-dctrl, - tau_act, tau_deact, tau_smooth); - mjtNum upper = mju_muscleDynamicsTimescale(dctrl, - tau_act, tau_deact, tau_smooth); - EXPECT_EQ(0.5*(upper + lower), 0.5*(tau_act + tau_deact)); + mjtNum lower = + mju_muscleDynamicsTimescale(-dctrl, tau_act, tau_deact, tau_smooth); + mjtNum upper = + mju_muscleDynamicsTimescale(dctrl, tau_act, tau_deact, tau_smooth); + EXPECT_EQ(0.5 * (upper + lower), 0.5 * (tau_act + tau_deact)); } } @@ -208,13 +207,13 @@ TEST_F(UtilMiscTest, MuscleGainLength) { mjtNum lmin = 0.5; mjtNum lmax = 1.5; - EXPECT_EQ(mju_muscleGainLength(0.0, lmin, lmax), 0); - EXPECT_EQ(mju_muscleGainLength(0.5, lmin, lmax), 0); + EXPECT_EQ(mju_muscleGainLength(0.0, lmin, lmax), 0); + EXPECT_EQ(mju_muscleGainLength(0.5, lmin, lmax), 0); EXPECT_EQ(mju_muscleGainLength(0.75, lmin, lmax), 0.5); - EXPECT_EQ(mju_muscleGainLength(1.0, lmin, lmax), 1); + EXPECT_EQ(mju_muscleGainLength(1.0, lmin, lmax), 1); EXPECT_EQ(mju_muscleGainLength(1.25, lmin, lmax), 0.5); - EXPECT_EQ(mju_muscleGainLength(1.5, lmin, lmax), 0); - EXPECT_EQ(mju_muscleGainLength(2.0, lmin, lmax), 0); + EXPECT_EQ(mju_muscleGainLength(1.5, lmin, lmax), 0); + EXPECT_EQ(mju_muscleGainLength(2.0, lmin, lmax), 0); } TEST_F(UtilMiscTest, MjuSparseMap) { @@ -250,11 +249,9 @@ TEST_F(UtilMiscTest, MjuSparseMap) { // Verify the map by checking values mjtNum mat_res_gathered[nnz_res]; mju_gather(mat_res_gathered, mat_src, map, nnz_res); - EXPECT_THAT(AsVector(mat_res_gathered, nnz_res), - ElementsAre(1, 3, 5, 6)); + EXPECT_THAT(AsVector(mat_res_gathered, nnz_res), ElementsAre(1, 3, 5, 6)); } - TEST_F(UtilMiscTest, MjuSparseLower2SymMap) { // nr = 3 // src = [[1, 0, 0], @@ -277,8 +274,8 @@ TEST_F(UtilMiscTest, MjuSparseLower2SymMap) { int map[res_nnz]; int cursor[nr]; - mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, - rowadr_src, rownnz_src, colind_src, cursor); + mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, rowadr_src, + rownnz_src, colind_src, cursor); // Expected map: // res(0,0) -> src(0,0) (k=0) => map[0] = 0 @@ -323,8 +320,8 @@ TEST_F(UtilMiscTest, MjuSparseLower2SymMapPartial) { int map[res_nnz]; int cursor[nr]; - mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, - rowadr_src, rownnz_src, colind_src, cursor); + mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, rowadr_src, + rownnz_src, colind_src, cursor); // Expected map for the non-zeros in res: // res(0,0) -> src(0,0) (k=0) => map[0] = 0 @@ -398,23 +395,24 @@ using InterpolationTest = MujocoTest; TEST_F(InterpolationTest, mju_interpolate3D) { // quadratic functions should be interpolated exactly if order = 2 auto quadratic_function_1 = [](mjtNum x, mjtNum y, mjtNum z) { - return x*x + y*y + z*z; + return x * x + y * y + z * z; }; auto quadratic_function_2 = [](mjtNum x, mjtNum y, mjtNum z) { - return x*y*z + y*z*z + x*z*z; + return x * y * z + y * z * z + x * z * z; }; auto quadratic_function_3 = [](mjtNum x, mjtNum y, mjtNum z) { - return x*y*z + y*z*z + x*z*z + y*y*z + x*x*z + x + y + z; + return x * y * z + y * z * z + x * z * z + y * y * z + x * x * z + x + y + + z; }; static constexpr int order = 2; - mjtNum coeff[3*(order+1)*(order+1)*(order+1)]; + mjtNum coeff[3 * (order + 1) * (order + 1) * (order + 1)]; int index = 0; for (int i = 0; i <= order; ++i) { for (int j = 0; j <= order; ++j) { for (int k = 0; k <= order; ++k) { - coeff[3*index+0] = quadratic_function_1(.5*i, .5*j, .5*k); - coeff[3*index+1] = quadratic_function_2(.5*i, .5*j, .5*k); - coeff[3*index+2] = quadratic_function_3(.5*i, .5*j, .5*k); + coeff[3 * index + 0] = quadratic_function_1(.5 * i, .5 * j, .5 * k); + coeff[3 * index + 1] = quadratic_function_2(.5 * i, .5 * j, .5 * k); + coeff[3 * index + 2] = quadratic_function_3(.5 * i, .5 * j, .5 * k); index++; } } @@ -460,8 +458,8 @@ TEST_F(InterpolationTest, mju_cellLookup_MultiCell) { // 2x3x4 grid, trilinear: 3x4x5 = 60 nodes int cellnum[3] = {2, 3, 4}; int order = 1; - int ny_g = 3*1 + 1; // 4 - int nz_g = 4*1 + 1; // 5 + int ny_g = 3 * 1 + 1; // 4 + int nz_g = 4 * 1 + 1; // 5 // point at (0.75, 0.5, 0.125) -> cell (1, 1, 0) mjtNum coord[3] = {0.75, 0.5, 0.125}; @@ -485,7 +483,7 @@ TEST_F(InterpolationTest, mju_cellLookup_MultiCell) { for (int li = 0; li <= 1; li++) { for (int lj = 0; lj <= 1; lj++) { for (int lk = 0; lk <= 1; lk++) { - expected[ni++] = (1+li)*ny_g*nz_g + (1+lj)*nz_g + lk; + expected[ni++] = (1 + li) * ny_g * nz_g + (1 + lj) * nz_g + lk; } } } @@ -519,8 +517,8 @@ TEST_F(InterpolationTest, mju_defGradient) { mjtNum mat[9]; mjtNum p1[3] = {.5, .5, .5}; mjtNum p2[3] = {.25, .25, .25}; - mjtNum dof0[24] = {0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 1, 1, - 1, 0, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1}; + mjtNum dof0[24] = {0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 1, 1, + 1, 0, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1}; // identity mjtNum dof1[24]; @@ -538,7 +536,7 @@ TEST_F(InterpolationTest, mju_defGradient) { // constant stretch mjtNum dof3[24]; - for (int i = 0; i < 24; ++i) dof3[i] = 2*dof0[i]; + for (int i = 0; i < 24; ++i) dof3[i] = 2 * dof0[i]; mju_defGradient(mat, p1, dof3, order); EXPECT_THAT(mat, ElementsAreArray({2, 0, 0, 0, 2, 0, 0, 0, 2})); mju_defGradient(mat, p2, dof3, order); @@ -546,7 +544,7 @@ TEST_F(InterpolationTest, mju_defGradient) { // axial stretch mjtNum dof4[24]; - for (int i = 0; i < 24; ++i) dof4[i] = (i%3 == 1 ? 2 : 1)*dof0[i]; + for (int i = 0; i < 24; ++i) dof4[i] = (i % 3 == 1 ? 2 : 1) * dof0[i]; mju_defGradient(mat, p1, dof4, order); EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 2, 0, 0, 0, 1})); mju_defGradient(mat, p2, dof4, order); @@ -557,8 +555,8 @@ TEST_F(InterpolationTest, mju_defGradient) { for (int i = 0; i < 8; ++i) { mjtNum quat[4] = {0, 0, 0, 1}; mjtNum axis[3] = {0, 0, 1}; - mju_axisAngle2Quat(quat, axis, mjPI/2); - mju_rotVecQuat(dof5 + 3*i, dof0 + 3*i, quat); + mju_axisAngle2Quat(quat, axis, mjPI / 2); + mju_rotVecQuat(dof5 + 3 * i, dof0 + 3 * i, quat); } mju_defGradient(mat, p1, dof5, order); EXPECT_THAT(mat, Pointwise(MjNear(1e-8, 1e-6), {0, -1, 0, 1, 0, 0, 0, 0, 1})); @@ -571,8 +569,8 @@ TEST_F(InterpolationTest, mju_defGradient) { for (int i = 0; i < 8; ++i) { mjtNum quat[4]; mjtNum axis[3] = {0, 0, 1}; - mju_axisAngle2Quat(quat, axis, mjPI/6); - mju_rotVecQuat(dof6 + 3*i, dof0 + 3*i, quat); + mju_axisAngle2Quat(quat, axis, mjPI / 6); + mju_rotVecQuat(dof6 + 3 * i, dof0 + 3 * i, quat); mju_quat2Mat(rot6, quat); } mju_defGradient(mat, p1, dof6, order); @@ -587,9 +585,9 @@ TEST_F(InterpolationTest, mju_defGradient) { mjtNum quat[4]; mjtNum axis[3] = {0, 0, 1}; mjtNum offset[3] = {-.5, -.5, 0}; - mju_axisAngle2Quat(quat, axis, mjPI/6); - mju_add3(dof7 + 3*i, dof0 + 3*i, offset); - mju_rotVecQuat(dof7 + 3*i, dof0 + 3*i, quat); + mju_axisAngle2Quat(quat, axis, mjPI / 6); + mju_add3(dof7 + 3 * i, dof0 + 3 * i, offset); + mju_rotVecQuat(dof7 + 3 * i, dof0 + 3 * i, quat); mju_quat2Mat(rot7, quat); } mju_defGradient(mat, p1, dof7, order); @@ -618,15 +616,15 @@ TEST_F(InterpolationTest, mju_flexInterpState_MultiCell) { mjtNum z = (k - 1) * 0.1; // Apply rotation - xpos[3*idx + 0] = -y; - xpos[3*idx + 1] = x; - xpos[3*idx + 2] = z; + xpos[3 * idx + 0] = -y; + xpos[3 * idx + 1] = x; + xpos[3 * idx + 2] = z; idx++; } } } - int npc = (order+1)*(order+1)*(order+1); + int npc = (order + 1) * (order + 1) * (order + 1); std::vector xpos_c(3 * npc); mju_flexGatherCellState(order, cy, cz, 0, 0, 0, xpos.data(), NULL, NULL, @@ -775,7 +773,7 @@ TEST_F(Base64Test, mju_isValidBase64_valid4) { TEST_F(Base64Test, mju_decodeBase64) { std::array buffer; - const char *s = "D4a+//A="; + const char* s = "D4a+//A="; std::size_t n = mju_decodeBase64(buffer.data(), s); @@ -785,7 +783,7 @@ TEST_F(Base64Test, mju_decodeBase64) { TEST_F(Base64Test, mju_decodeBase6_align0) { std::array buffer; - const char *s = "QUJD"; + const char* s = "QUJD"; std::size_t n = mju_decodeBase64(buffer.data(), s); @@ -795,7 +793,7 @@ TEST_F(Base64Test, mju_decodeBase6_align0) { TEST_F(Base64Test, mju_decodeBase64_align1) { std::array buffer; - const char *s = "QUI="; + const char* s = "QUI="; std::size_t n = mju_decodeBase64(buffer.data(), s); @@ -805,7 +803,7 @@ TEST_F(Base64Test, mju_decodeBase64_align1) { TEST_F(Base64Test, mju_decodeBase64_align2) { std::array buffer; - const char *s = "QQ=="; + const char* s = "QQ=="; std::size_t n = mju_decodeBase64(buffer.data(), s); @@ -814,7 +812,7 @@ TEST_F(Base64Test, mju_decodeBase64_align2) { } TEST_F(Base64Test, mju_decodeBase64_null) { - const char *s = ""; + const char* s = ""; std::size_t n = mju_decodeBase64(NULL, s); @@ -823,7 +821,7 @@ TEST_F(Base64Test, mju_decodeBase64_null) { TEST_F(Base64Test, mju_decodeBase64_ones) { std::array buffer; - const char *s = "////"; + const char* s = "////"; std::size_t n = mju_decodeBase64(buffer.data(), s); @@ -834,7 +832,7 @@ TEST_F(Base64Test, mju_decodeBase64_ones) { TEST_F(Base64Test, decodeAndEncode) { std::array buffer1; std::array buffer2; - const char *s = "D4a+/vA="; + const char* s = "D4a+/vA="; mju_decodeBase64(buffer1.data(), s); mju_encodeBase64(buffer2.data(), buffer1.data(), buffer1.size()); @@ -853,15 +851,15 @@ using HistoryTest = MujocoTest; TEST_F(HistoryTest, Init) { constexpr int n = 4; constexpr int dim = 1; - mjtNum buf[2 + n + n*dim]; + mjtNum buf[2 + n + n * dim]; std::vector times = {4, 6, 8, 10}; std::vector values = {99, 99, 99, 99}; mju_historyInit(buf, n, dim, times.data(), values.data(), 0.0); // check header - EXPECT_EQ(buf[0], 0.0); // user - EXPECT_EQ(buf[1], static_cast(n-1)); // cursor = n-1 + EXPECT_EQ(buf[0], 0.0); // user + EXPECT_EQ(buf[1], static_cast(n - 1)); // cursor = n-1 // timestamps: [4, 6, 8, 10] (t=10 is newest) // values: [99, 99, 99, 99] @@ -874,13 +872,13 @@ TEST_F(HistoryTest, Init) { TEST_F(HistoryTest, Init_Vector) { constexpr int n = 3; constexpr int dim = 2; - mjtNum buf[2 + n + n*dim]; + mjtNum buf[2 + n + n * dim]; std::vector times = {-2, -1, 0}; std::vector values = {1.0, 2.0, 1.0, 2.0, 1.0, 2.0}; mju_historyInit(buf, n, dim, times.data(), values.data(), 0.0); - EXPECT_EQ(buf[1], static_cast(n-1)); // cursor = n-1 + EXPECT_EQ(buf[1], static_cast(n - 1)); // cursor = n-1 // verify via read function mjtNum res[dim]; @@ -894,7 +892,7 @@ TEST_F(HistoryTest, Append) { constexpr int n = 4; constexpr int dim = 1; // Initialize buffer properly, then insert - mjtNum buf[2 + 2*n]; + mjtNum buf[2 + 2 * n]; buf[0] = 0.0; buf[1] = n - 1; // timestamps: [4, 6, 8, 10] @@ -926,7 +924,7 @@ TEST_F(HistoryTest, Append) { TEST_F(HistoryTest, Append_Multiple) { constexpr int n = 3; constexpr int dim = 1; - mjtNum buf[2 + 2*n]; + mjtNum buf[2 + 2 * n]; buf[0] = 0.0; buf[1] = n - 1; mjtNum times[] = {-2, -1, 0}; @@ -947,20 +945,23 @@ TEST_F(HistoryTest, Append_Multiple) { TEST_F(HistoryTest, ReadVector_ExactMatch) { constexpr int n = 3; constexpr int dim = 2; - mjtNum buf[2 + n + n*dim]; + mjtNum buf[2 + n + n * dim]; buf[0] = 0.0; buf[1] = n - 1; mjtNum times[] = {0, 1, 2}; mju_copy(buf + 2, times, n); - mju_zero(buf + 2 + n, n*dim); + mju_zero(buf + 2 + n, n * dim); // set values: t=0->(1,2), t=1->(3,4), t=2->(5,6) mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); - slot0[0] = 1.0; slot0[1] = 2.0; + slot0[0] = 1.0; + slot0[1] = 2.0; mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0); - slot1[0] = 3.0; slot1[1] = 4.0; + slot1[0] = 3.0; + slot1[1] = 4.0; mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0); - slot2[0] = 5.0; slot2[1] = 6.0; + slot2[0] = 5.0; + slot2[1] = 6.0; mjtNum res[dim]; const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 1.0, 0); @@ -972,19 +973,22 @@ TEST_F(HistoryTest, ReadVector_ExactMatch) { TEST_F(HistoryTest, ReadVector_ZOH) { constexpr int n = 3; constexpr int dim = 2; - mjtNum buf[2 + n + n*dim]; + mjtNum buf[2 + n + n * dim]; buf[0] = 0.0; buf[1] = n - 1; mjtNum times[] = {0, 1, 2}; mju_copy(buf + 2, times, n); - mju_zero(buf + 2 + n, n*dim); + mju_zero(buf + 2 + n, n * dim); mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); - slot0[0] = 1.0; slot0[1] = 2.0; + slot0[0] = 1.0; + slot0[1] = 2.0; mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0); - slot1[0] = 3.0; slot1[1] = 4.0; + slot1[0] = 3.0; + slot1[1] = 4.0; mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0); - slot2[0] = 5.0; slot2[1] = 6.0; + slot2[0] = 5.0; + slot2[1] = 6.0; mjtNum res[dim]; const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 0); @@ -996,19 +1000,22 @@ TEST_F(HistoryTest, ReadVector_ZOH) { TEST_F(HistoryTest, ReadVector_Linear) { constexpr int n = 3; constexpr int dim = 2; - mjtNum buf[2 + n + n*dim]; + mjtNum buf[2 + n + n * dim]; buf[0] = 0.0; buf[1] = n - 1; mjtNum times[] = {0, 1, 2}; mju_copy(buf + 2, times, n); - mju_zero(buf + 2 + n, n*dim); + mju_zero(buf + 2 + n, n * dim); mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); - slot0[0] = 1.0; slot0[1] = 2.0; + slot0[0] = 1.0; + slot0[1] = 2.0; mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0); - slot1[0] = 3.0; slot1[1] = 4.0; + slot1[0] = 3.0; + slot1[1] = 4.0; mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0); - slot2[0] = 5.0; slot2[1] = 6.0; + slot2[0] = 5.0; + slot2[1] = 6.0; mjtNum res[dim]; const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 1); @@ -1020,7 +1027,7 @@ TEST_F(HistoryTest, ReadVector_Linear) { TEST_F(HistoryTest, InsertOutOfOrder) { constexpr int n = 4; constexpr int dim = 1; - mjtNum buf[2 + 2*n]; + mjtNum buf[2 + 2 * n]; mjtNum res; auto reset = [&]() { @@ -1067,7 +1074,7 @@ TEST_F(HistoryTest, InsertOutOfOrder) { TEST_F(HistoryTest, InsertReplaceOnCollision) { constexpr int n = 4; constexpr int dim = 1; - mjtNum buf[2 + 2*n]; + mjtNum buf[2 + 2 * n]; mjtNum res; auto reset = [&]() { @@ -1129,15 +1136,17 @@ TEST_F(HistoryTest, CubicInterpolation) { buf[1] = n - 1; mjtNum times[] = {-1, 0}; mju_copy(buf + 2, times, n); - mju_zero(buf + 2 + n, n*dim); + mju_zero(buf + 2 + n, n * dim); // Insert (0, 0, 1) and (1, 1, 0). // Dim 0: 0 -> 1. Spline: p(x) = 3x^2 - 2x^3 // Dim 1: 1 -> 0. Spline: p(x) = 1 - 3x^2 + 2x^3 mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); - slot0[0] = 0.0; slot0[1] = 1.0; + slot0[0] = 0.0; + slot0[1] = 1.0; mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0); - slot1[0] = 1.0; slot1[1] = 0.0; + slot1[0] = 1.0; + slot1[1] = 0.0; mjtNum res[2]; @@ -1152,7 +1161,7 @@ TEST_F(HistoryTest, CubicInterpolation) { // Dim 0: 3*0.25^2 - 2*0.25^3 // Dim 1: 1 - (3*0.25^2 - 2*0.25^3) mju_historyRead(buf, n, dim, res, 0.25, 2); - mjtNum expected_0_25 = 3*0.25*0.25 - 2*0.25*0.25*0.25; + mjtNum expected_0_25 = 3 * 0.25 * 0.25 - 2 * 0.25 * 0.25 * 0.25; EXPECT_NEAR(res[0], expected_0_25, MjTol(1e-9, 1e-9)); EXPECT_NEAR(res[1], 1.0 - expected_0_25, MjTol(1e-9, 1e-9)); @@ -1160,7 +1169,7 @@ TEST_F(HistoryTest, CubicInterpolation) { // Dim 0: 3*0.8^2 - 2*0.8^3 // Dim 1: 1 - (3*0.8^2 - 2*0.8^3) mju_historyRead(buf, n, dim, res, 0.8, 2); - mjtNum expected_0_8 = 3*0.8*0.8 - 2*0.8*0.8*0.8; + mjtNum expected_0_8 = 3 * 0.8 * 0.8 - 2 * 0.8 * 0.8 * 0.8; EXPECT_NEAR(res[0], expected_0_8, MjTol(1e-9, 1e-9)); EXPECT_NEAR(res[1], 1.0 - expected_0_8, MjTol(1e-9, 1e-9)); } @@ -1184,9 +1193,9 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) { // create dummy positions for 8 nodes std::vector xpos(3 * 8, 0); for (int i = 0; i < 8; i++) { - xpos[3*i + 0] = (i / 4) * 1.0; - xpos[3*i + 1] = ((i / 2) % 2) * 1.0; - xpos[3*i + 2] = (i % 2) * 1.0; + xpos[3 * i + 0] = (i / 4) * 1.0; + xpos[3 * i + 1] = ((i / 2) % 2) * 1.0; + xpos[3 * i + 2] = (i % 2) * 1.0; } // helper: compute expected global node index from (gx, gy, gz) @@ -1198,8 +1207,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) { // normal_axis=0, na0=1, na1=2 { int indices[4]; - mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, - NULL, NULL, NULL, indices, NULL); + mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, NULL, + NULL, NULL, indices, NULL); EXPECT_EQ(indices[0], gidx(0, 0, 0)); EXPECT_EQ(indices[1], gidx(0, 0, 1)); EXPECT_EQ(indices[2], gidx(0, 1, 0)); @@ -1209,8 +1218,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) { // face 1: x=max (fixed g[0]=1, varying g[1], g[2]) { int indices[4]; - mju_flexGatherFaceState(order, cx, cy, cz, 1, xpos.data(), NULL, NULL, - NULL, NULL, NULL, indices, NULL); + mju_flexGatherFaceState(order, cx, cy, cz, 1, xpos.data(), NULL, NULL, NULL, + NULL, NULL, indices, NULL); EXPECT_EQ(indices[0], gidx(1, 0, 0)); EXPECT_EQ(indices[1], gidx(1, 0, 1)); EXPECT_EQ(indices[2], gidx(1, 1, 0)); @@ -1222,8 +1231,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) { // loop order: l0→z, l1→x { int indices[4]; - mju_flexGatherFaceState(order, cx, cy, cz, 2, xpos.data(), NULL, NULL, - NULL, NULL, NULL, indices, NULL); + mju_flexGatherFaceState(order, cx, cy, cz, 2, xpos.data(), NULL, NULL, NULL, + NULL, NULL, indices, NULL); EXPECT_EQ(indices[0], gidx(0, 0, 0)); // l0=0(z=0), l1=0(x=0) EXPECT_EQ(indices[1], gidx(1, 0, 0)); // l0=0(z=0), l1=1(x=1) EXPECT_EQ(indices[2], gidx(0, 0, 1)); // l0=1(z=1), l1=0(x=0) @@ -1234,8 +1243,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) { // normal_axis=1, na0=2(z slow), na1=0(x fast) { int indices[4]; - mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, - NULL, NULL, NULL, indices, NULL); + mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, NULL, + NULL, NULL, indices, NULL); EXPECT_EQ(indices[0], gidx(0, 1, 0)); // l0=0(z=0), l1=0(x=0) EXPECT_EQ(indices[1], gidx(1, 1, 0)); // l0=0(z=0), l1=1(x=1) EXPECT_EQ(indices[2], gidx(0, 1, 1)); // l0=1(z=1), l1=0(x=0) @@ -1246,8 +1255,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) { // normal_axis=2, na0=0, na1=1 { int indices[4]; - mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, - NULL, NULL, NULL, indices, NULL); + mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, NULL, + NULL, NULL, indices, NULL); EXPECT_EQ(indices[0], gidx(0, 0, 0)); EXPECT_EQ(indices[1], gidx(0, 1, 0)); EXPECT_EQ(indices[2], gidx(1, 0, 0)); @@ -1257,8 +1266,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) { // face 5: z=max (fixed g[2]=1) { int indices[4]; - mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL, - NULL, NULL, NULL, indices, NULL); + mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL, NULL, + NULL, NULL, indices, NULL); EXPECT_EQ(indices[0], gidx(0, 0, 1)); EXPECT_EQ(indices[1], gidx(0, 1, 1)); EXPECT_EQ(indices[2], gidx(1, 0, 1)); @@ -1285,9 +1294,9 @@ TEST_F(FaceStateTest, NodeIndicesMultiCell) { int gi = i / 9; int gj = (i / 3) % 3; int gk = i % 3; - xpos[3*i + 0] = gi * 0.1; - xpos[3*i + 1] = gj * 0.1; - xpos[3*i + 2] = gk * 0.1; + xpos[3 * i + 0] = gi * 0.1; + xpos[3 * i + 1] = gj * 0.1; + xpos[3 * i + 2] = gk * 0.1; } auto gidx = [&](int gx, int gy, int gz) { @@ -1299,8 +1308,8 @@ TEST_F(FaceStateTest, NodeIndicesMultiCell) { // na0=1, na1=2: g[0]=0, g[1]=0..1, g[2]=0..1 { int indices[4]; - mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, - NULL, NULL, NULL, indices, NULL); + mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, NULL, + NULL, NULL, indices, NULL); EXPECT_EQ(indices[0], gidx(0, 0, 0)); EXPECT_EQ(indices[1], gidx(0, 0, 1)); EXPECT_EQ(indices[2], gidx(0, 1, 0)); @@ -1310,8 +1319,8 @@ TEST_F(FaceStateTest, NodeIndicesMultiCell) { // face 0 (x=0), quad 3: (q0=1, q1=1) → within_face = 1*2+1 = 3 { int indices[4]; - mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, - NULL, NULL, NULL, indices, NULL); + mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, NULL, + NULL, NULL, indices, NULL); EXPECT_EQ(indices[0], gidx(0, 1, 1)); EXPECT_EQ(indices[1], gidx(0, 1, 2)); EXPECT_EQ(indices[2], gidx(0, 2, 1)); @@ -1322,8 +1331,8 @@ TEST_F(FaceStateTest, NodeIndicesMultiCell) { // g[0] = cx*order = 2 { int indices[4]; - mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, - NULL, NULL, NULL, indices, NULL); + mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, NULL, + NULL, NULL, indices, NULL); EXPECT_EQ(indices[0], gidx(2, 0, 0)); EXPECT_EQ(indices[1], gidx(2, 0, 1)); EXPECT_EQ(indices[2], gidx(2, 1, 0)); @@ -1344,9 +1353,9 @@ TEST_F(FaceStateTest, NodeIndicesNonCubicGrid) { int gi = i / 8; int gj = (i / 4) % 2; int gk = i % 4; - xpos[3*i + 0] = gi * 0.1; - xpos[3*i + 1] = gj * 0.1; - xpos[3*i + 2] = gk * 0.1; + xpos[3 * i + 0] = gi * 0.1; + xpos[3 * i + 1] = gj * 0.1; + xpos[3 * i + 2] = gk * 0.1; } auto gidx = [&](int gx, int gy, int gz) { @@ -1366,8 +1375,8 @@ TEST_F(FaceStateTest, NodeIndicesNonCubicGrid) { // Total flat index = 3 + 3 + 2 = 8 { int indices[4]; - mju_flexGatherFaceState(order, cx, cy, cz, 8, xpos.data(), NULL, NULL, - NULL, NULL, NULL, indices, NULL); + mju_flexGatherFaceState(order, cx, cy, cz, 8, xpos.data(), NULL, NULL, NULL, + NULL, NULL, indices, NULL); EXPECT_EQ(indices[0], gidx(0, 0, 1)); EXPECT_EQ(indices[1], gidx(1, 0, 1)); EXPECT_EQ(indices[2], gidx(0, 0, 2)); @@ -1388,9 +1397,9 @@ TEST_F(FaceStateTest, DataGathering) { std::vector xpos0(3 * nnodes); for (int i = 0; i < nnodes; i++) { for (int d = 0; d < 3; d++) { - xpos[3*i + d] = 10 * i + d; - vel[3*i + d] = 100 * i + d; - xpos0[3*i + d] = 1000 * i + d; + xpos[3 * i + d] = 10 * i + d; + vel[3 * i + d] = 100 * i + d; + xpos0[3 * i + d] = 1000 * i + d; } } @@ -1408,9 +1417,9 @@ TEST_F(FaceStateTest, DataGathering) { for (int n = 0; n < npe; n++) { int gi = indices[n]; for (int d = 0; d < 3; d++) { - EXPECT_EQ(xpos_f[3*n + d], xpos[3*gi + d]); - EXPECT_EQ(vel_f[3*n + d], vel[3*gi + d]); - EXPECT_EQ(xpos0_f[3*n + d], xpos0[3*gi + d]); + EXPECT_EQ(xpos_f[3 * n + d], xpos[3 * gi + d]); + EXPECT_EQ(vel_f[3 * n + d], vel[3 * gi + d]); + EXPECT_EQ(xpos0_f[3 * n + d], xpos0[3 * gi + d]); } } } @@ -1428,9 +1437,9 @@ TEST_F(FaceStateTest, IdentityRotationAxisAligned) { for (int i = 0; i <= 1; i++) { for (int j = 0; j <= 1; j++) { for (int k = 0; k <= 1; k++) { - xpos[3*idx + 0] = i; - xpos[3*idx + 1] = j; - xpos[3*idx + 2] = k; + xpos[3 * idx + 0] = i; + xpos[3 * idx + 1] = j; + xpos[3 * idx + 2] = k; idx++; } } @@ -1469,7 +1478,7 @@ TEST_F(FaceStateTest, RotatedCubeRotation) { mjtNum axis[3] = {0, 0, 1}; mjtNum rot_quat[4]; mju_axisAngle2Quat(rot_quat, axis, mjPI / 2); - mju_rotVecQuat(xpos.data() + 3*idx, orig, rot_quat); + mju_rotVecQuat(xpos.data() + 3 * idx, orig, rot_quat); idx++; } } @@ -1519,7 +1528,7 @@ TEST_F(FaceStateTest, RotationConsistencyWith3D) { mjtNum axis[3] = {0, 0, 1}; mjtNum rot_quat[4]; mju_axisAngle2Quat(rot_quat, axis, mjPI / 6); - mju_rotVecQuat(xpos.data() + 3*idx, orig, rot_quat); + mju_rotVecQuat(xpos.data() + 3 * idx, orig, rot_quat); idx++; } } @@ -1543,8 +1552,8 @@ TEST_F(FaceStateTest, RotationConsistencyWith3D) { xpos_f.data(), NULL, NULL, NULL, quat_2d); // quaternions may differ by sign; compare unsigned - mjtNum dot = quat_3d[0]*quat_2d[0] + quat_3d[1]*quat_2d[1] + - quat_3d[2]*quat_2d[2] + quat_3d[3]*quat_2d[3]; + mjtNum dot = quat_3d[0] * quat_2d[0] + quat_3d[1] * quat_2d[1] + + quat_3d[2] * quat_2d[2] + quat_3d[3] * quat_2d[3]; EXPECT_NEAR(mju_abs(dot), 1.0, 1e-5) << "face " << fe << ": 2D rotation differs from 3D cell rotation"; } @@ -1559,10 +1568,10 @@ static void MakeRegularGrid(mjtNum* nodexpos, int nx, int ny, int nz) { for (int i = 0; i < nx; i++) { for (int j = 0; j < ny; j++) { for (int k = 0; k < nz; k++) { - int idx = i*ny*nz + j*nz + k; - nodexpos[3*idx+0] = (mjtNum)i; - nodexpos[3*idx+1] = (mjtNum)j; - nodexpos[3*idx+2] = (mjtNum)k; + int idx = i * ny * nz + j * nz + k; + nodexpos[3 * idx + 0] = (mjtNum)i; + nodexpos[3 * idx + 1] = (mjtNum)j; + nodexpos[3 * idx + 2] = (mjtNum)k; } } } @@ -1571,7 +1580,7 @@ static void MakeRegularGrid(mjtNum* nodexpos, int nx, int ny, int nz) { TEST_F(ShellTFITest, IdentityGrid) { // 3x3x3 grid: 1 interior node at (1,1,1) constexpr int nx = 3, ny = 3, nz = 3; - mjtNum nodexpos[3*nx*ny*nz]; + mjtNum nodexpos[3 * nx * ny * nz]; MakeRegularGrid(nodexpos, nx, ny, nz); // save expected interior position @@ -1581,44 +1590,44 @@ TEST_F(ShellTFITest, IdentityGrid) { mju_shellTrackInterior(nodexpos, nx, ny, nz); // interior node at (1,1,1) should match - int idx = 1*ny*nz + 1*nz + 1; - EXPECT_NEAR(nodexpos[3*idx+0], expected[0], MjTol(1e-12, 1e-5)); - EXPECT_NEAR(nodexpos[3*idx+1], expected[1], MjTol(1e-12, 1e-5)); - EXPECT_NEAR(nodexpos[3*idx+2], expected[2], MjTol(1e-12, 1e-5)); + int idx = 1 * ny * nz + 1 * nz + 1; + EXPECT_NEAR(nodexpos[3 * idx + 0], expected[0], MjTol(1e-12, 1e-5)); + EXPECT_NEAR(nodexpos[3 * idx + 1], expected[1], MjTol(1e-12, 1e-5)); + EXPECT_NEAR(nodexpos[3 * idx + 2], expected[2], MjTol(1e-12, 1e-5)); } TEST_F(ShellTFITest, UniformScaling) { // 3x3x3: scale all boundary nodes by 2x, interior should follow constexpr int nx = 3, ny = 3, nz = 3; - mjtNum nodexpos[3*nx*ny*nz]; + mjtNum nodexpos[3 * nx * ny * nz]; MakeRegularGrid(nodexpos, nx, ny, nz); // scale all nodes - for (int i = 0; i < 3*nx*ny*nz; i++) { + for (int i = 0; i < 3 * nx * ny * nz; i++) { nodexpos[i] *= 2.0; } // run TFI — interior should be reconstructed to 2*original mju_shellTrackInterior(nodexpos, nx, ny, nz); - int idx = 1*ny*nz + 1*nz + 1; - EXPECT_NEAR(nodexpos[3*idx+0], 2.0, MjTol(1e-12, 1e-5)); - EXPECT_NEAR(nodexpos[3*idx+1], 2.0, MjTol(1e-12, 1e-5)); - EXPECT_NEAR(nodexpos[3*idx+2], 2.0, MjTol(1e-12, 1e-5)); + int idx = 1 * ny * nz + 1 * nz + 1; + EXPECT_NEAR(nodexpos[3 * idx + 0], 2.0, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(nodexpos[3 * idx + 1], 2.0, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(nodexpos[3 * idx + 2], 2.0, MjTol(1e-12, 1e-5)); } TEST_F(ShellTFITest, AffineDeformation) { // 4x4x4 grid with 8 interior nodes. Apply affine transform to boundary, // then verify TFI reproduces the same affine transform on interior nodes. constexpr int nx = 4, ny = 4, nz = 4; - mjtNum nodexpos[3*nx*ny*nz]; + mjtNum nodexpos[3 * nx * ny * nz]; MakeRegularGrid(nodexpos, nx, ny, nz); // affine: F(x,y,z) = A*[x,y,z]^T + b // A = [[2, 0.5, 0], [0.3, 1.5, 0], [0, 0, 1]], b = [10, 20, 30] auto affine = [](mjtNum x, mjtNum y, mjtNum z, mjtNum out[3]) { - out[0] = 2.0*x + 0.5*y + 10.0; - out[1] = 0.3*x + 1.5*y + 20.0; + out[0] = 2.0 * x + 0.5 * y + 10.0; + out[1] = 0.3 * x + 1.5 * y + 20.0; out[2] = z + 30.0; }; @@ -1626,20 +1635,20 @@ TEST_F(ShellTFITest, AffineDeformation) { for (int i = 0; i < nx; i++) { for (int j = 0; j < ny; j++) { for (int k = 0; k < nz; k++) { - int idx = i*ny*nz + j*nz + k; - affine((mjtNum)i, (mjtNum)j, (mjtNum)k, nodexpos + 3*idx); + int idx = i * ny * nz + j * nz + k; + affine((mjtNum)i, (mjtNum)j, (mjtNum)k, nodexpos + 3 * idx); } } } // corrupt interior nodes to verify TFI actually reconstructs them - for (int i = 1; i < nx-1; i++) { - for (int j = 1; j < ny-1; j++) { - for (int k = 1; k < nz-1; k++) { - int idx = i*ny*nz + j*nz + k; - nodexpos[3*idx+0] = -999; - nodexpos[3*idx+1] = -999; - nodexpos[3*idx+2] = -999; + for (int i = 1; i < nx - 1; i++) { + for (int j = 1; j < ny - 1; j++) { + for (int k = 1; k < nz - 1; k++) { + int idx = i * ny * nz + j * nz + k; + nodexpos[3 * idx + 0] = -999; + nodexpos[3 * idx + 1] = -999; + nodexpos[3 * idx + 2] = -999; } } } @@ -1648,17 +1657,17 @@ TEST_F(ShellTFITest, AffineDeformation) { mju_shellTrackInterior(nodexpos, nx, ny, nz); // check all interior nodes match affine - for (int i = 1; i < nx-1; i++) { - for (int j = 1; j < ny-1; j++) { - for (int k = 1; k < nz-1; k++) { - int idx = i*ny*nz + j*nz + k; + for (int i = 1; i < nx - 1; i++) { + for (int j = 1; j < ny - 1; j++) { + for (int k = 1; k < nz - 1; k++) { + int idx = i * ny * nz + j * nz + k; mjtNum expected[3]; affine((mjtNum)i, (mjtNum)j, (mjtNum)k, expected); - EXPECT_NEAR(nodexpos[3*idx+0], expected[0], MjTol(1e-12, 1e-4)) + EXPECT_NEAR(nodexpos[3 * idx + 0], expected[0], MjTol(1e-12, 1e-4)) << "i=" << i << " j=" << j << " k=" << k; - EXPECT_NEAR(nodexpos[3*idx+1], expected[1], MjTol(1e-12, 1e-4)) + EXPECT_NEAR(nodexpos[3 * idx + 1], expected[1], MjTol(1e-12, 1e-4)) << "i=" << i << " j=" << j << " k=" << k; - EXPECT_NEAR(nodexpos[3*idx+2], expected[2], MjTol(1e-12, 1e-4)) + EXPECT_NEAR(nodexpos[3 * idx + 2], expected[2], MjTol(1e-12, 1e-4)) << "i=" << i << " j=" << j << " k=" << k; } } @@ -1668,12 +1677,12 @@ TEST_F(ShellTFITest, AffineDeformation) { TEST_F(ShellTFITest, BoundaryUnmodified) { // verify that boundary nodes are not modified by TFI constexpr int nx = 4, ny = 4, nz = 4; - mjtNum nodexpos[3*nx*ny*nz]; + mjtNum nodexpos[3 * nx * ny * nz]; MakeRegularGrid(nodexpos, nx, ny, nz); // save boundary node values - mjtNum saved[3*nx*ny*nz]; - mju_copy(saved, nodexpos, 3*nx*ny*nz); + mjtNum saved[3 * nx * ny * nz]; + mju_copy(saved, nodexpos, 3 * nx * ny * nz); mju_shellTrackInterior(nodexpos, nx, ny, nz); @@ -1681,14 +1690,13 @@ TEST_F(ShellTFITest, BoundaryUnmodified) { for (int i = 0; i < nx; i++) { for (int j = 0; j < ny; j++) { for (int k = 0; k < nz; k++) { - bool is_boundary = (i == 0 || i == nx-1 || - j == 0 || j == ny-1 || - k == 0 || k == nz-1); + bool is_boundary = (i == 0 || i == nx - 1 || j == 0 || j == ny - 1 || + k == 0 || k == nz - 1); if (is_boundary) { - int idx = i*ny*nz + j*nz + k; - EXPECT_EQ(nodexpos[3*idx+0], saved[3*idx+0]); - EXPECT_EQ(nodexpos[3*idx+1], saved[3*idx+1]); - EXPECT_EQ(nodexpos[3*idx+2], saved[3*idx+2]); + int idx = i * ny * nz + j * nz + k; + EXPECT_EQ(nodexpos[3 * idx + 0], saved[3 * idx + 0]); + EXPECT_EQ(nodexpos[3 * idx + 1], saved[3 * idx + 1]); + EXPECT_EQ(nodexpos[3 * idx + 2], saved[3 * idx + 2]); } } } @@ -1698,16 +1706,16 @@ TEST_F(ShellTFITest, BoundaryUnmodified) { TEST_F(ShellTFITest, NoInteriorSmallGrid) { // 2x2x2 and 2x3x2: no interior nodes, TFI should be a no-op constexpr int nx = 2, ny = 3, nz = 2; - mjtNum nodexpos[3*nx*ny*nz]; + mjtNum nodexpos[3 * nx * ny * nz]; MakeRegularGrid(nodexpos, nx, ny, nz); - mjtNum saved[3*nx*ny*nz]; - mju_copy(saved, nodexpos, 3*nx*ny*nz); + mjtNum saved[3 * nx * ny * nz]; + mju_copy(saved, nodexpos, 3 * nx * ny * nz); mju_shellTrackInterior(nodexpos, nx, ny, nz); // all nodes unchanged - for (int i = 0; i < 3*nx*ny*nz; i++) { + for (int i = 0; i < 3 * nx * ny * nz; i++) { EXPECT_EQ(nodexpos[i], saved[i]); } } diff --git a/test/engine/engine_util_solve_test.cc b/test/engine/engine_util_solve_test.cc index 029d840d..32703efd 100644 --- a/test/engine/engine_util_solve_test.cc +++ b/test/engine/engine_util_solve_test.cc @@ -35,26 +35,25 @@ namespace mujoco { namespace { - using ::testing::Pointwise; -using ::testing::ElementsAre; +using ::std::setw; using ::std::string; using ::std::vector; -using ::std::setw; +using ::testing::ElementsAre; using QCQP2Test = MujocoTest; TEST_F(QCQP2Test, DegenerateAMatrix) { // A 2x2 matrix with determinant zero. - const mjtNum Ain[9] { 6, -15, 2, -5 }; + const mjtNum Ain[9]{6, -15, 2, -5}; // Any values will do for these three inputs. - const mjtNum bin[3] { -12, 49 }; - const mjtNum d[3] { 11, 31 }; + const mjtNum bin[3]{-12, 49}; + const mjtNum d[3]{11, 31}; const mjtNum r = 0.01; // Make output array explicitly nonzero to simulate uninitialized memory. - mjtNum res[2] { 999, 999 }; + mjtNum res[2]{999, 999}; EXPECT_EQ(mju_QCQP2(res, Ain, bin, d, r), 0); EXPECT_EQ(res[0], 0); @@ -65,15 +64,15 @@ using QCQP3Test = MujocoTest; TEST_F(QCQP3Test, DegenerateAMatrix) { // A 3x3 matrix with determinant zero. - const mjtNum Ain[9] { 1, 4, -2, -3, -7, 5, 2, -9, 0 }; + const mjtNum Ain[9]{1, 4, -2, -3, -7, 5, 2, -9, 0}; // Any values will do for these three inputs. - const mjtNum bin[3] { -12, 49, 8 }; - const mjtNum d[3] { 11, 31, -23 }; + const mjtNum bin[3]{-12, 49, 8}; + const mjtNum d[3]{11, 31, -23}; const mjtNum r = 0.1; // Make output array explicitly nonzero to simulate uninitialized memory. - mjtNum res[3] { 999, 999, 999 }; + mjtNum res[3]{999, 999, 999}; EXPECT_EQ(mju_QCQP3(res, Ain, bin, d, r), 0); EXPECT_EQ(res[0], 0); @@ -93,10 +92,10 @@ mjtNum objective(const mjtNum* x, const mjtNum* H, const mjtNum* g, int n) { // utility: test if res is the minimum of a given box-QP problem bool isQPminimum(const mjtNum* res, const mjtNum* H, const mjtNum* g, int n, const mjtNum* lower, const mjtNum* upper) { - const mjtNum eps = MjTol(1e-4, 5e-2); // epsilon used for nudging + const mjtNum eps = MjTol(1e-4, 5e-2); // epsilon used for nudging const mjtNum threshold = MjTol(0, -2e-3); // comparison threshold bool is_minimum = true; - mjtNum* res_nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*n); + mjtNum* res_nudge = (mjtNum*)mju_malloc(sizeof(mjtNum) * n); // get solution value mjtNum value = objective(res, H, g, n); @@ -104,7 +103,7 @@ bool isQPminimum(const mjtNum* res, const mjtNum* H, const mjtNum* g, int n, // compare to nudged solution mju_copy(res_nudge, res, n); - for (int i=0; i < n; i++) { + for (int i = 0; i < n; i++) { // nudge down res_nudge[i] = res[i] - eps; if (lower) { @@ -144,12 +143,12 @@ void randomBoxQP(int n, mjtNum* H, mjtNum* g, mjtNum* lower, mjtNum* upper, std::normal_distribution dist(0, 1); // square root of H - mjtNum* sqrtH = (mjtNum*) mju_malloc(sizeof(mjtNum)*n*n); + mjtNum* sqrtH = (mjtNum*)mju_malloc(sizeof(mjtNum) * n * n); - for (int i=0; i < n; i++) { + for (int i = 0; i < n; i++) { g[i] = dist(rng); - lower[i] = 5*dist(rng); - upper[i] = 5*dist(rng); + lower[i] = 5 * dist(rng); + upper[i] = 5 * dist(rng); // fix invalid bounds if (lower[i] > upper[i]) { @@ -159,8 +158,8 @@ void randomBoxQP(int n, mjtNum* H, mjtNum* g, mjtNum* lower, mjtNum* upper, } // sample temp - for (int j=0; j < n; j++) { - sqrtH[n*i+j] = dist(rng); + for (int j = 0; j < n; j++) { + sqrtH[n * i + j] = dist(rng); } } @@ -174,21 +173,18 @@ void randomBoxQP(int n, mjtNum* H, mjtNum* g, mjtNum* lower, mjtNum* upper, TEST_F(BoxQPTest, UnboundedQP) { // small arrays, allocate on stack static const int n = 2; - mjtNum H[n*n] = { - 2, 0, - 0, 2 - }; + mjtNum H[n * n] = {2, 0, 0, 2}; mjtNum g[n] = {1, 3}; mjtNum res[n] = {0, 0}; - mjtNum R[n*(n+7)]; + mjtNum R[n * (n + 7)]; int nfree = mju_boxQP(res, R, /*index=*/nullptr, H, g, n, /*lower=*/nullptr, /*upper=*/nullptr); // no bounds, expect Newton point EXPECT_EQ(nfree, 2); - EXPECT_MJTNUM_EQ(res[0], -g[0]/H[0]); - EXPECT_MJTNUM_EQ(res[1], -g[1]/H[3]); + EXPECT_MJTNUM_EQ(res[0], -g[0] / H[0]); + EXPECT_MJTNUM_EQ(res[1], -g[1] / H[3]); // check that solution is actual minimum EXPECT_TRUE(isQPminimum(res, H, g, n, /*lower=*/nullptr, /*upper=*/nullptr)); @@ -209,23 +205,20 @@ TEST_F(BoxQPTest, UnboundedQP) { TEST_F(BoxQPTest, AsymmetricUpperIgnored) { // small arrays, allocate on stack static const int n = 2; - mjtNum H[n*n] = { - 1, -400, - 0, 1 - }; + mjtNum H[n * n] = {1, -400, 0, 1}; mjtNum g[n] = {1, 3}; mjtNum res[n] = {0, 0}; mjtNum lower[n] = {-2, -2}; mjtNum upper[n] = {0, 0}; int index[n]; - mjtNum R[n*(n+7)]; + mjtNum R[n * (n + 7)]; // solve box-QP int nfree = mju_boxQP(res, R, index, H, g, n, lower, upper); EXPECT_EQ(nfree, 1); - EXPECT_MJTNUM_EQ(res[0], -g[0]/H[0]); + EXPECT_MJTNUM_EQ(res[0], -g[0] / H[0]); EXPECT_MJTNUM_EQ(res[1], lower[1]); } @@ -266,9 +259,9 @@ TEST_F(BoxQPTest, UpperTrianglePoisoned) { // solve with symmetric H to get the reference result mju_zero(res, n); - int nfree_ref = mju_boxQPoption(res, R, index, H, g, n, lower, upper, - maxiter, mingrad, backtrack, minstep, - armijo, nullptr, 0); + int nfree_ref = + mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter, mingrad, + backtrack, minstep, armijo, nullptr, 0); ASSERT_GT(nfree_ref, -1); // save reference @@ -277,25 +270,25 @@ TEST_F(BoxQPTest, UpperTrianglePoisoned) { std::vector R_ref(R, R + nfree_ref * nfree_ref); // poison the strict upper triangle of H with NaN - for (int i=0; i < n; i++) { - for (int j=i+1; j < n; j++) { - H[n*i+j] = nan; + for (int i = 0; i < n; i++) { + for (int j = i + 1; j < n; j++) { + H[n * i + j] = nan; } } // solve again; result must match because only lower triangle should be read mju_zero(res, n); - int nfree_poisoned = mju_boxQPoption(res, R, index, H, g, n, lower, upper, - maxiter, mingrad, backtrack, minstep, - armijo, nullptr, 0); + int nfree_poisoned = + mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter, mingrad, + backtrack, minstep, armijo, nullptr, 0); EXPECT_EQ(nfree_poisoned, nfree_ref); - for (int i=0; i < n; i++) { + for (int i = 0; i < n; i++) { EXPECT_EQ(res[i], res_ref[i]) << "mismatch at index " << i; } - for (int i=0; i < nfree_ref; i++) { + for (int i = 0; i < nfree_ref; i++) { EXPECT_EQ(index[i], index_ref[i]) << "index mismatch at " << i; - for (int j=0; j <= i; j++) { + for (int j = 0; j <= i; j++) { int k = i * nfree_ref + j; EXPECT_EQ(R[k], R_ref[k]) << "R mismatch at row " << i << ", col " << j; } @@ -304,7 +297,7 @@ TEST_F(BoxQPTest, UpperTrianglePoisoned) { // test mju_boxQP on a single random bounded QP TEST_F(BoxQPTest, BoundedQP) { - int n = 50; // problem size + int n = 50; // problem size // allocate on heap mjtNum *H, *g, *lower, *upper; // inputs @@ -318,19 +311,19 @@ TEST_F(BoxQPTest, BoundedQP) { mju_zero(res, n); // use default options - int maxiter = 100; // maximum number of iterations - mjtNum mingrad = MjTol(1E-16, 1E-5); // minimum squared norm of (unclamped) gradient - mjtNum backtrack = 0.5; // backtrack factor for decreasing stepsize - mjtNum minstep = MjTol(1E-22, 1E-10); // minimum stepsize for linesearch - mjtNum armijo = 0.1; // Armijo parameter + int maxiter = 100; // maximum number of iterations + mjtNum mingrad = + MjTol(1E-16, 1E-5); // minimum squared norm of (unclamped) gradient + mjtNum backtrack = 0.5; // backtrack factor for decreasing stepsize + mjtNum minstep = MjTol(1E-22, 1E-10); // minimum stepsize for linesearch + mjtNum armijo = 0.1; // Armijo parameter // logging static const int logsz = 10000; char log[logsz]; - int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper, - maxiter, mingrad, backtrack, - minstep, armijo, log, logsz); + int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter, + mingrad, backtrack, minstep, armijo, log, logsz); // ADD_FAILURE() << log; // uncomment to print `log` to error log @@ -340,12 +333,12 @@ TEST_F(BoxQPTest, BoundedQP) { // verify clamping int j = nfree > 0 ? 0 : -1; - for (int i=0; i < n; i++) { + for (int i = 0; i < n; i++) { if (j >= 0 && i == index[j]) { // free dimension EXPECT_GT(res[i], lower[i]); EXPECT_LT(res[i], upper[i]); j++; - } else { // clamped dimension + } else { // clamped dimension EXPECT_TRUE(res[i] == lower[i] || res[i] == upper[i]); } } @@ -388,7 +381,7 @@ TEST_F(BoxQPTest, BoundedQPvariations) { // make random box-QP randomBoxQP(n, H, g, lower, upper, seed++); - mju_scl(H, H, scaleH, n*n); + mju_scl(H, H, scaleH, n * n); mju_scl(g, g, scaleg, n); mju_scl(lower, lower, scalebounds, n); mju_scl(upper, upper, scalebounds, n); @@ -397,16 +390,16 @@ TEST_F(BoxQPTest, BoundedQPvariations) { mju_zero(res, n); // default algorithm options - int maxiter = 100; - mjtNum mingrad = MjTol(1E-16, 1E-5); - mjtNum backtrack = 0.5; - mjtNum minstep = MjTol(1E-22, 1E-10); - mjtNum armijo = 0.1; + int maxiter = 100; + mjtNum mingrad = MjTol(1E-16, 1E-5); + mjtNum backtrack = 0.5; + mjtNum minstep = MjTol(1E-22, 1E-10); + mjtNum armijo = 0.1; // solve box-QP with logging - int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper, - maxiter, mingrad, backtrack, - minstep, armijo, log, logsz); + int nfree = + mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter, + mingrad, backtrack, minstep, armijo, log, logsz); // check solution EXPECT_GT(nfree, -1) << log; @@ -435,8 +428,8 @@ TEST_F(BoxQPTest, BoundedQPvariations) { // average of 4.5 factorizations is expected EXPECT_LE(meanfactor, 5.0); - std::cerr << "n=" << setw(3) << n - << ": average of " << meanfactor << " factorizations\n"; + std::cerr << "n=" << setw(3) << n << ": average of " << meanfactor + << " factorizations\n"; } mju_free(res); mju_free(R); @@ -461,13 +454,13 @@ void randomBanded(mjtNum* H, int nTotal, int nBand, int nDense, int seed, std::normal_distribution dist(0, 1); // allocate square root - mjtNum* sqrtH = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal*nTotal); + mjtNum* sqrtH = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal * nTotal); // sample - for (int i=0; i < nTotal; i++) { + for (int i = 0; i < nTotal; i++) { if (vec) vec[i] = dist(rng); - for (int j=0; j < nTotal; j++) { - sqrtH[nTotal*i+j] = dist(rng); + for (int j = 0; j < nTotal; j++) { + sqrtH[nTotal * i + j] = dist(rng); } } @@ -475,24 +468,23 @@ void randomBanded(mjtNum* H, int nTotal, int nBand, int nDense, int seed, mju_mulMatTMat(H, sqrtH, sqrtH, nTotal, nTotal, nTotal); // set zeros - int nSparse = nTotal-nDense; - for (int i=0; i < nSparse; i++) { + int nSparse = nTotal - nDense; + for (int i = 0; i < nSparse; i++) { int nzeros = mjMAX(0, i + 1 - nBand); - for (int j=0; j < nzeros; j++) { - H[nTotal*i + j] = 0; - H[nTotal*j + i] = 0; + for (int j = 0; j < nzeros; j++) { + H[nTotal * i + j] = 0; + H[nTotal * j + i] = 0; } } // add regularizer to diagonal - for (int i=0; i < nTotal; i++) { - H[nTotal*i + i] += reg; + for (int i = 0; i < nTotal; i++) { + H[nTotal * i + i] += reg; } mju_free(sqrtH); } - // test banded-vector diagonal values TEST_F(BandMatrixTest, Diagonal) { int seed = 1; @@ -500,10 +492,10 @@ TEST_F(BandMatrixTest, Diagonal) { for (int nBand : {1, 3}) { for (int nDense : {0, 2}) { // allocate - int nB = (nTotal-nDense)*nBand + nDense*nTotal; - mjtNum* B = (mjtNum*) mju_malloc(sizeof(mjtNum)*nB); - int nH = nTotal*nTotal; - mjtNum* H = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH); + int nB = (nTotal - nDense) * nBand + nDense * nTotal; + mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * nB); + int nH = nTotal * nTotal; + mjtNum* H = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH); // make random banded SPD matrix, dense representation randomBanded(H, nTotal, nBand, nDense, seed++); @@ -512,8 +504,8 @@ TEST_F(BandMatrixTest, Diagonal) { mju_dense2Band(B, H, nTotal, nBand, nDense); // expect diagonals to be equal - for (int i=0; i < nTotal; i++) { - EXPECT_EQ(H[i*nTotal + i], B[mju_bandDiag(i, nTotal, nBand, nDense)]); + for (int i = 0; i < nTotal; i++) { + EXPECT_EQ(H[i * nTotal + i], B[mju_bandDiag(i, nTotal, nBand, nDense)]); } mju_free(H); @@ -529,11 +521,11 @@ TEST_F(BandMatrixTest, Conversion) { for (int nBand : {0, 1, 3}) { for (int nDense : {0, 2}) { // allocate - int nB = (nTotal-nDense)*nBand + nDense*nTotal; - mjtNum* B = (mjtNum*) mju_malloc(sizeof(mjtNum)*nB); - int nH = nTotal*nTotal; - mjtNum* H = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH); - mjtNum* H1 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH); + int nB = (nTotal - nDense) * nBand + nDense * nTotal; + mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * nB); + int nH = nTotal * nTotal; + mjtNum* H = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH); + mjtNum* H1 = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH); // make random banded SPD matrix, dense randomBanded(H, nTotal, nBand, nDense, seed++); @@ -561,13 +553,13 @@ TEST_F(BandMatrixTest, Multiplication) { for (int nBand : {0, 1, 3}) { for (int nDense : {0, 2}) { // allocate - int nB = (nTotal-nDense)*nBand + nDense*nTotal; - mjtNum* B = (mjtNum*) mju_malloc(sizeof(mjtNum)*nB); - int nH = nTotal*nTotal; - mjtNum* H = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH); - mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal); - mjtNum* res = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal); - mjtNum* res1 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal); + int nB = (nTotal - nDense) * nBand + nDense * nTotal; + mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * nB); + int nH = nTotal * nTotal; + mjtNum* H = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH); + mjtNum* vec = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal); + mjtNum* res = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal); + mjtNum* res1 = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal); // make random banded SPD matrix, dense randomBanded(H, nTotal, nBand, nDense, seed++, vec); @@ -603,14 +595,14 @@ TEST_F(BandMatrixTest, Factorization) { for (mjtNum diagadd : {0.0, 1.0}) { for (int diagmul : {0.0, 1.3}) { // allocate - int nB = (nTotal-nDense)*nBand + nDense*nTotal; - mjtNum* B = (mjtNum*) mju_malloc(sizeof(mjtNum)*nB); - int nH = nTotal*nTotal; - mjtNum* H = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH); - mjtNum* H1 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH); - mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal); - mjtNum* res = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal); - mjtNum* res1 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal); + int nB = (nTotal - nDense) * nBand + nDense * nTotal; + mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * nB); + int nH = nTotal * nTotal; + mjtNum* H = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH); + mjtNum* H1 = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH); + mjtNum* vec = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal); + mjtNum* res = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal); + mjtNum* res1 = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal); // make random banded matrix, dense representation // add regularizer to ensure PD @@ -620,8 +612,8 @@ TEST_F(BandMatrixTest, Factorization) { mju_dense2Band(B, H, nTotal, nBand, nDense); // apply diagadd and diagmul - for (int i=0; i < nTotal; i++) { - H[nTotal*i + i] += diagadd + diagmul*H[nTotal*i + i]; + for (int i = 0; i < nTotal; i++) { + H[nTotal * i + i] += diagadd + diagmul * H[nTotal * i + i]; } // in-place dense factorization @@ -631,8 +623,8 @@ TEST_F(BandMatrixTest, Factorization) { EXPECT_EQ(rank, nTotal); // banded factorization - mjtNum minDiag = mju_cholFactorBand(B, nTotal, nBand, nDense, - diagadd, diagmul); + mjtNum minDiag = + mju_cholFactorBand(B, nTotal, nBand, nDense, diagadd, diagmul); // expect factorization to have succeeded EXPECT_GT(minDiag, 0); @@ -641,8 +633,8 @@ TEST_F(BandMatrixTest, Factorization) { mju_band2Dense(H1, B, nTotal, nBand, nDense, /*flg_sym=*/0); // zero upper triangle of H (unused) - for (int i=0; i < nTotal-1; i++) { - mju_zero(H + nTotal*i + i + 1, nTotal - i - 1); + for (int i = 0; i < nTotal - 1; i++) { + mju_zero(H + nTotal * i + i + 1, nTotal - i - 1); } // expect numerical equality @@ -680,14 +672,14 @@ TEST_F(BandMatrixTest, Solve) { for (int nBand : {1, 3}) { for (int nDense : {0, 2}) { // allocate - int nB = (nTotal-nDense)*nBand + nDense*nTotal; - mjtNum* B = (mjtNum*) mju_malloc(sizeof(mjtNum)*nB); - int nH = nTotal*nTotal; - mjtNum* H = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH); - mjtNum* H1 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH); - mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal); - mjtNum* res = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal); - mjtNum* res1 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal); + int nB = (nTotal - nDense) * nBand + nDense * nTotal; + mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * nB); + int nH = nTotal * nTotal; + mjtNum* H = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH); + mjtNum* H1 = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH); + mjtNum* vec = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal); + mjtNum* res = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal); + mjtNum* res1 = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal); // make random banded matrix, dense representation // add regularizer to ensure PD @@ -736,8 +728,8 @@ TEST_F(BandMatrixTest, Solve) { using EngineUtilSolveTest = MujocoTest; TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) { - mjModel* model = LoadModelFromString(""); - mjData* d = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr d = MakeData(model); // Test matrix (upper triangular, representing symmetric matrix): // [10 1 2 3] @@ -760,7 +752,7 @@ TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) { int LT_rownnz[4], LT_rowadr[4]; int nnz = mju_cholFactorSymbolic(nullptr, L_rownnz, L_rowadr, nullptr, LT_rownnz, LT_rowadr, nullptr, H_rownnz, - H_rowadr, H_colind, n, d); + H_rowadr, H_colind, n, d.get()); // verify counting phase outputs EXPECT_EQ(nnz, 8); @@ -774,7 +766,8 @@ TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) { // phase 2: filling int L_colind[8], LT_colind[8], LT_pos[8]; mju_cholFactorSymbolic(L_colind, L_rownnz, L_rowadr, LT_colind, LT_rownnz, - LT_rowadr, LT_pos, H_rownnz, H_rowadr, H_colind, n, d); + LT_rowadr, LT_pos, H_rownnz, H_rowadr, H_colind, n, + d.get()); // verify L_colind EXPECT_THAT(AsVector(L_colind, 8), ElementsAre(0, 0, 1, 0, 2, 0, 2, 3)); @@ -801,9 +794,6 @@ TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) { << "LT_pos mismatch at column " << c << ", entry " << k; } } - - mj_deleteData(d); - mj_deleteModel(model); } // Test for mju_cholUpdate: rank-one Cholesky update L*L' +/- x*x' @@ -904,8 +894,8 @@ TEST_F(EngineUtilSolveTest, MjuCholUpdate) { // Test for mju_cholUpdateSparse: sparse rank-one Cholesky update L'*L +/- x*x' // Uses sparse reverse Cholesky factorization and sparse rank-one update. TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) { - mjModel* model = LoadModelFromString(""); - mjData* d = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr d = MakeData(model); std::mt19937_64 rng; rng.seed(123); @@ -987,12 +977,12 @@ TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) { // sparse Cholesky factorization (reverse-order: L'*L) mju_cholFactorSparse(L_sparse.data(), n, 0, rownnz.data(), rowadr.data(), - colind.data(), d); + colind.data(), d.get()); // apply sparse rank-one update int rank_sparse = mju_cholUpdateSparse( L_sparse.data(), x_sparse.data(), n, flg_plus, rownnz.data(), - rowadr.data(), colind.data(), x_nnz, x_ind.data(), d); + rowadr.data(), colind.data(), x_nnz, x_ind.data(), d.get()); EXPECT_EQ(rank_sparse, n) << "Sparse update rank loss for n=" << n << ", flg_plus=" << flg_plus; @@ -1014,16 +1004,13 @@ TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) { } } } - - mj_deleteData(d); - mj_deleteModel(model); } // Test that mju_cholFactorSymbolic + mju_cholFactorNumeric produces identical // results to the reference implementation mju_cholFactorSparse TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) { - mjModel* model = LoadModelFromString(""); - mjData* d = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr d = MakeData(model); // test matrix with fill-in: upper triangle structure int n = 4; @@ -1044,13 +1031,13 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) { int LT_rownnz[4], LT_rowadr[4]; int nnz = mju_cholFactorSymbolic(nullptr, L_rownnz, L_rowadr, nullptr, LT_rownnz, LT_rowadr, nullptr, HT_rownnz, - HT_rowadr, HT_colind, n, d); + HT_rowadr, HT_colind, n, d.get()); // filling phase: compute L_colind, LT_colind, and LT_pos int L_colind[16], LT_colind[16], LT_pos[16]; mju_cholFactorSymbolic(L_colind, L_rownnz, L_rowadr, LT_colind, LT_rownnz, LT_rowadr, LT_pos, HT_rownnz, HT_rowadr, HT_colind, n, - d); + d.get()); // verify LT structure matches what we'd get from a separate transpose int LT_rownnz_ref[4], LT_rowadr_ref[4], LT_colind_ref[16]; @@ -1080,7 +1067,7 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) { mjtNum L_new[16]; int rank_new = mju_cholFactorNumeric( L_new, n, 1e-10, L_rownnz, L_rowadr, L_colind, LT_rownnz, LT_rowadr, - LT_colind, LT_pos, sparseH, H_rownnz, H_rowadr, H_colind, d); + LT_colind, LT_pos, sparseH, H_rownnz, H_rowadr, H_colind, d.get()); // reference implementation: copy sparse H into L_ref, then factor in-place mjtNum L_ref[16]; @@ -1099,7 +1086,7 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) { L_ref_rownnz[r] = lower_nnz; } int rank_ref = mju_cholFactorSparse(L_ref, n, 1e-10, L_ref_rownnz, L_rowadr, - L_ref_colind, d); + L_ref_colind, d.get()); // compare results EXPECT_EQ(rank_new, rank_ref); @@ -1110,9 +1097,6 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) { for (int i = 0; i < nnz; i++) { EXPECT_NEAR(L_new[i], L_ref[i], eps) << "mismatch at index " << i; } - - mj_deleteData(d); - mj_deleteModel(model); } // ----------------------------- dense LU -------------------------------------- @@ -1122,11 +1106,8 @@ using DenseLUTest = MujocoTest; // factor identity, solve recovers b exactly TEST_F(DenseLUTest, Identity) { constexpr int n = 4; - mjtNum A[n*n] = { - 1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1, + mjtNum A[n * n] = { + 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }; int pivot[n]; mjtNum b[n] = {1, 2, 3, 4}; @@ -1145,10 +1126,8 @@ TEST_F(DenseLUTest, SmallKnown) { constexpr int n = 3; // A = [2 1 1; 4 3 3; 8 7 9], b = [1; 1; 1] // solution: x = [1; -1; 0] (verified: A*x = [2-1; 4-3; 8-7] = [1;1;1]) - mjtNum A[n*n] = { - 2, 1, 1, - 4, 3, 3, - 8, 7, 9, + mjtNum A[n * n] = { + 2, 1, 1, 4, 3, 3, 8, 7, 9, }; int pivot[n]; mjtNum b[n] = {1, 1, 1}; @@ -1158,9 +1137,9 @@ TEST_F(DenseLUTest, SmallKnown) { mju_solveLU(x, A, b, pivot, n); mjtNum eps = MjTol(1e-14, 1e-6); - EXPECT_NEAR(x[0], 1, eps); + EXPECT_NEAR(x[0], 1, eps); EXPECT_NEAR(x[1], -1, eps); - EXPECT_NEAR(x[2], 0, eps); + EXPECT_NEAR(x[2], 0, eps); } // random SPD matrices: compare LU solve against Cholesky solve @@ -1183,7 +1162,7 @@ TEST_F(DenseLUTest, RandomSPD) { mju_mulMatTMat(A.data(), sqrtH.data(), sqrtH.data(), n, n, n); // add diagonal regularizer - for (int i = 0; i < n; i++) A[i*n+i] += n; + for (int i = 0; i < n; i++) A[i * n + i] += n; // generate random rhs for (int i = 0; i < n; i++) b[i] = dist(rng); @@ -1202,8 +1181,7 @@ TEST_F(DenseLUTest, RandomSPD) { // compare mjtNum eps = MjTol(1e-15, 1e-7); EXPECT_THAT(AsVector(x_lu.data(), n), - Pointwise(MjNear(eps, eps), - AsVector(x_chol.data(), n))); + Pointwise(MjNear(eps, eps), AsVector(x_chol.data(), n))); } } @@ -1224,9 +1202,9 @@ TEST_F(DenseLUTest, RandomGeneral) { // random non-symmetric matrix with diagonal dominance for (int i = 0; i < n; i++) { for (int j = 0; j < n; j++) { - A[i*n+j] = dist(rng); + A[i * n + j] = dist(rng); } - A[i*n+i] += 2 * n; + A[i * n + i] += 2 * n; } mju_copy(A_orig.data(), A.data(), n * n); @@ -1251,7 +1229,7 @@ TEST_F(DenseLUTest, RandomGeneral) { TEST_F(DenseLUTest, Singular) { constexpr int n = 3; // all zeros: maximally singular - mjtNum A[n*n] = {0}; + mjtNum A[n * n] = {0}; int pivot[n]; EXPECT_EQ(mju_factorLU(A, n, pivot), 0); diff --git a/test/engine/engine_util_sparse_test.cc b/test/engine/engine_util_sparse_test.cc index 4e0dd3de..f2b7a6a2 100644 --- a/test/engine/engine_util_sparse_test.cc +++ b/test/engine/engine_util_sparse_test.cc @@ -29,8 +29,8 @@ namespace { // permute the rows and columns of a dense matrix inline void PermuteMat(mjtNum* res, const mjtNum* mat, int nr, int nc, - const int* perm_r, const int* perm_c, - bool scatter_r, bool scatter_c) { + const int* perm_r, const int* perm_c, bool scatter_r, + bool scatter_c) { for (int r = 0; r < nr; r++) { for (int c = 0; c < nc; c++) { if (scatter_r && scatter_c) { @@ -54,22 +54,22 @@ using ::testing::ElementsAre; using EngineUtilSparseTest = MujocoTest; TEST_F(EngineUtilSparseTest, MjuDot) { - mjtNum a[] = {2, 3, 4, 5, 6, 7, 8}; + mjtNum a[] = {2, 3, 4, 5, 6, 7, 8}; mjtNum b[] = {8, 1, 7, 1, 1, 6, 1, 1, 1, 5, 1, 1, 1, 4, 1, 1, 3, 1, 2}; int i[] = {0, 2, 5, 9, 13, 16, 18}; // test various vector lengths as mju_dotSparse adds numbers in groups of four EXPECT_EQ(mju_dotSparse(a, b, 0, i), 0); - EXPECT_EQ(mju_dotSparse(a, b, 1, i), 2*8); - EXPECT_EQ(mju_dotSparse(a, b, 2, i), 2*8 + 3*7); - EXPECT_EQ(mju_dotSparse(a, b, 3, i), 2*8 + 3*7 + 4*6); - EXPECT_EQ(mju_dotSparse(a, b, 4, i), 2*8 + 3*7 + 4*6 + 5*5); - EXPECT_EQ(mju_dotSparse(a, b, 5, i), 2*8 + 3*7 + 4*6 + 5*5 + 6*4); + EXPECT_EQ(mju_dotSparse(a, b, 1, i), 2 * 8); + EXPECT_EQ(mju_dotSparse(a, b, 2, i), 2 * 8 + 3 * 7); + EXPECT_EQ(mju_dotSparse(a, b, 3, i), 2 * 8 + 3 * 7 + 4 * 6); + EXPECT_EQ(mju_dotSparse(a, b, 4, i), 2 * 8 + 3 * 7 + 4 * 6 + 5 * 5); + EXPECT_EQ(mju_dotSparse(a, b, 5, i), 2 * 8 + 3 * 7 + 4 * 6 + 5 * 5 + 6 * 4); EXPECT_EQ(mju_dotSparse(a, b, 6, i), - 2*8 + 3*7 + 4*6 + 5*5 + 6*4 + 7*3); + 2 * 8 + 3 * 7 + 4 * 6 + 5 * 5 + 6 * 4 + 7 * 3); EXPECT_EQ(mju_dotSparse(a, b, 7, i), - 2*8 + 3*7 + 4*6 + 5*5 + 6*4 + 7*3 + 8*2); + 2 * 8 + 3 * 7 + 4 * 6 + 5 * 5 + 6 * 4 + 7 * 3 + 8 * 2); } TEST_F(EngineUtilSparseTest, MjuDot2) { @@ -77,12 +77,12 @@ TEST_F(EngineUtilSparseTest, MjuDot2) { constexpr int bnnz = 5; // values - mjtNum a[annz] = {2, 3, 4, 5, 6}; - mjtNum b[bnnz] = { 8, 7, 6, 5, 4}; + mjtNum a[annz] = {2, 3, 4, 5, 6}; + mjtNum b[bnnz] = {8, 7, 6, 5, 4}; // indices - int ia[annz] = {0, 2, 5, 6, 7}; - int ib[bnnz] = { 1, 2, 3, 5, 7}; + int ia[annz] = {0, 2, 5, 6, 7}; + int ib[bnnz] = {1, 2, 3, 5, 7}; EXPECT_EQ(mju_dotSparse2(a, ia, annz, b, ib, bnnz), 3 * 7 + 4 * 5 + 6 * 4); } @@ -91,71 +91,79 @@ TEST_F(EngineUtilSparseTest, CombineSparseCount) { { std::array a_ind{0, 1}; std::array b_ind{2}; - EXPECT_EQ(mju_combineSparseCount( - a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 3); + EXPECT_EQ(mju_combineSparseCount(a_ind.size(), b_ind.size(), a_ind.data(), + b_ind.data()), + 3); } { std::array a_ind{2}; std::array b_ind{0, 1}; - EXPECT_EQ(mju_combineSparseCount( - a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 3); + EXPECT_EQ(mju_combineSparseCount(a_ind.size(), b_ind.size(), a_ind.data(), + b_ind.data()), + 3); } { std::array a_ind{0, 1}; std::array b_ind{2, 3, 4}; - EXPECT_EQ(mju_combineSparseCount( - a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 5); + EXPECT_EQ(mju_combineSparseCount(a_ind.size(), b_ind.size(), a_ind.data(), + b_ind.data()), + 5); } { std::array a_ind{5, 6}; std::array b_ind{1, 3, 8}; - EXPECT_EQ(mju_combineSparseCount( - a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 5); + EXPECT_EQ(mju_combineSparseCount(a_ind.size(), b_ind.size(), a_ind.data(), + b_ind.data()), + 5); } { std::array a_ind{1, 2, 3}; std::array b_ind{0, 4}; - EXPECT_EQ(mju_combineSparseCount( - a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 5); + EXPECT_EQ(mju_combineSparseCount(a_ind.size(), b_ind.size(), a_ind.data(), + b_ind.data()), + 5); } { std::array a_ind{1, 4}; std::array b_ind{2, 3}; - EXPECT_EQ(mju_combineSparseCount( - a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 4); + EXPECT_EQ(mju_combineSparseCount(a_ind.size(), b_ind.size(), a_ind.data(), + b_ind.data()), + 4); } { std::array a_ind{0, 1, 3}; std::array b_ind{0, 3, 4}; - EXPECT_EQ(mju_combineSparseCount( - a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 4); + EXPECT_EQ(mju_combineSparseCount(a_ind.size(), b_ind.size(), a_ind.data(), + b_ind.data()), + 4); } { std::array a_ind{1, 3, 5, 6}; std::array b_ind{1, 3, 5, 6}; - EXPECT_EQ(mju_combineSparseCount( - a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 4); + EXPECT_EQ(mju_combineSparseCount(a_ind.size(), b_ind.size(), a_ind.data(), + b_ind.data()), + 4); } EXPECT_EQ(mju_combineSparseCount(0, 0, nullptr, nullptr), 0); { std::array b_ind{1, 2}; - EXPECT_EQ( - mju_combineSparseCount(0, b_ind.size(), nullptr, b_ind.data()), 2); + EXPECT_EQ(mju_combineSparseCount(0, b_ind.size(), nullptr, b_ind.data()), + 2); } { std::array a_ind{0}; - EXPECT_EQ( - mju_combineSparseCount(a_ind.size(), 0, a_ind.data(), nullptr), 1); + EXPECT_EQ(mju_combineSparseCount(a_ind.size(), 0, a_ind.data(), nullptr), + 1); } } @@ -264,7 +272,7 @@ TEST_F(EngineUtilSparseTest, MjuTransposeSuper) { // super: 0, 1, 0, 0, 2, 1, 0, 2, 1, 0, 2, 1, 0 mjtNum mat[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}; - int colind[] = {1, 2, 4, 5, 6, 3, 4, 5, 6, 7, 8, 9}; + int colind[] = {1, 2, 4, 5, 6, 3, 4, 5, 6, 7, 8, 9}; int rownnz[] = {5, 7}; int rowadr[] = {0, 5}; @@ -277,11 +285,11 @@ TEST_F(EngineUtilSparseTest, MjuTransposeSuper) { mju_transposeSparse(matT, mat, 2, 13, rownnzT, rowadrT, colindT, rowsuperT, rownnz, rowadr, colind); - EXPECT_THAT(matT, ElementsAre(1, 2, 6, 3, 7, 4, 8, 5, 9, 10, 11, 12)); - EXPECT_THAT(colindT, ElementsAre(0, 0, 1, 0, 1, 0, 1, 0, 1, 1, 1, 1)); - EXPECT_THAT(rownnzT, ElementsAre(0, 1, 1, 1, 2, 2, 2, 1, 1, 1, 0, 0, 0)); + EXPECT_THAT(matT, ElementsAre(1, 2, 6, 3, 7, 4, 8, 5, 9, 10, 11, 12)); + EXPECT_THAT(colindT, ElementsAre(0, 0, 1, 0, 1, 0, 1, 0, 1, 1, 1, 1)); + EXPECT_THAT(rownnzT, ElementsAre(0, 1, 1, 1, 2, 2, 2, 1, 1, 1, 0, 0, 0)); EXPECT_THAT(rowadrT, ElementsAre(0, 0, 1, 2, 3, 5, 7, 9, 10, 11, 12, 12, 12)); - EXPECT_THAT(rowsuperT, ElementsAre(0, 1, 0, 0, 2, 1, 0, 2, 1, 0, 2, 1, 0)); + EXPECT_THAT(rowsuperT, ElementsAre(0, 1, 0, 0, 2, 1, 0, 2, 1, 0, 2, 1, 0)); } TEST_F(EngineUtilSparseTest, MjuTranspose1by1) { @@ -330,7 +338,7 @@ TEST_F(EngineUtilSparseTest, MjuCompressSparse) { // sparse matrix (uncompressed with spurious values between the rows): // [[1, 0, 2] // [0, O, 3] (second zero represented) - mjtNum mat[] = {1, 2, 9, 0, 3}; // spurious 9 value + mjtNum mat[] = {1, 2, 9, 0, 3}; // spurious 9 value int colind[] = {0, 2, -1, 1, 2}; // spurious -1 index int rownnz[] = {2, 2}; int rowadr[] = {0, 3}; @@ -341,20 +349,20 @@ TEST_F(EngineUtilSparseTest, MjuCompressSparse) { EXPECT_EQ(AsVector(dense, 6), AsVector(dense_expected, 6)); // check that spurious values are removed - int nnz = mju_compressSparse(mat, 2, 3, rownnz, rowadr, colind, - /*minval=*/-1); + int nnz = mju_compressSparse(mat, 2, 3, rownnz, rowadr, colind, + /*minval=*/-1); EXPECT_EQ(nnz, 4); mju_sparse2dense(dense, mat, 2, 3, rownnz, rowadr, colind); EXPECT_EQ(AsVector(dense, 6), AsVector(dense_expected, 6)); // check that represented zero gets compressed aways with minval=0 - nnz = mju_compressSparse(mat, 2, 3, rownnz, rowadr, colind, /*minval=*/0); + nnz = mju_compressSparse(mat, 2, 3, rownnz, rowadr, colind, /*minval=*/0); EXPECT_EQ(nnz, 3); mju_sparse2dense(dense, mat, 2, 3, rownnz, rowadr, colind); EXPECT_EQ(AsVector(dense, 6), AsVector(dense_expected, 6)); // check that 1 gets compressed aways with minval=1 - nnz = mju_compressSparse(mat, 2, 3, rownnz, rowadr, colind, /*minval=*/1); + nnz = mju_compressSparse(mat, 2, 3, rownnz, rowadr, colind, /*minval=*/1); EXPECT_EQ(nnz, 2); mju_sparse2dense(dense, mat, 2, 3, rownnz, rowadr, colind); mjtNum dense_expected_minval1[] = {0, 0, 2, 0, 0, 3}; @@ -391,47 +399,44 @@ TEST_F(EngineUtilSparseTest, MjuSym2DenseWithUpper) { mjtNum dense[9]; mju_sym2dense(dense, mat, 3, rownnz, rowadr, colind); - mjtNum expected[] = {1, 2, 0, - 2, 3, 4, - 0, 4, 5}; + mjtNum expected[] = {1, 2, 0, 2, 3, 4, 0, 4, 5}; EXPECT_EQ(AsVector(dense, 9), AsVector(expected, 9)); } // helper: run split-col approach and return dense result -static void SqrMatTDSplitCol( - std::vector& dense_result, int nr, int nc, - const mjtNum* mat, const int* rownnz, const int* rowadr, const int* colind, - const mjtNum* matT, const int* rownnzT, const int* rowadrT, - const int* colindT, const int* rowsuperT, const mjtNum* diag, - int* out_diagind, mjData* d) { +static void SqrMatTDSplitCol(std::vector& dense_result, int nr, int nc, + const mjtNum* mat, const int* rownnz, + const int* rowadr, const int* colind, + const mjtNum* matT, const int* rownnzT, + const int* rowadrT, const int* colindT, + const int* rowsuperT, const mjtNum* diag, + int* out_diagind, mjData* d) { // count mode std::vector H_rownnz(nc, 0); std::vector H_rowadr(nc, 0); int nnz = mju_sqrMatTDSparseSymbolic( - H_rownnz.data(), H_rowadr.data(), nullptr, - out_diagind, nr, nc, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, d); + H_rownnz.data(), H_rowadr.data(), nullptr, out_diagind, nr, nc, rownnz, + rowadr, colind, rownnzT, rowadrT, colindT, rowsuperT, d); // fill mode std::vector H_colind(nnz); - mju_sqrMatTDSparseSymbolic( - H_rownnz.data(), H_rowadr.data(), H_colind.data(), - out_diagind, nr, nc, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, d); + mju_sqrMatTDSparseSymbolic(H_rownnz.data(), H_rowadr.data(), H_colind.data(), + out_diagind, nr, nc, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, rowsuperT, d); // numeric phase std::vector H(nnz, 0); - mju_sqrMatTDSparseNumeric( - H.data(), nc, H_rownnz.data(), H_rowadr.data(), - H_colind.data(), out_diagind, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, rowsuperT, diag, d); + mju_sqrMatTDSparseNumeric(H.data(), nc, H_rownnz.data(), H_rowadr.data(), + H_colind.data(), out_diagind, mat, rownnz, rowadr, + colind, matT, rownnzT, rowadrT, colindT, rowsuperT, + diag, d); // densify dense_result.assign(nc * nc, 0); for (int r = 0; r < nc; r++) { for (int j = 0; j < H_rownnz[r]; j++) { int c = H_colind[H_rowadr[r] + j]; - dense_result[r*nc + c] = H[H_rowadr[r] + j]; + dense_result[r * nc + c] = H[H_rowadr[r] + j]; } } } @@ -441,8 +446,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse1) { // M = 0 0 0 // 0 0 0 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; int colind[] = {0, 1, 2, 0, 1, 2, 0, 1, 2}; @@ -456,14 +461,10 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse1) { int diagindH[3]; std::vector dense; - SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, nullptr, nullptr, - diagindH, data); + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, nullptr, nullptr, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(0, 0, 0, 0, 0, 0, 0, 0, 0)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseLower) { @@ -471,8 +472,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseLower) { // M = 2 -1 2 // 2 2 3 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {2, -1, 1, 2, -1, 2, 2, 2, 3}; int colind[] = {0, 1, 2, 0, 1, 2, 0, 1, 2}; @@ -485,14 +486,10 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseLower) { int rowadrT[] = {0, 3, 6}; std::vector dense; - SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, nullptr, nullptr, - nullptr, data); + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, nullptr, nullptr, nullptr, data.get()); EXPECT_THAT(dense, ElementsAre(12, 0, 0, 0, 6, 0, 12, 3, 14)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse2) { @@ -500,8 +497,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse2) { // M = 2 -1 2 // 2 2 3 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {2, -1, 1, 2, -1, 2, 2, 2, 3}; int colind[] = {0, 1, 2, 0, 1, 2, 0, 1, 2}; @@ -515,14 +512,10 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse2) { int diagindH[3]; std::vector dense; - SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, nullptr, nullptr, - diagindH, data); + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, nullptr, nullptr, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(12, 0, 12, 0, 6, 3, 12, 3, 14)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3) { @@ -530,8 +523,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3) { // M = 0 3 0 // 4 0 0 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {1, 2, 3, 4}; int colind[] = {0, 1, 1, 0}; @@ -547,14 +540,10 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3) { int diagindH[3]; std::vector dense; - SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, nullptr, diag, - diagindH, data); + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, nullptr, diag, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(66, 4, 0, 4, 35, 0, 0, 0, 0)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3b) { @@ -562,8 +551,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3b) { // M = 0 3 4 // 5 0 0 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {1, 2, 3, 4, 5}; int colind[] = {0, 1, 1, 2, 0}; @@ -579,14 +568,10 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3b) { int diagindH[3]; std::vector dense; - SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, nullptr, diag, - diagindH, data); + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, nullptr, diag, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(26, 2, 0, 2, 13, 12, 0, 12, 16)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse4) { @@ -594,8 +579,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse4) { // M = 0 0 3 // 4 0 0 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {1, 2, 3, 4}; int colind[] = {0, 2, 2, 0}; @@ -611,14 +596,10 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse4) { int diagindH[3]; std::vector dense; - SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, nullptr, diag, - diagindH, data); + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, nullptr, diag, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(66, 0, 4, 0, 0, 0, 4, 0, 35)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse5) { @@ -626,8 +607,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse5) { // M = 0 0 0 // 2 3 0 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {1, 4, 2, 3}; int colind[] = {0, 2, 0, 1}; @@ -641,14 +622,10 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse5) { int diagindH[3]; std::vector dense; - SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, nullptr, nullptr, - diagindH, data); + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, nullptr, nullptr, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(5, 6, 4, 6, 9, 0, 4, 0, 16)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse6) { @@ -656,8 +633,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse6) { // M = 0 2 0 // 0 0 3 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {1, 2, 2, 3}; int colind[] = {0, 2, 1, 2}; @@ -671,14 +648,10 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse6) { int diagindH[3]; std::vector dense; - SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, nullptr, nullptr, - diagindH, data); + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, nullptr, nullptr, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(1, 0, 2, 0, 4, 0, 2, 0, 13)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse7) { @@ -686,8 +659,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse7) { // M = 0 3 // 4 0 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {1, 2, 3, 4}; int colind[] = {0, 1, 1, 0}; @@ -703,22 +676,18 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse7) { int diagindH[2]; std::vector dense; - SqrMatTDSplitCol(dense, 3, 2, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, nullptr, diag, - diagindH, data); + SqrMatTDSplitCol(dense, 3, 2, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, nullptr, diag, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(66, 4, 4, 35)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse8) { // M = 1 0 4 // 2 3 0 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {1, 4, 2, 3}; int colind[] = {0, 2, 0, 1}; @@ -734,14 +703,10 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse8) { int diagindH[3]; std::vector dense; - SqrMatTDSplitCol(dense, 2, 3, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, nullptr, diag, - diagindH, data); + SqrMatTDSplitCol(dense, 2, 3, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, nullptr, diag, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(14, 18, 8, 18, 27, 0, 8, 0, 32)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse9) { @@ -749,8 +714,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse9) { // M = 1 3 4 // 4 4 4 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {1, 2, 2, 1, 3, 4, 4, 4, 4}; int colind[] = {0, 1, 2, 0, 1, 2, 0, 1, 2}; @@ -766,14 +731,10 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse9) { int diagindH[3]; std::vector dense; - SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, nullptr, diag, - diagindH, data); + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, nullptr, diag, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(69, 77, 80, 77, 99, 108, 80, 108, 120)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse10) { @@ -781,8 +742,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse10) { // M = 2 3 2 // 3 1 1 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {1, 2, 3, 2, 3, 2, 3, 1, 1}; int colind[] = {0, 1, 2, 0, 1, 2, 0, 1, 2}; @@ -799,14 +760,10 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse10) { int diagindH[3]; std::vector dense; - SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, rowsuperT, diag, - diagindH, data); + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, rowsuperT, diag, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(18, 17, 14, 17, 23, 19, 14, 19, 18)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse11) { @@ -814,8 +771,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse11) { // M = 0 0 0 // 0 3 3 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {1, 1, 1, 3, 3}; int colind[] = {0, 1, 2, 1, 2}; @@ -832,14 +789,10 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse11) { int diagindH[3]; std::vector dense; - SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, rowsuperT, diag, - diagindH, data); + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, rowsuperT, diag, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(1, 1, 1, 1, 10, 10, 1, 10, 10)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse12) { @@ -847,8 +800,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse12) { // M = 0 0 0 0 // 0 0 3 3 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {1, 1, 1, 1, 3, 3}; int colind[] = {0, 1, 2, 3, 2, 3}; @@ -865,15 +818,11 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse12) { int diagindH[4]; std::vector dense; - SqrMatTDSplitCol(dense, 3, 4, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, rowsuperT, diag, - diagindH, data); + SqrMatTDSplitCol(dense, 3, 4, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, rowsuperT, diag, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 10, 10, 1, 1, 10, 10)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse13) { @@ -881,8 +830,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse13) { // M = 1 1 0 0 0 // 1 1 0 0 0 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {1, 1, 1, 1, 1, 1}; int colind[] = {0, 1, 0, 1, 0, 1}; @@ -899,22 +848,18 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse13) { int diagindH[5]; std::vector dense; - SqrMatTDSplitCol(dense, 3, 5, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, rowsuperT, diag, - diagindH, data); + SqrMatTDSplitCol(dense, 3, 5, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, rowsuperT, diag, diagindH, data.get()); EXPECT_THAT(dense, ElementsAre(3, 3, 0, 0, 0, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse14) { // M = 1 1 1 1 2 2 2 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); mjtNum mat[] = {1, 1, 1, 1, 2, 2, 2}; int colind[] = {0, 1, 2, 3, 4, 5, 6}; @@ -929,17 +874,13 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse14) { int diagindH[7]; std::vector dense; - SqrMatTDSplitCol(dense, 1, 7, mat, rownnz, rowadr, colind, - matT, rownnzT, rowadrT, colindT, rowsuperT, nullptr, - diagindH, data); + SqrMatTDSplitCol(dense, 1, 7, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, rowsuperT, nullptr, diagindH, data.get()); - EXPECT_THAT( - dense, ElementsAre(1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2, 2, 2, 1, 1, 1, - 1, 2, 2, 2, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 4, 4, - 4, 2, 2, 2, 2, 4, 4, 4, 2, 2, 2, 2, 4, 4, 4)); - - mj_deleteData(data); - mj_deleteModel(model); + EXPECT_THAT(dense, + ElementsAre(1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, + 2, 2, 2, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 4, 4, 4, 2, + 2, 2, 2, 4, 4, 4, 2, 2, 2, 2, 4, 4, 4)); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolic) { @@ -949,8 +890,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolic) { // // M'M (lower triangle) should have 3 elements: (0,0), (1,0), (1,1) - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); // M in CSR: row 0 has cols 0,1; row 1 has cols 0,1 int colind[] = {0, 1, 0, 1}; @@ -971,7 +912,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolic) { int rowadrH_expected[] = {0, 0}; int nnz_expected = mju_sqrMatTDSparseCount( rownnzH_expected, rowadrH_expected, 2, rownnz, rowadr, colind, rownnzT, - rowadrT, colindT, nullptr, data, /*flg_upper=*/0); + rowadrT, colindT, nullptr, data.get(), /*flg_upper=*/0); // verify: lower triangle should have 3 elements: (0,0), (1,0), (1,1) EXPECT_EQ(nnz_expected, 3); @@ -982,9 +923,9 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolic) { int rownnzH[] = {0, 0}; int rowadrH[] = {0, 0}; - int nnz = mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, nullptr, nullptr, - 2, 2, rownnz, rowadr, colind, rownnzT, - rowadrT, colindT, nullptr, data); + int nnz = mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, nullptr, nullptr, 2, 2, + rownnz, rowadr, colind, rownnzT, rowadrT, + colindT, nullptr, data.get()); EXPECT_EQ(nnz, nnz_expected); EXPECT_THAT(rownnzH, ElementsAre(rownnzH_expected[0], rownnzH_expected[1])); @@ -994,22 +935,19 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolic) { std::vector colindH(nnz, -1); mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, colindH.data(), nullptr, 2, 2, - rownnz, rowadr, colind, rownnzT, rowadrT, - colindT, nullptr, data); + rownnz, rowadr, colind, rownnzT, rowadrT, colindT, + nullptr, data.get()); // verify: row 0 should have {0}, row 1 should have {0, 1} EXPECT_THAT(colindH, ElementsAre(0, 0, 1)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolicUpper) { // Test flg_upper=1: count both lower and upper triangle // Same matrix as previous test - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); int colind[] = {0, 1, 0, 1}; int rownnz[] = {2, 2}; @@ -1028,30 +966,27 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolicUpper) { int rowadrH_expected[] = {0, 0}; int nnz_expected = mju_sqrMatTDSparseCount( rownnzH_expected, rowadrH_expected, 2, rownnz, rowadr, colind, rownnzT, - rowadrT, colindT, nullptr, data, /*flg_upper=*/1); + rowadrT, colindT, nullptr, data.get(), /*flg_upper=*/1); // test new function with diagind (upper triangle) int rownnzH[] = {0, 0}; int rowadrH[] = {0, 0}; int diagindH[] = {0, 0}; - int nnz = mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, nullptr, diagindH, - 2, 2, rownnz, rowadr, colind, rownnzT, - rowadrT, colindT, nullptr, data); + int nnz = mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, nullptr, diagindH, 2, + 2, rownnz, rowadr, colind, rownnzT, + rowadrT, colindT, nullptr, data.get()); EXPECT_EQ(nnz, nnz_expected); EXPECT_THAT(rownnzH, ElementsAre(rownnzH_expected[0], rownnzH_expected[1])); EXPECT_THAT(rowadrH, ElementsAre(rowadrH_expected[0], rowadrH_expected[1])); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolicSupernode) { // Test supernode exploitation with a matrix that has supernodes // M has two rows with identical sparsity pattern - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); // 3x2 matrix where rows 1 and 2 have same pattern // 1 0 @@ -1078,14 +1013,14 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolicSupernode) { int rowadrH_expected[] = {0, 0}; int nnz_expected = mju_sqrMatTDSparseCount( rownnzH_expected, rowadrH_expected, 2, rownnz, rowadr, colind, rownnzT, - rowadrT, colindT, rowsuperT, data, /*flg_upper=*/0); + rowadrT, colindT, rowsuperT, data.get(), /*flg_upper=*/0); // test new function with supernodes int rownnzH[] = {0, 0}; int rowadrH[] = {0, 0}; - int nnz = mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, nullptr, nullptr, - 3, 2, rownnz, rowadr, colind, rownnzT, - rowadrT, colindT, rowsuperT, data); + int nnz = mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, nullptr, nullptr, 3, 2, + rownnz, rowadr, colind, rownnzT, rowadrT, + colindT, rowsuperT, data.get()); EXPECT_EQ(nnz, nnz_expected); EXPECT_THAT(rownnzH, ElementsAre(rownnzH_expected[0], rownnzH_expected[1])); @@ -1094,8 +1029,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolicSupernode) { // test fill mode with supernodes std::vector colindH(nnz, -1); mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, colindH.data(), nullptr, 3, 2, - rownnz, rowadr, colind, rownnzT, rowadrT, - colindT, rowsuperT, data); + rownnz, rowadr, colind, rownnzT, rowadrT, colindT, + rowsuperT, data.get()); // verify all filled for (int i = 0; i < nnz; i++) { @@ -1107,7 +1042,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolicSupernode) { mjtNum diag[] = {1, 1, 1, 1, 1}; // dummy diagonal mju_sqrMatTDSparseNumeric(resH.data(), 2, rownnzH, rowadrH, colindH.data(), nullptr, mat, rownnz, rowadr, colind, matT, rownnzT, - rowadrT, colindT, rowsuperT, diag, data); + rowadrT, colindT, rowsuperT, diag, data.get()); // ground truth numeric std::vector res_expected(4); @@ -1117,7 +1052,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolicSupernode) { mju_sqrMatTDSparse(res_expected.data(), mat, matT, diag, 3, 2, rownnzH_exp, rowadrH_exp, colindH_expected.data(), rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, rowsuperT, - data, nullptr); + data.get(), nullptr); // compare values (sparse result vs sparse ground truth) for (int r = 0; r < 2; r++) { @@ -1138,9 +1073,6 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolicSupernode) { << " not found in ground truth for row " << r; } } - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseNumeric) { @@ -1148,8 +1080,8 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseNumeric) { // 1 2 // M = 3 4 - mjModel* model = LoadModelFromString(""); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(""); + MjDataPtr data = MakeData(model); int colind[] = {0, 1, 0, 1}; int rownnz[] = {2, 2}; @@ -1179,19 +1111,20 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseNumeric) { int diagindH[] = {0, 0}; int nnz = mju_sqrMatTDSparseSymbolic( rownnzH, rowadrH, nullptr, use_diagind ? diagindH : nullptr, 2, 2, - rownnz, rowadr, colind, rownnzT, rowadrT, colindT, nullptr, data); + rownnz, rowadr, colind, rownnzT, rowadrT, colindT, nullptr, data.get()); std::vector colindH(nnz); - mju_sqrMatTDSparseSymbolic( - rownnzH, rowadrH, colindH.data(), use_diagind ? diagindH : nullptr, 2, - 2, rownnz, rowadr, colind, rownnzT, rowadrT, colindT, nullptr, data); + mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, colindH.data(), + use_diagind ? diagindH : nullptr, 2, 2, rownnz, + rowadr, colind, rownnzT, rowadrT, colindT, + nullptr, data.get()); // compute values using numeric phase std::vector resH(nnz); - mju_sqrMatTDSparseNumeric(resH.data(), 2, rownnzH, rowadrH, - colindH.data(), use_diagind ? diagindH : nullptr, - mat, rownnz, rowadr, colind, matT, rownnzT, - rowadrT, colindT, rowsuperT, diag, data); + mju_sqrMatTDSparseNumeric(resH.data(), 2, rownnzH, rowadrH, colindH.data(), + use_diagind ? diagindH : nullptr, mat, rownnz, + rowadr, colind, matT, rownnzT, rowadrT, colindT, + rowsuperT, diag, data.get()); // compute ground truth using existing mju_sqrMatTDSparse // use uncompressed storage to give the old function enough room @@ -1203,7 +1136,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseNumeric) { mju_sqrMatTDSparse(res_expected.data(), mat, matT, diag, 2, 2, rownnzH_exp, rowadrH_exp, colindH_expected.data(), rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, nullptr, - data, use_diagind ? diagind_exp : nullptr); + data.get(), use_diagind ? diagind_exp : nullptr); // check that rownnz matches (nnz may differ due to compressed vs // uncompressed storage) @@ -1226,16 +1159,12 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseNumeric) { } } } - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(EngineUtilSparseTest, MjuMulMatTVec) { int nr = 2; int nc = 3; - mjtNum mat[] = {1, 2, 0, - 0, 3, 4}; + mjtNum mat[] = {1, 2, 0, 0, 3, 4}; mjtNum mat_sparse[4]; int rownnz[2]; @@ -1265,40 +1194,26 @@ TEST_F(EngineUtilSparseTest, MjuAddToSymSparse) { // 0 0 0 // A = 5 4 2 // 4 3 2 - mjtNum A[] = {0, 0, 0, - 5, 4, 2, - 4, 3, 2}; + mjtNum A[] = {0, 0, 0, 5, 4, 2, 4, 3, 2}; mju_addToSymSparse(A, mat, 3, rownnz, rowadr, colind, /*flg_upper=*/1); - EXPECT_THAT(AsVector(A, 9), ElementsAre(1, 2, 4, - 7, 7, 2, - 8, 3, 7)); + EXPECT_THAT(AsVector(A, 9), ElementsAre(1, 2, 4, 7, 7, 2, 8, 3, 7)); // same as A - mjtNum B[] = {0, 0, 0, - 5, 4, 2, - 4, 3, 2}; + mjtNum B[] = {0, 0, 0, 5, 4, 2, 4, 3, 2}; mju_addToSymSparse(B, mat, 3, rownnz, rowadr, colind, /*flg_upper=*/0); - EXPECT_THAT(AsVector(B, 9), ElementsAre(1, 0, 0, - 7, 7, 2, - 8, 3, 7)); + EXPECT_THAT(AsVector(B, 9), ElementsAre(1, 0, 0, 7, 7, 2, 8, 3, 7)); } TEST_F(EngineUtilSparseTest, MjuMulSymVecSparse) { constexpr int n = 4; constexpr int nnz = 9; - mjtNum mat[n*n] = {1, 0, 0, 0, - 0, 2, 0, 0, - 3, 0, 4, 0, - 5, 6, 7, 8}; + mjtNum mat[n * n] = {1, 0, 0, 0, 0, 2, 0, 0, 3, 0, 4, 0, 5, 6, 7, 8}; // dense, full matrix - mjtNum sym[n*n] = {1, 0, 3, 5, - 0, 2, 0, 6, - 3, 0, 4, 7, - 5, 6, 7, 8}; + mjtNum sym[n * n] = {1, 0, 3, 5, 0, 2, 0, 6, 3, 0, 4, 7, 5, 6, 7, 8}; mjtNum mat_sparse[nnz]; int rownnz[n]; @@ -1315,7 +1230,7 @@ TEST_F(EngineUtilSparseTest, MjuMulSymVecSparse) { mjtNum res2[n]; mju_mulMatVec(res2, sym, vec, n, n); - for (int i=0; i < n; i++) { + for (int i = 0; i < n; i++) { EXPECT_EQ(res[i], res2[i]); } } @@ -1323,8 +1238,7 @@ TEST_F(EngineUtilSparseTest, MjuMulSymVecSparse) { TEST_F(EngineUtilSparseTest, MjuDenseToSparse) { int nr = 2; int nc = 2; - mjtNum mat[] = {1, 2, - 0, 3}; + mjtNum mat[] = {1, 2, 0, 3}; mjtNum mat_sparse[4]; int rownnz[2]; @@ -1378,10 +1292,8 @@ TEST_F(EngineUtilSparseTest, BlockDiag) { // 4x5 matrix with 3 blocks constexpr int nr = 4; constexpr int nc = 5; - const mjtNum mat[nr * nc] = {1, 2, 0, 0, 0, - 0, 0, 3, 4, 0, - 0, 0, 5, 6, 0, - 0, 0, 0, 0, 7}; + const mjtNum mat[nr * nc] = {1, 2, 0, 0, 0, 0, 0, 3, 4, 0, + 0, 0, 5, 6, 0, 0, 0, 0, 0, 7}; // block structure constexpr int nb = 3; @@ -1394,26 +1306,18 @@ TEST_F(EngineUtilSparseTest, BlockDiag) { const int perm_r[nr] = {0, 1, 2, 3}; const int perm_c[nc] = {0, 1, 2, 3, 4}; mjtNum res[nr * nc] = {0}; - mju_blockDiag(res, mat, nc, nc, nb, - perm_r, perm_c, - block_nr, block_nc, + mju_blockDiag(res, mat, nc, nc, nb, perm_r, perm_c, block_nr, block_nc, block_r, block_c); - EXPECT_THAT(res, ElementsAre(1, 2, 0, 0, 0, - 3, 4, 5, 6, 0, - 0, 0, 0, 0, 0, - 7, 0, 0, 0, 0)); + EXPECT_THAT(res, ElementsAre(1, 2, 0, 0, 0, 3, 4, 5, 6, 0, 0, 0, 0, 0, 0, 7, + 0, 0, 0, 0)); } TEST_F(EngineUtilSparseTest, BlockDiagPerm) { // 4x5 matrix with 3 blocks constexpr int nr = 4; constexpr int nc = 5; - const mjtNum mat[nr * nc] = { - 1, 2, 0, 0, 0, - 0, 0, 3, 4, 0, - 0, 0, 5, 6, 0, - 0, 0, 0, 0, 7 - }; + const mjtNum mat[nr * nc] = {1, 2, 0, 0, 0, 0, 0, 3, 4, 0, + 0, 0, 5, 6, 0, 0, 0, 0, 0, 7}; // block structure constexpr int nb = 3; @@ -1430,26 +1334,18 @@ TEST_F(EngineUtilSparseTest, BlockDiagPerm) { // test with permutation mjtNum res[nr * nc] = {0}; - mju_blockDiag(res, mat_p, nc, nc, nb, - perm_r, perm_c, - block_nr, block_nc, + mju_blockDiag(res, mat_p, nc, nc, nb, perm_r, perm_c, block_nr, block_nc, block_r, block_c); - EXPECT_THAT(res, ElementsAre(1, 2, 0, 0, 0, - 3, 4, 5, 6, 0, - 0, 0, 0, 0, 0, - 7, 0, 0, 0, 0)); + EXPECT_THAT(res, ElementsAre(1, 2, 0, 0, 0, 3, 4, 5, 6, 0, 0, 0, 0, 0, 0, 7, + 0, 0, 0, 0)); } TEST_F(EngineUtilSparseTest, BlockDiagLessCols) { // 4x5 matrix with 3 blocks constexpr int nr = 4; constexpr int nc = 5; - const mjtNum mat[nr * nc] = { - 1, 2, 0, 0, 0, - 0, 0, 3, 4, 0, - 0, 0, 5, 6, 0, - 0, 0, 0, 0, 7 - }; + const mjtNum mat[nr * nc] = {1, 2, 0, 0, 0, 0, 0, 3, 4, 0, + 0, 0, 5, 6, 0, 0, 0, 0, 0, 7}; // block structure (ignore middle block) constexpr int nb = 2; @@ -1467,26 +1363,17 @@ TEST_F(EngineUtilSparseTest, BlockDiagLessCols) { // test with permutation and less columns (ignore middle block) constexpr int nc_res = 3; mjtNum res2[nr * nc_res] = {0}; - mju_blockDiag(res2, mat_p, nc, nc_res, nb, - perm_r, perm_c, - block_nr, block_nc, + mju_blockDiag(res2, mat_p, nc, nc_res, nb, perm_r, perm_c, block_nr, block_nc, block_r, block_c); - EXPECT_THAT(res2, ElementsAre(1, 2, 0, - 0, 0, 0, - 0, 0, 0, - 7, 0, 0)); + EXPECT_THAT(res2, ElementsAre(1, 2, 0, 0, 0, 0, 0, 0, 0, 7, 0, 0)); } TEST_F(EngineUtilSparseTest, BlockDiagSparse) { // 4x5 matrix with 3 blocks constexpr int nr = 4; constexpr int nc = 5; - const mjtNum mat[nr * nc] = { - 1, 2, 0, 0, 0, - 0, 0, 3, 4, 0, - 0, 0, 5, 6, 0, - 0, 0, 0, 0, 7 - }; + const mjtNum mat[nr * nc] = {1, 2, 0, 0, 0, 0, 0, 3, 4, 0, + 0, 0, 5, 6, 0, 0, 0, 0, 0, 7}; constexpr int nnz = 7; // block structure @@ -1508,46 +1395,36 @@ TEST_F(EngineUtilSparseTest, BlockDiagSparse) { int res_rowadr[nr]; int res_colind[nnz]; mjtNum res[nnz]; - mju_blockDiagSparse(res, res_rownnz, res_rowadr, res_colind, - mat_sparse, rownnz, rowadr, colind, nr, nb, - perm_r, perm_c, - block_r, block_c, nullptr, nullptr); + mju_blockDiagSparse(res, res_rownnz, res_rowadr, res_colind, mat_sparse, + rownnz, rowadr, colind, nr, nb, perm_r, perm_c, block_r, + block_c, nullptr, nullptr); mjtNum dense_res[nr * nc]; mju_sparse2dense(dense_res, res, nr, nc, res_rownnz, res_rowadr, res_colind); - EXPECT_THAT(dense_res, ElementsAre(1, 2, 0, 0, 0, - 3, 4, 0, 0, 0, - 5, 6, 0, 0, 0, - 7, 0, 0, 0, 0)); + EXPECT_THAT(dense_res, ElementsAre(1, 2, 0, 0, 0, 3, 4, 0, 0, 0, 5, 6, 0, 0, + 0, 7, 0, 0, 0, 0)); // permute mat into mat_p (scatter rows, gather columns) const int perm_r2[nr] = {3, 1, 0, 2}; const int perm_c2[nc] = {4, 0, 2, 1, 3}; - mjtNum mat_p[nr*nc]; + mjtNum mat_p[nr * nc]; PermuteMat(mat_p, mat, nr, nc, perm_r2, perm_c2, true, false); mju_dense2sparse(mat_sparse, mat_p, nr, nc, rownnz, rowadr, colind, nnz); // test with permutation - mju_blockDiagSparse(res, res_rownnz, res_rowadr, res_colind, - mat_sparse, rownnz, rowadr, colind, nr, nb, - perm_r2, perm_c2, - block_r, block_c, nullptr, nullptr); + mju_blockDiagSparse(res, res_rownnz, res_rowadr, res_colind, mat_sparse, + rownnz, rowadr, colind, nr, nb, perm_r2, perm_c2, block_r, + block_c, nullptr, nullptr); mju_sparse2dense(dense_res, res, nr, nc, res_rownnz, res_rowadr, res_colind); - EXPECT_THAT(dense_res, ElementsAre(1, 2, 0, 0, 0, - 3, 4, 0, 0, 0, - 5, 6, 0, 0, 0, - 7, 0, 0, 0, 0)); + EXPECT_THAT(dense_res, ElementsAre(1, 2, 0, 0, 0, 3, 4, 0, 0, 0, 5, 6, 0, 0, + 0, 7, 0, 0, 0, 0)); } TEST_F(EngineUtilSparseTest, BlockDiagSparseTranspose) { // 4x5 matrix with 3 blocks constexpr int nr = 4; constexpr int nc = 5; - const mjtNum mat[nr * nc] = { - 1, 2, 0, 0, 0, - 0, 0, 3, 4, 0, - 0, 0, 5, 6, 0, - 0, 0, 0, 0, 7 - }; + const mjtNum mat[nr * nc] = {1, 2, 0, 0, 0, 0, 0, 3, 4, 0, + 0, 0, 5, 6, 0, 0, 0, 0, 0, 7}; constexpr int nnz = 7; // block structure @@ -1571,10 +1448,9 @@ TEST_F(EngineUtilSparseTest, BlockDiagSparseTranspose) { int res_rowadr[nr]; int res_colind[nnz]; mjtNum res[nnz]; - mju_blockDiagSparse(res, res_rownnz, res_rowadr, res_colind, - mat_sparse, rownnz, rowadr, colind, nr, nb, - perm_r, perm_c, - block_r, block_c, nullptr, nullptr); + mju_blockDiagSparse(res, res_rownnz, res_rowadr, res_colind, mat_sparse, + rownnz, rowadr, colind, nr, nb, perm_r, perm_c, block_r, + block_c, nullptr, nullptr); // transpose each block mjtNum matT[nnz]; @@ -1602,10 +1478,9 @@ TEST_F(EngineUtilSparseTest, BlockDiagSparseTranspose) { int* block_res_rowadr = res_rowadr + r_offset; int* block_res_colind = res_colind + block_start_adr; - mju_transposeSparse( - matT, block_res_vals, bnr, bnc, - rownnzT, rowadrT, colindT, nullptr, - block_res_rownnz, block_res_rowadr, block_res_colind); + mju_transposeSparse(matT, block_res_vals, bnr, bnc, rownnzT, rowadrT, + colindT, nullptr, block_res_rownnz, block_res_rowadr, + block_res_colind); // verification: // 1. convert transposed sparse block to dense @@ -1634,31 +1509,21 @@ TEST_F(EngineUtilSparseTest, BlockDiagSparseTranspose) { } TEST_F(EngineUtilSparseTest, PermuteMat) { - const mjtNum mat[] = {1, 2, 0, 0, - 0, 0, 3, 4, - 0, 0, 5, 6}; + const mjtNum mat[] = {1, 2, 0, 0, 0, 0, 3, 4, 0, 0, 5, 6}; const int perm_r[] = {2, 0, 1}; const int perm_c[] = {3, 2, 0, 1}; mjtNum gather[3 * 4]; PermuteMat(gather, mat, 3, 4, perm_r, perm_c, false, false); - EXPECT_THAT(gather, ElementsAre(6, 5, 0, 0, - 0, 0, 1, 2, - 4, 3, 0, 0)); + EXPECT_THAT(gather, ElementsAre(6, 5, 0, 0, 0, 0, 1, 2, 4, 3, 0, 0)); mjtNum scatter[3 * 4]; PermuteMat(scatter, gather, 3, 4, perm_r, perm_c, true, true); - EXPECT_THAT(scatter, ElementsAre(1, 2, 0, 0, - 0, 0, 3, 4, - 0, 0, 5, 6)); + EXPECT_THAT(scatter, ElementsAre(1, 2, 0, 0, 0, 0, 3, 4, 0, 0, 5, 6)); mjtNum mixed[3 * 4]; PermuteMat(mixed, mat, 3, 4, perm_r, perm_c, true, false); - EXPECT_THAT(mixed, ElementsAre(4, 3, 0, 0, - 6, 5, 0, 0, - 0, 0, 1, 2)); + EXPECT_THAT(mixed, ElementsAre(4, 3, 0, 0, 6, 5, 0, 0, 0, 0, 1, 2)); mjtNum mixed_back[3 * 4]; PermuteMat(mixed_back, mixed, 3, 4, perm_r, perm_c, false, true); - EXPECT_THAT(mixed_back, ElementsAre(1, 2, 0, 0, - 0, 0, 3, 4, - 0, 0, 5, 6)); + EXPECT_THAT(mixed_back, ElementsAre(1, 2, 0, 0, 0, 0, 3, 4, 0, 0, 5, 6)); } } // namespace diff --git a/test/engine/engine_vis_visualize_test.cc b/test/engine/engine_vis_visualize_test.cc index 31462697..328779d1 100644 --- a/test/engine/engine_vis_visualize_test.cc +++ b/test/engine/engine_vis_visualize_test.cc @@ -44,9 +44,7 @@ class MjvSceneTest : public MujocoTest { } } - void FreeSceneObjects() { - mjv_freeScene(&scn_); - } + void FreeSceneObjects() { mjv_freeScene(&scn_); } mjvScene scn_; mjvOption opt_; @@ -80,7 +78,6 @@ TEST_F(MjvSceneTest, UpdateScene) { mj_deleteData(data_copy); mj_deleteData(data); - FreeSceneObjects(); mj_deleteModel(model); } @@ -101,7 +98,6 @@ TEST_F(MjvSceneTest, UpdateSceneGeomsExhausted) { mjv_updateScene(model, data, &opt_, &pert_, &cam_, mjCAT_ALL, &scn_); EXPECT_EQ(scn_.status, 1); EXPECT_EQ(scn_.ngeom, maxgeoms); - mj_deleteData(data); FreeSceneObjects(); mj_deleteModel(model); @@ -118,17 +114,17 @@ TEST_F(MjvSceneTest, PrincipalPointFrustumSign) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - mjData* data = mj_makeData(model); - mj_forward(model, data); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + MjDataPtr data = MakeData(model); + mj_forward(model.get(), data.get()); - InitSceneObjects(model); + InitSceneObjects(model.get()); // point camera at the fixed cam cam_.type = mjCAMERA_FIXED; cam_.fixedcamid = 0; - mjv_updateCamera(model, data, &cam_, &scn_); + mjv_updateCamera(model.get(), data.get(), &cam_, &scn_); float top = scn_.camera[0].frustum_top; float bottom = scn_.camera[0].frustum_bottom; @@ -148,9 +144,7 @@ TEST_F(MjvSceneTest, PrincipalPointFrustumSign) { EXPECT_FLOAT_EQ(top, half - offset); EXPECT_FLOAT_EQ(bottom, -(half + offset)); - mj_deleteData(data); FreeSceneObjects(); - mj_deleteModel(model); } } // namespace diff --git a/test/fixture.cc b/test/fixture.cc index f342365e..6839f81e 100644 --- a/test/fixture.cc +++ b/test/fixture.cc @@ -157,7 +157,7 @@ const std::string GetModelPath(std::string_view path) { // NOLINT 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) { if (error) { error[0] = '\0'; @@ -197,7 +197,11 @@ mjModel* LoadModelFromString(std::string_view xml, char* error, } SetGlobalXmlSpec(spec); - return model; + return MjModelPtr(model); +} + +MjDataPtr MakeData(const MjModelPtr& model) { + return MjDataPtr(mj_makeData(model.get())); } static void AssertModelNotNull(mjModel* model, diff --git a/test/fixture.h b/test/fixture.h index 92b1e062..e2bcec9c 100644 --- a/test/fixture.h +++ b/test/fixture.h @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -39,6 +40,20 @@ MJAPI mjfLogHandler _mjPRIVATE_setTlsLogHandler(mjfLogHandler handler); namespace mujoco { +struct MjModelDeleter { + void operator()(mjModel* m) const { + mj_deleteModel(m); + } +}; +using MjModelPtr = std::unique_ptr; + +struct MjDataDeleter { + void operator()(mjData* d) const { + mj_deleteData(d); + } +}; +using MjDataPtr = std::unique_ptr; + // 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. 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. // 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); +// 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. // On failure it asserts that model is null. mjModel* LoadModelFromPath(const char* model_path); diff --git a/test/plugin/actuator/pid_test.cc b/test/plugin/actuator/pid_test.cc index b949a5da..8e7a365c 100644 --- a/test/plugin/actuator/pid_test.cc +++ b/test/plugin/actuator/pid_test.cc @@ -65,23 +65,21 @@ TEST_F(PidTest, PGain) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; EXPECT_EQ(m->plugin_statenum[0], 0) << "Plugin should have no state variables"; EXPECT_EQ(m->actuator_actnum[0], 0) << "Plugin should have no activation variables"; - mjData* d = mj_makeData(m); - absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + MjDataPtr d = MakeData(m); // apply the same ctrl to both actuators and see if the same qpos results d->ctrl[0] = 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->qfrc_actuator[0], d->qfrc_actuator[1]); @@ -120,24 +118,22 @@ TEST_F(PidTest, PGainWithFilterExact) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; EXPECT_EQ(m->plugin_statenum[0], 0) << "Plugin should have no state variables"; EXPECT_EQ(m->actuator_actnum[0], 1) << "Plugin should have one activation variable"; - mjData* d = mj_makeData(m); - absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + MjDataPtr d = MakeData(m); // apply the same ctrl to both actuators and see if the same qpos results d->ctrl[0] = 1.0; d->ctrl[1] = 1.0; 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->qfrc_actuator[0], d->qfrc_actuator[1]); @@ -178,27 +174,25 @@ TEST_F(PidTest, SlewMaxRate) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; // having a slew rate means that there should be one extra state variable // for the plugin. EXPECT_EQ(m->actuator_actnum[0], 1); - mjData* d = mj_makeData(m); - absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + MjDataPtr d = MakeData(m); // going from ctrl=0.0 to ctrl=1.0 immediately should be equivalent to slowly // incrementing the setpoint d->ctrl[0] = d->ctrl[1] = 0.0; - mj_step(m, d); + mj_step(m.get(), d.get()); mjtNum max_slew_rate = 0.75; for (int i = 0; i < 2; i++) { d->ctrl[0] = 1.0; 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]) << "actuator_force mismatch at step " << i; @@ -246,9 +240,8 @@ TEST_F(PidTest, IntegratedVelocitySlewMaxRate) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; // having a slew rate means that there should be one extra state variable // for the plugin. @@ -256,22 +249,20 @@ TEST_F(PidTest, IntegratedVelocitySlewMaxRate) { // The integrated-velocity controller should have one activation variable too. ASSERT_EQ(m->actuator_actnum[1], 1); - mjData* d = mj_makeData(m); - absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + MjDataPtr d = MakeData(m); // going from ctrl=0.0 to ctrl=1.0 immediately should be equivalent to using // an integrated-velocity controller, with actearly = true d->ctrl[0] = d->ctrl[1] = 0.0; - mj_step(m, d); + mj_step(m.get(), d.get()); mjtNum max_slew_rate = 0.75; for (int i = 0; i < 2; i++) { d->ctrl[0] = 1.0; d->ctrl[1] = max_slew_rate; - mj_step(m, d); + mj_step(m.get(), d.get()); - EXPECT_EQ(d->act[0], d->act[1]) - << "act mismatch at step " << i; + EXPECT_EQ(d->act[0], d->act[1]) << "act mismatch at step " << i; EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]) << "actuator_force mismatch at step " << i; EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]) @@ -308,16 +299,14 @@ TEST_F(PidTest, SlewMaxRateUsesFirstCtrl) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; - mjData* d = mj_makeData(m); - absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + MjDataPtr d = MakeData(m); // When starting with ctrl = 1.0, there shouldn't be a slew rate restriction d->ctrl[0] = 1.0; - mj_forward(m, d); + mj_forward(m.get(), d.get()); EXPECT_EQ(d->actuator_force[0], 4.0); } @@ -374,17 +363,15 @@ TEST_F(PidTest, ITerm) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; // only the actuators with an I term should have state. EXPECT_EQ(m->actuator_actnum[0], 0); EXPECT_EQ(m->actuator_actnum[1], 1); EXPECT_EQ(m->actuator_actnum[2], 1); - mjData* d = mj_makeData(m); - absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + MjDataPtr d = MakeData(m); // when applying a constant 1.0 control for a while: // - 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 d->ctrl[0] = d->ctrl[1] = d->ctrl[2] = 1.0; 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)); @@ -423,28 +410,26 @@ TEST_F(PidTest, FiniteDifferencing) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; // actuators with an I term and max slew rate should have 2 activation // variables. ASSERT_EQ(m->actuator_actnum[0], 2); - mjData* d = mj_makeData(m); - absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + MjDataPtr d = MakeData(m); d->ctrl[0] = 1.0; - mj_step(m, d); + mj_step(m.get(), d.get()); EXPECT_NE(d->act[0], 0); mjtNum state_before_finite_differencing = d->act[0]; - std::vector A((2*m->nv+m->na) * (2*m->nv+m->na), 0); - std::vector B((2*m->nv+m->na) * m->nu, 0); - std::vector C((m->nsensordata) * (2*m->nv+m->na), 0); + std::vector A((2 * m->nv + m->na) * (2 * m->nv + m->na), 0); + std::vector B((2 * m->nv + m->na) * m->nu, 0); + std::vector C((m->nsensordata) * (2 * m->nv + m->na), 0); std::vector D((m->nsensordata) * m->nu, 0); - mjd_transitionFD(m, d, /*eps=*/1e-3, /*flg_centered=*/true, A.data(), - B.data(), C.data(), D.data()); + mjd_transitionFD(m.get(), d.get(), /*eps=*/1e-3, /*flg_centered=*/true, + A.data(), B.data(), C.data(), D.data()); EXPECT_EQ(d->act[0], state_before_finite_differencing) << "mjd_transitionFD should not change actuator plugin state."; @@ -481,25 +466,23 @@ TEST_F(PidTest, CtrlClamp) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; - mjData* d = mj_makeData(m); - absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + MjDataPtr d = MakeData(m); // when applying a constant 1.0 control for a while, control should be clamped // to 0.75, and the body should reach that point. d->ctrl[0] = 1.0; 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)); // when applying 0, it should be clamped to 0.25 d->ctrl[0] = 0.0; 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)); } @@ -544,23 +527,21 @@ TEST_F(PidTest, CopyData) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; - mjData* d1 = mj_makeData(m); - absl::Cleanup d1_deleter = [d1] { mj_deleteData(d1); }; + MjDataPtr d1 = MakeData(m); d1->ctrl[0] = d1->ctrl[1] = -1.0; 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); }; - mj_step(m, d1); - mj_step(m, d2); + mj_step(m.get(), d1.get()); + mj_step(m.get(), d2); EXPECT_EQ(d1->qpos[0], d2->qpos[0]); EXPECT_EQ(d1->qpos[1], d2->qpos[1]); @@ -599,17 +580,15 @@ TEST_F(PidTest, MultipleActuatorsSamePlugin) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; // having a slew rate means that there should be one extra state variable // for the plugin. EXPECT_EQ(m->actuator_actnum[0], 1); EXPECT_EQ(m->actuator_actnum[1], 1); - mjData* d = mj_makeData(m); - absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + MjDataPtr d = MakeData(m); // Set different ctrls for the two actuators, and check that they're // independent. @@ -617,7 +596,7 @@ TEST_F(PidTest, MultipleActuatorsSamePlugin) { d->ctrl[1] = -1.0; 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]) << "actuator_force mismatch at step " << i; @@ -649,8 +628,8 @@ TEST_F(PidTest, InvalidClamp) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error)); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(std::string_view(error), HasSubstr("plugin")); EXPECT_THAT(std::string_view(error), HasSubstr("imax")); @@ -680,8 +659,8 @@ TEST_F(PidTest, InvalidSlew) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); - ASSERT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(std::string_view(error), HasSubstr("plugin")); EXPECT_THAT(std::string_view(error), HasSubstr("slewmax")); @@ -717,24 +696,25 @@ TEST_F(PidTest, WrongActdim) { { std::string no_actdim = absl::StrReplaceAll(kBaseXml, {{"PLACEHOLDER", ""}}); - mjModel* m = LoadModelFromString(no_actdim, error, sizeof(error)); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(no_actdim, error, sizeof(error)); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"1\"")); } { std::string big_actdim = absl::StrReplaceAll(kBaseXml, {{"PLACEHOLDER", "actdim=\"2\""}}); - mjModel* m = LoadModelFromString(big_actdim, error, sizeof(error)); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(big_actdim, error, sizeof(error)); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"1\"")); } { std::string dyntype_integrator = absl::StrReplaceAll( kBaseXml, {{"PLACEHOLDER", "dyntype=\"integrator\" actdim=\"1\""}}); - mjModel* m = LoadModelFromString(dyntype_integrator, error, sizeof(error)); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = + LoadModelFromString(dyntype_integrator, error, sizeof(error)); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"2\"")); } } @@ -769,9 +749,8 @@ TEST_F(PidTest, Keyframe) { )"; char error[1024] = {0}; - mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; // having a slew rate means that there should be one extra state variable // for the plugin. @@ -780,10 +759,9 @@ TEST_F(PidTest, Keyframe) { ASSERT_EQ(m->nkey, 1); EXPECT_EQ(m->key_act[0], 1.0); - mjData* d = mj_makeData(m); - absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + MjDataPtr d = MakeData(m); - mj_resetDataKeyframe(m, d, 0); + mj_resetDataKeyframe(m.get(), d.get(), 0); EXPECT_EQ(d->act[0], 1.0); } } // namespace diff --git a/test/plugin/elasticity/elasticity_test.cc b/test/plugin/elasticity/elasticity_test.cc index ff1052ad..8aca6387 100644 --- a/test/plugin/elasticity/elasticity_test.cc +++ b/test/plugin/elasticity/elasticity_test.cc @@ -28,7 +28,6 @@ namespace { using ElasticityTest = MujocoTest; - // -------------------------------- cable ----------------------------------- TEST_F(ElasticityTest, CantileverIntoCircle) { #ifdef mjUSESINGLE @@ -66,25 +65,24 @@ TEST_F(ElasticityTest, CantileverIntoCircle) { char error[1024] = {0}; - mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; - mjData* d = mj_makeData(m); + MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; + MjDataPtr d = MakeData(m); - // see Oliver Weeger, Sai-Kit Yeung, Martin L. Dunn, "Isogeometric collocation 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. + // see Oliver Weeger, Sai-Kit Yeung, Martin L. Dunn, "Isogeometric collocation + // 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 torque = 2 * mjPI * 1e9 * Iy; - for (int i=0; i < 1300; i++) { + for (int i = 0; i < 1300; i++) { if (i < 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::epsilon()); EXPECT_NEAR(d->sensordata[1], 0, std::numeric_limits::epsilon()); EXPECT_NEAR(d->sensordata[2], 1, std::numeric_limits::epsilon()); - mj_deleteData(d); - mj_deleteModel(m); } TEST_F(ElasticityTest, InvalidTxtAttribute) { @@ -106,8 +104,8 @@ TEST_F(ElasticityTest, InvalidTxtAttribute) { char error[1024] = {0}; - mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); - ASSERT_THAT(m, testing::IsNull()); + MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::IsNull()); } TEST_F(ElasticityTest, InvalidMixedAttribute) { @@ -129,8 +127,8 @@ TEST_F(ElasticityTest, InvalidMixedAttribute) { char error[1024] = {0}; - mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); - ASSERT_THAT(m, testing::IsNull()); + MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::IsNull()); } TEST_F(ElasticityTest, ValidAttributes) { @@ -152,9 +150,8 @@ TEST_F(ElasticityTest, ValidAttributes) { char error[1024] = {0}; - mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); - ASSERT_THAT(m, testing::NotNull()) << error; - mj_deleteModel(m); + MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error)); + ASSERT_THAT(m.get(), testing::NotNull()) << error; } } // namespace diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 4b8b3ac0..6ae7339e 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -24,12 +24,12 @@ #include #include -#include #include #include -#include -#include +#include #include +#include +#include #include "src/xml/xml_api.h" #include "test/compare_model.h" #include "test/fixture.h" @@ -228,7 +228,6 @@ TEST_F(MujocoTest, OriginSpecInvariantToAttachment) { mjs_findElement(parent_spec, mjOBJ_BODY, "parent_body"); EXPECT_EQ(mjs_getOriginSpec(parent_spec_body), parent_spec); - mj_deleteSpec(child_spec); mj_deleteSpec(parent_spec); } @@ -404,10 +403,9 @@ TEST_F(MujocoTest, SetToDCMotorNullable) { double motorconst[2] = {0.05, 0.05}; double resistance = 2.0; - const char* err = mjs_setToDCMotor(actuator, motorconst, resistance, - nullptr, nullptr, nullptr, - nullptr, nullptr, nullptr, - nullptr, 0); + const char* err = + mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, nullptr, + nullptr, nullptr, nullptr, nullptr, nullptr, 0); EXPECT_STREQ(err, ""); EXPECT_EQ(actuator->gainprm[0], 2.0); EXPECT_EQ(actuator->gainprm[1], 0.05); @@ -427,10 +425,9 @@ TEST_F(MujocoTest, SetToDCMotorDeriveKe) { double resistance = 2.0; double nominal[3] = {12.0, 0, 100.0}; // vn=12, omega0=100 - const char* err = mjs_setToDCMotor(actuator, nullptr, resistance, - nominal, nullptr, nullptr, - nullptr, nullptr, nullptr, - nullptr, 0); + const char* err = + mjs_setToDCMotor(actuator, nullptr, resistance, nominal, nullptr, nullptr, + nullptr, nullptr, nullptr, nullptr, 0); EXPECT_STREQ(err, ""); EXPECT_EQ(actuator->gainprm[0], 2.0); 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 controller[6] = {10.0, 20.0, 30.0, 40.0, 50.0, 60.0}; - const char* err = mjs_setToDCMotor(actuator, motorconst, resistance, - nullptr, saturation, nullptr, - nullptr, controller, nullptr, - nullptr, 0); + const char* err = + mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, saturation, + nullptr, nullptr, controller, nullptr, nullptr, 0); EXPECT_STREQ(err, ""); EXPECT_EQ(actuator->gainprm[0], 2.0); // resistance EXPECT_EQ(actuator->gainprm[1], 0.05); // K @@ -473,10 +469,9 @@ TEST_F(MujocoTest, SetToDCMotorLuGre) { double resistance = 2.0; double lugre[5] = {100.0, 1.0, 0.5, 0.7, 10.0}; - const char* err = mjs_setToDCMotor(actuator, motorconst, resistance, - nullptr, nullptr, nullptr, - nullptr, nullptr, nullptr, - lugre, 0); + const char* err = + mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, nullptr, + nullptr, nullptr, nullptr, nullptr, lugre, 0); EXPECT_STREQ(err, ""); EXPECT_EQ(actuator->dynprm[5], 100.0); // stiffness EXPECT_EQ(actuator->dynprm[6], 1.0); // damping @@ -764,7 +759,6 @@ TEST_F(MujocoTest, ModifyShellInertiaFails) { mj_deleteModel(model); } - TEST_F(MujocoTest, RecompileEdit) { static constexpr char xml[] = R"( @@ -778,18 +772,18 @@ TEST_F(MujocoTest, RecompileEdit) { )"; std::array er; - mjSpec *spec = mj_parseXMLString(xml, 0, er.data(), er.size()); + mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size()); EXPECT_THAT(spec, NotNull()) << er.data(); - mjModel *m1 = mj_compile(spec, nullptr); + mjModel* m1 = mj_compile(spec, nullptr); EXPECT_THAT(m1, NotNull()); // add a geom - mjsBody *world = mjs_findBody(spec, "world"); - mjsGeom *geom = mjs_addGeom(world, nullptr); + mjsBody* world = mjs_findBody(spec, "world"); + mjsGeom* geom = mjs_addGeom(world, nullptr); geom->size[0] = 1; // compile again - mjModel *m2 = mj_compile(spec, nullptr); + mjModel* m2 = mj_compile(spec, nullptr); EXPECT_THAT(m2, NotNull()); mj_deleteModel(m1); @@ -837,43 +831,40 @@ TEST_F(MujocoTest, RecompileCompareObjCache) { std::array error; // 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); - mj_deleteModel(m); // update cube.obj, load again mj_deleteFileVFS(vfs.get(), "cube.obj"); mj_addBufferVFS(vfs.get(), "cube.obj", cube2, sizeof(cube2)); m = LoadModelFromString(xml, error.data(), error.size(), vfs.get()); EXPECT_EQ(m->mesh_vert[0], -1); - mj_deleteModel(m); mj_deleteVFS(vfs.get()); } // tiny RGB 2 x 3 PNG file static constexpr uint8_t tex1[] = { - 0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00, 0x0d, - 0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x02, - 0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d, 0x00, 0x00, 0x00, - 0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63, 0x78, 0xc1, 0xc0, 0xc0, - 0xc0, 0xf0, 0xbf, 0xb8, 0xb8, 0x98, 0x81, 0xe1, 0x3f, 0xc3, 0xff, 0xff, - 0xff, 0xc5, 0xc4, 0xc4, 0x00, 0x46, 0xd7, 0x07, 0x7f, 0xd2, 0x52, 0xa1, - 0x41, 0x00, 0x00, 0x00, 0x00, 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60, - 0x82 -}; + 0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00, + 0x0d, 0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x02, 0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d, + 0x00, 0x00, 0x00, 0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63, + 0x78, 0xc1, 0xc0, 0xc0, 0xc0, 0xf0, 0xbf, 0xb8, 0xb8, 0x98, 0x81, + 0xe1, 0x3f, 0xc3, 0xff, 0xff, 0xff, 0xc5, 0xc4, 0xc4, 0x00, 0x46, + 0xd7, 0x07, 0x7f, 0xd2, 0x52, 0xa1, 0x41, 0x00, 0x00, 0x00, 0x00, + 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60, 0x82}; // previous PNG file, but rotated by 180 degrees static constexpr uint8_t tex2[] = { - 0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00, 0x0d, - 0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x02, - 0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d, 0x00, 0x00, 0x00, - 0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63, 0x10, 0x13, 0x13, 0xfb, - 0xff, 0xff, 0x3f, 0xc3, 0x7f, 0x06, 0x96, 0xd8, 0xd8, 0x58, 0x46, 0x46, - 0x86, 0x17, 0x0c, 0x0c, 0x00, 0x49, 0x22, 0x06, 0x44, 0xe4, 0x91, 0xb8, - 0x83, 0x00, 0x00, 0x00, 0x00, 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60, - 0x82 -}; + 0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00, + 0x0d, 0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x02, 0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d, + 0x00, 0x00, 0x00, 0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63, + 0x10, 0x13, 0x13, 0xfb, 0xff, 0xff, 0x3f, 0xc3, 0x7f, 0x06, 0x96, + 0xd8, 0xd8, 0x58, 0x46, 0x46, 0x86, 0x17, 0x0c, 0x0c, 0x00, 0x49, + 0x22, 0x06, 0x44, 0xe4, 0x91, 0xb8, 0x83, 0x00, 0x00, 0x00, 0x00, + 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60, 0x82}; TEST_F(MujocoTest, RecompileComparePngCache) { static constexpr char xml[] = R"( @@ -896,10 +887,10 @@ TEST_F(MujocoTest, RecompileComparePngCache) { std::array error; // 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 mjtByte byte = m->tex_data[0]; - mj_deleteModel(m); // update tex.png, load again mj_deleteFileVFS(vfs.get(), "tex.png"); @@ -907,7 +898,6 @@ TEST_F(MujocoTest, RecompileComparePngCache) { m = LoadModelFromString(xml, error.data(), error.size(), vfs.get()); EXPECT_NE(m->tex_data[0], byte); EXPECT_EQ(m->tex_data[15], byte); // first pixel is now last pixel - mj_deleteModel(m); mj_deleteVFS(vfs.get()); } @@ -937,14 +927,13 @@ TEST_F(MujocoTest, DisableCache) { std::array error; // 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(m->ntexdata, 18); // w x h x rgb = 3 x 2 x 3 mj_setCacheCapacity(cache, capacity); - - mj_deleteModel(m); mj_deleteVFS(vfs.get()); } @@ -1226,16 +1215,16 @@ TEST_F(MujocoTest, AttachSame) { EXPECT_THAT(m_attached->body_parentid[4], 0); // compare with expected XML - mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size()); - EXPECT_THAT(m_expected, NotNull()) << er.data(); - EXPECT_LE(CompareModel(m_attached, m_expected, field), tol) - << "Expected and attached models are different!\n" - << "Different field: " << field << '\n';; + MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size()); + EXPECT_THAT(m_expected.get(), NotNull()) << er.data(); + EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; + ; // destroy everything mj_deleteSpec(parent); mj_deleteModel(m_attached); - mj_deleteModel(m_expected); } TEST_F(MujocoTest, AttachSpatialTendonWithoutSidesite) { @@ -1283,13 +1272,15 @@ TEST_F(MujocoTest, AttachSpatialTendonWithoutSidesite) { mjsSite* attach_site = mjs_addSite(parent_body, 0); mjs_setName(attach_site->element, "attach_site"); - mjs_attach(attach_site->element, - mjs_findBody(child, "child_body")->element, "", "_child"); + mjs_attach(attach_site->element, mjs_findBody(child, "child_body")->element, + "", "_child"); - EXPECT_THAT(mjs_findElement(parent, mjOBJ_TENDON, - "tendon_with_sidesite_child"), NotNull()); - EXPECT_THAT(mjs_findElement(parent, mjOBJ_TENDON, - "tendon_without_sidesite_child"), NotNull()); + EXPECT_THAT( + mjs_findElement(parent, mjOBJ_TENDON, "tendon_with_sidesite_child"), + NotNull()); + EXPECT_THAT( + mjs_findElement(parent, mjOBJ_TENDON, "tendon_without_sidesite_child"), + NotNull()); mjModel* model = mj_compile(parent, nullptr); ASSERT_THAT(model, NotNull()) << mjs_getError(parent); @@ -1335,11 +1326,9 @@ TEST_F(MujocoTest, AttachSpatialTendonGitHubIssue3119) { )"; std::array er; - mjSpec* parent_spec = - mj_parseXMLString(parent_xml, 0, er.data(), er.size()); + mjSpec* parent_spec = mj_parseXMLString(parent_xml, 0, er.data(), er.size()); ASSERT_THAT(parent_spec, NotNull()) << er.data(); - mjSpec* child_spec = - mj_parseXMLString(child_xml, 0, er.data(), er.size()); + mjSpec* child_spec = mj_parseXMLString(child_xml, 0, er.data(), er.size()); ASSERT_THAT(child_spec, NotNull()) << er.data(); 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_attach(attach_site->element, - mjs_findBody(child_spec, "child_body")->element, - "", "_child"); + mjs_findBody(child_spec, "child_body")->element, "", "_child"); + EXPECT_THAT( + mjs_findElement(parent_spec, mjOBJ_TENDON, "tendon_with_sidesite_child"), + NotNull()); EXPECT_THAT(mjs_findElement(parent_spec, mjOBJ_TENDON, - "tendon_with_sidesite_child"), NotNull()); - EXPECT_THAT(mjs_findElement(parent_spec, mjOBJ_TENDON, - "tendon_without_sidesite_child"), NotNull()); + "tendon_without_sidesite_child"), + NotNull()); mjModel* model = mj_compile(parent_spec, nullptr); 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()); // compare with expected XML - mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size()); - EXPECT_THAT(m_expected, NotNull()) << er.data(); - EXPECT_LE(CompareModel(m_attached, m_expected, field), tol) - << "Expected and attached models are different!\n" - << "Different field: " << field << '\n';; + MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size()); + EXPECT_THAT(m_expected.get(), NotNull()) << er.data(); + EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; + ; // destroy everything mj_deleteSpec(parent); mj_deleteSpec(child); mj_deleteModel(m_attached); - mj_deleteModel(m_expected); } TEST_F(MujocoTest, AttachFrame) { @@ -1633,17 +1623,17 @@ TEST_F(MujocoTest, AttachFrame) { EXPECT_THAT(m_attached->body_parentid[2], 1); // compare with expected XML - mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size()); - EXPECT_THAT(m_expected, NotNull()) << er.data(); - EXPECT_LE(CompareModel(m_attached, m_expected, field), tol) - << "Expected and attached models are different!\n" - << "Different field: " << field << '\n';; + MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size()); + EXPECT_THAT(m_expected.get(), NotNull()) << er.data(); + EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; + ; // destroy everything mj_deleteSpec(parent); mj_deleteSpec(child); mj_deleteModel(m_attached); - mj_deleteModel(m_expected); } TEST_F(MujocoTest, AttachCompiled) { @@ -1691,8 +1681,8 @@ TEST_F(MujocoTest, AttachCompiled) { // check that attached model can be compiled mjModel* m_attached = mj_compile(parent, 0); - EXPECT_THAT(m_attached, NotNull()) << - "Failed to compile attached model" << mjs_getError(parent); + EXPECT_THAT(m_attached, NotNull()) + << "Failed to compile attached model" << mjs_getError(parent); // destroy everything mj_deleteSpec(parent); @@ -1758,24 +1748,23 @@ void TestDetachBody(bool compile) { std::array s; EXPECT_EQ(mj_saveXMLString(child, s.data(), 1024, e.data(), 1024), -1); EXPECT_THAT(e.data(), compile - ? HasSubstr("Model has pending keyframes") - : HasSubstr("Only compiled model can be written")); + ? HasSubstr("Model has pending keyframes") + : HasSubstr("Only compiled model can be written")); // compile new model mjModel* m_detached = mj_compile(child, 0); EXPECT_THAT(m_detached, NotNull()); // compare with expected XML - mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size()); - EXPECT_THAT(m_expected, NotNull()) << er.data(); - EXPECT_LE(CompareModel(m_detached, m_expected, field), tol) - << "Expected and attached models are different!\n" - << "Different field: " << field << '\n'; + MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size()); + EXPECT_THAT(m_expected.get(), NotNull()) << er.data(); + EXPECT_LE(CompareModel(m_detached, m_expected.get(), field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; // destroy everything mj_deleteSpec(child); mj_deleteModel(m_detached); - mj_deleteModel(m_expected); if (m_child) mj_deleteModel(m_child); } @@ -1836,16 +1825,15 @@ TEST_F(MujocoTest, AttachToSite) { mjModel* model = mj_compile(parent, 0); EXPECT_THAT(model, NotNull()); - mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size()); - EXPECT_THAT(expected, NotNull()) << er.data(); - EXPECT_LE(CompareModel(model, expected, field), tol) - << "Expected and attached models are different!\n" - << "Different field: " << field << '\n'; + MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size()); + EXPECT_THAT(expected.get(), NotNull()) << er.data(); + EXPECT_LE(CompareModel(model, expected.get(), field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; mj_deleteSpec(parent); mj_deleteSpec(child); mj_deleteModel(model); - mj_deleteModel(expected); } TEST_F(MujocoTest, AttachFrameToSite) { @@ -1902,16 +1890,15 @@ TEST_F(MujocoTest, AttachFrameToSite) { mjModel* model = mj_compile(parent, 0); EXPECT_THAT(model, NotNull()); - mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size()); - EXPECT_THAT(expected, NotNull()) << er.data(); - EXPECT_LE(CompareModel(model, expected, field), tol) - << "Expected and attached models are different!\n" - << "Different field: " << field << '\n'; + MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size()); + EXPECT_THAT(expected.get(), NotNull()) << er.data(); + EXPECT_LE(CompareModel(model, expected.get(), field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; mj_deleteSpec(parent); mj_deleteSpec(child); mj_deleteModel(model); - mj_deleteModel(expected); } TEST_F(MujocoTest, BodyToFrame) { @@ -1991,18 +1978,17 @@ TEST_F(MujocoTest, BodyToFrame) { // compile and compare mjModel* model2 = mj_compile(parent, 0); EXPECT_THAT(model2, NotNull()); - mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size()); - EXPECT_THAT(expected, NotNull()) << er.data(); - EXPECT_LE(CompareModel(model2, expected, field), tol) - << "Expected and attached models are different!\n" - << "Different field: " << field << '\n'; + MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size()); + EXPECT_THAT(expected.get(), NotNull()) << er.data(); + EXPECT_LE(CompareModel(model2, expected.get(), field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; mj_deleteSpec(parent); mj_deleteSpec(child1); mj_deleteSpec(child2); mj_deleteModel(model1); mj_deleteModel(model2); - mj_deleteModel(expected); } TEST_F(MujocoTest, BodyToFrameWithInertial) { @@ -2100,11 +2086,11 @@ TEST_F(MujocoTest, AttachSpecToSite) { // compile and compare mjModel* model = mj_compile(parent, 0); EXPECT_THAT(model, NotNull()); - mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size()); - EXPECT_THAT(expected, NotNull()) << er.data(); - EXPECT_LE(CompareModel(model, expected, field), tol) - << "Expected and attached models are different!\n" - << "Different field: " << field << '\n'; + MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size()); + EXPECT_THAT(expected.get(), NotNull()) << er.data(); + EXPECT_LE(CompareModel(model, expected.get(), field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; // check that the child world still exists mjsBody* child_world = mjs_findBody(child, "world"); @@ -2113,7 +2099,6 @@ TEST_F(MujocoTest, AttachSpecToSite) { mj_deleteSpec(parent); mj_deleteSpec(child); mj_deleteModel(model); - mj_deleteModel(expected); } TEST_F(MujocoTest, AttachSpecToBody) { @@ -2181,11 +2166,11 @@ TEST_F(MujocoTest, AttachSpecToBody) { // compile and compare mjModel* model = mj_compile(parent, 0); EXPECT_THAT(model, NotNull()); - mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size()); - EXPECT_THAT(expected, NotNull()) << er.data(); - EXPECT_LE(CompareModel(model, expected, field), tol) - << "Expected and attached models are different!\n" - << "Different field: " << field << '\n'; + MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size()); + EXPECT_THAT(expected.get(), NotNull()) << er.data(); + EXPECT_LE(CompareModel(model, expected.get(), field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; // check that the child world still exists mjsBody* child_world = mjs_findBody(child, "world"); @@ -2194,7 +2179,6 @@ TEST_F(MujocoTest, AttachSpecToBody) { mj_deleteSpec(parent); mj_deleteSpec(child); mj_deleteModel(model); - mj_deleteModel(expected); } TEST_F(MujocoTest, PreserveState) { @@ -2246,31 +2230,32 @@ TEST_F(MujocoTest, PreserveState) { // compile models mjModel* model = mj_compile(spec, 0); EXPECT_THAT(model, NotNull()); - mjModel* m_expected = LoadModelFromString(xml_expected, er.data(), er.size()); - EXPECT_THAT(m_expected, NotNull()); + MjModelPtr m_expected = + LoadModelFromString(xml_expected, er.data(), er.size()); + EXPECT_THAT(m_expected.get(), NotNull()); // create data mjData* data = mj_makeData(model); EXPECT_THAT(data, NotNull()); - mjData* d_expected = mj_makeData(m_expected); - EXPECT_THAT(d_expected, NotNull()); + MjDataPtr d_expected = MakeData(m_expected); + EXPECT_THAT(d_expected.get(), NotNull()); // set ctrl data->ctrl[0] = 1; data->ctrl[1] = 2; - d_expected->ctrl[0] = 2; + d_expected.get()->ctrl[0] = 2; // set mocap data->mocap_pos[3] = 1; data->mocap_quat[4] = 0; data->mocap_quat[5] = 1; - d_expected->mocap_pos[0] = 1; - d_expected->mocap_quat[0] = 0; - d_expected->mocap_quat[1] = 1; + d_expected.get()->mocap_pos[0] = 1; + d_expected.get()->mocap_quat[0] = 0; + d_expected.get()->mocap_quat[1] = 1; // step models 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); // detach subtree @@ -2298,7 +2283,7 @@ TEST_F(MujocoTest, PreserveState) { joint->axis[0] = 0; joint->axis[1] = 0; 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 mj_recompile(spec, 0, model, data); @@ -2308,32 +2293,36 @@ TEST_F(MujocoTest, PreserveState) { // compare qpos EXPECT_EQ(model->nq, m_expected->nq); 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 EXPECT_EQ(model->nv, m_expected->nv); - for (int i = 0; i < model->nv-1; ++i) { - EXPECT_EQ(data->qvel[i], d_expected->qvel[i]) << i; + for (int i = 0; i < model->nv - 1; ++i) { + EXPECT_EQ(data->qvel[i], d_expected.get()->qvel[i]) << i; } // second body was added after stepping so qvel should be zero - EXPECT_EQ(data->qvel[model->nv-1], 0); + EXPECT_EQ(data->qvel[model->nv - 1], 0); // compare act EXPECT_EQ(model->na, m_expected->na); 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 EXPECT_EQ(model->nmocap, m_expected->nmocap); for (int i = 0; i < model->nmocap; ++i) { 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) { - 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); // destroy everything - mj_deleteData(d_expected); mj_deleteSpec(spec); mj_deleteModel(model); - mj_deleteModel(m_expected); } TEST_F(MujocoTest, RecompileAttach) { @@ -2445,15 +2432,15 @@ TEST_F(MujocoTest, AttachMocap) { mjModel* model = mj_compile(spec, 0); EXPECT_THAT(model, NotNull()); - mjModel* m_expected = LoadModelFromString(xml_expected, er.data(), er.size()); - EXPECT_THAT(m_expected, NotNull()) << er.data(); - EXPECT_LE(CompareModel(model, m_expected, field), tol) - << "Expected and attached models are different!\n" - << "Different field: " << field << '\n'; + MjModelPtr m_expected = + LoadModelFromString(xml_expected, er.data(), er.size()); + EXPECT_THAT(m_expected.get(), NotNull()) << er.data(); + EXPECT_LE(CompareModel(model, m_expected.get(), field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; mj_deleteSpec(spec); mj_deleteModel(model); - mj_deleteModel(m_expected); } TEST_F(MujocoTest, ReplicateKeyframe) { @@ -2475,8 +2462,8 @@ TEST_F(MujocoTest, ReplicateKeyframe) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, testing::NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), testing::NotNull()) << error.data(); EXPECT_THAT(m->ngeom, 1); EXPECT_THAT(m->nbody, 2); @@ -2485,10 +2472,8 @@ TEST_F(MujocoTest, ReplicateKeyframe) { EXPECT_THAT(m->nq, 1); EXPECT_THAT(m->key_qpos[0], 1); EXPECT_THAT(m->key_qpos[1], 0); - EXPECT_STREQ(mj_id2name(m, mjOBJ_KEY, 0), "keyframe0"); - EXPECT_STREQ(mj_id2name(m, mjOBJ_KEY, 1), "keyframe"); - - mj_deleteModel(m); + EXPECT_STREQ(mj_id2name(m.get(), mjOBJ_KEY, 0), "keyframe0"); + EXPECT_STREQ(mj_id2name(m.get(), mjOBJ_KEY, 1), "keyframe"); } TEST_F(MujocoTest, AttachUnnamedAssets) { @@ -2690,17 +2675,17 @@ void AttachNestedKeyframe(bool compile) { std::string field = ""; mjModel* m_attached = mj_compile(parent, 0); EXPECT_THAT(m_attached, NotNull()); - mjModel* m_expected = LoadModelFromString( + MjModelPtr m_expected = LoadModelFromString( compile ? expected_xml : expected_xml_uncompiled, er.data(), er.size()); - EXPECT_THAT(m_expected, NotNull()) << er.data(); - EXPECT_LE(CompareModel(m_attached, m_expected, field), tol) - << "Expected and attached models are different!\n" - << "Different field: " << field << '\n';; + EXPECT_THAT(m_expected.get(), NotNull()) << er.data(); + EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; + ; mj_deleteSpec(parent); mj_deleteSpec(child); mj_deleteSpec(gchild); - mj_deleteModel(m_expected); mj_deleteModel(m_attached); mj_deleteModel(m_child); } @@ -2736,30 +2721,30 @@ TEST_F(MujocoTest, RepeatedAttachKeyframe) { )"; - std::array er; - mjSpec* parent = mj_parseXMLString(xml_1, 0, er.data(), er.size()); - EXPECT_THAT(parent, NotNull()) << er.data(); - mjSpec* child = mj_parseXMLString(xml_2, 0, er.data(), er.size()); - EXPECT_THAT(child, NotNull()) << er.data(); + std::array er; + mjSpec* parent = mj_parseXMLString(xml_1, 0, er.data(), er.size()); + EXPECT_THAT(parent, NotNull()) << er.data(); + mjSpec* child = mj_parseXMLString(xml_2, 0, er.data(), er.size()); + EXPECT_THAT(child, NotNull()) << er.data(); - mjsBody* body_1 = mjs_findBody(parent, "body"); - mjsElement* attachment_frame = mjs_addFrame(body_1, 0)->element; - mjs_attach(attachment_frame, mjs_findBody(child, "b1")->element, "b1-", ""); - mjModel* model_1 = mj_compile(parent, 0); - EXPECT_THAT(model_1, NotNull()); - mjs_attach(attachment_frame, mjs_findBody(child, "b2")->element, "b2-", ""); - mjModel* model_2 = mj_compile(parent, 0); - EXPECT_THAT(model_2, NotNull()); + mjsBody* body_1 = mjs_findBody(parent, "body"); + mjsElement* attachment_frame = mjs_addFrame(body_1, 0)->element; + mjs_attach(attachment_frame, mjs_findBody(child, "b1")->element, "b1-", ""); + mjModel* model_1 = mj_compile(parent, 0); + EXPECT_THAT(model_1, NotNull()); + mjs_attach(attachment_frame, mjs_findBody(child, "b2")->element, "b2-", ""); + mjModel* model_2 = mj_compile(parent, 0); + EXPECT_THAT(model_2, NotNull()); - EXPECT_EQ(model_1->nkey, 1); - EXPECT_EQ(model_2->nkey, 2); - EXPECT_STREQ(mj_id2name(model_2, mjOBJ_KEY, 0), "b1-home"); - EXPECT_STREQ(mj_id2name(model_2, mjOBJ_KEY, 1), "b2-home"); + EXPECT_EQ(model_1->nkey, 1); + EXPECT_EQ(model_2->nkey, 2); + EXPECT_STREQ(mj_id2name(model_2, mjOBJ_KEY, 0), "b1-home"); + EXPECT_STREQ(mj_id2name(model_2, mjOBJ_KEY, 1), "b2-home"); - mj_deleteSpec(parent); - mj_deleteSpec(child); - mj_deleteModel(model_1); - mj_deleteModel(model_2); + mj_deleteSpec(parent); + mj_deleteSpec(child); + mj_deleteModel(model_1); + mj_deleteModel(model_2); } TEST_F(MujocoTest, ResizeParentKeyframe) { @@ -2820,16 +2805,15 @@ TEST_F(MujocoTest, ResizeParentKeyframe) { mjtNum tol = 0; std::string field = ""; - mjModel* expected = LoadModelFromString(xml_expected, er.data(), er.size()); - EXPECT_THAT(expected, NotNull()) << er.data(); - EXPECT_LE(CompareModel(model, expected, field), tol) - << "Expected and attached models are different!\n" - << "Different field: " << field << '\n'; + MjModelPtr expected = LoadModelFromString(xml_expected, er.data(), er.size()); + EXPECT_THAT(expected.get(), NotNull()) << er.data(); + EXPECT_LE(CompareModel(model, expected.get(), field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; mj_deleteSpec(parent); mj_deleteSpec(child); mj_deleteModel(model); - mj_deleteModel(expected); } TEST_F(MujocoTest, KeyframeSizeError) { @@ -2854,8 +2838,10 @@ TEST_F(MujocoTest, KeyframeSizeError) { std::array er; mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size()); EXPECT_THAT(spec, IsNull()); - EXPECT_THAT(er.data(), HasSubstr( - "Keyframe 'invalid_qpos' has invalid qpos size, got 2, should be 1")); + EXPECT_THAT( + er.data(), + HasSubstr( + "Keyframe 'invalid_qpos' has invalid qpos size, got 2, should be 1")); } TEST_F(MujocoTest, DifferentUnitsAllowed) { @@ -2899,8 +2885,7 @@ TEST_F(MujocoTest, DifferentUnitsAllowed) { )"; std::array error; - mjSpec* gchild = - mj_parseXMLString(gchild_xml, 0, error.data(), error.size()); + mjSpec* gchild = mj_parseXMLString(gchild_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()); ASSERT_THAT(spec, NotNull()) << error.data(); diff --git a/test/user/user_composite_test.cc b/test/user/user_composite_test.cc index 07f66c0f..45a9867d 100644 --- a/test/user/user_composite_test.cc +++ b/test/user/user_composite_test.cc @@ -48,9 +48,8 @@ TEST_F(UserCompositeTest, ShapeCanBeOmitted) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, NotNull()) << error.data(); - mj_deleteModel(m); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), NotNull()) << error.data(); } TEST_F(UserCompositeTest, InvalidShape) { @@ -64,8 +63,8 @@ TEST_F(UserCompositeTest, InvalidShape) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("The curve array contains an invalid shape")); } diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index f5a31d0e..b1ae4459 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -24,19 +24,15 @@ #include #include "test/fixture.h" - - namespace mujoco { namespace { +using ::testing::HasSubstr; using ::testing::IsNull; using ::testing::NotNull; -using ::testing::HasSubstr; using ::testing::Pointwise; using UserFlexTest = MujocoTest; - - TEST_F(UserFlexTest, ParentMustHaveName) { static constexpr char xml[] = R"( @@ -46,8 +42,8 @@ TEST_F(UserFlexTest, ParentMustHaveName) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("required attribute missing: 'name'")); } @@ -60,8 +56,8 @@ TEST_F(UserFlexTest, InvalidDim) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("Invalid dim, must be between 1 and 3")); } @@ -74,8 +70,8 @@ TEST_F(UserFlexTest, CountTooSmall) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("Count too small")); } @@ -95,8 +91,8 @@ TEST_F(UserFlexTest, CellnumZeroInterpolated) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("cellcount cannot be 0")); } @@ -109,8 +105,8 @@ TEST_F(UserFlexTest, SpacingGreaterThanGeometry) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("Spacing must be larger than geometry size")); } @@ -124,8 +120,8 @@ TEST_F(UserFlexTest, ScaleMinValue) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("Scale must be larger than mjMINVAL")); } @@ -138,8 +134,8 @@ TEST_F(UserFlexTest, MassMinValue) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("Mass and inertiabox must be larger than mjMINVAL")); } @@ -153,8 +149,8 @@ TEST_F(UserFlexTest, PointSizeNotMultipleOf3) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("Point size must be a multiple of 3")); } @@ -167,8 +163,8 @@ TEST_F(UserFlexTest, ElementSize) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("Element size must be a multiple of dim+1")); } @@ -182,8 +178,8 @@ TEST_F(UserFlexTest, PointAndElementNotInDirect) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("Point and element required")); } @@ -196,8 +192,8 @@ TEST_F(UserFlexTest, UnknownFlexCompType) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("invalid keyword: 'unknown'")); } @@ -212,8 +208,8 @@ TEST_F(UserFlexTest, InvalidPinid) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("element 1 has point id 1, number of points is 1")); } @@ -227,8 +223,8 @@ TEST_F(UserFlexTest, MeshFileMissing) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("File is required")); } @@ -249,8 +245,8 @@ TEST_F(UserFlexTest, CreateBVHSuccess) { ASSERT_THAT(m, NotNull()) << error.data(); mjData* d = mj_makeData(m); mj_step(m, d); - mj_deleteModel(m); mj_deleteData(d); + mj_deleteModel(m); } TEST_F(UserFlexTest, RigidFlex) { @@ -261,8 +257,8 @@ TEST_F(UserFlexTest, RigidFlex) { ASSERT_THAT(m, NotNull()) << error.data(); mjData* d = mj_makeData(m); mj_step(m, d); - mj_deleteModel(m); mj_deleteData(d); + mj_deleteModel(m); } TEST_F(UserFlexTest, FlexNotCollide) { @@ -277,17 +273,16 @@ TEST_F(UserFlexTest, FlexNotCollide) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); - mjData* d = mj_makeData(m); - mj_step(m, d); - mj_deleteModel(m); - mj_deleteData(d); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); + MjDataPtr d = MakeData(m); + mj_step(m.get(), d.get()); } TEST_F(UserFlexTest, BoundingBoxCoordinates) { #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 static constexpr char xml[] = R"( @@ -298,34 +293,31 @@ TEST_F(UserFlexTest, BoundingBoxCoordinates) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); - mjData* d = mj_makeData(m); - mj_kinematics(m, d); - mj_flex(m, d); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); + MjDataPtr d = MakeData(m); + mj_kinematics(m.get(), d.get()); + mj_flex(m.get(), d.get()); - EXPECT_EQ(m->nflexvert, 5*5*5); - EXPECT_EQ(m->nflexelem, 4*4*4*6); + EXPECT_EQ(m->nflexvert, 5 * 5 * 5); + EXPECT_EQ(m->nflexelem, 4 * 4 * 4 * 6); EXPECT_EQ(m->flex_dim[0], 3); // Cartesian coordinates EXPECT_EQ(d->flexvert_xpos[0], -1); EXPECT_EQ(d->flexvert_xpos[1], -2); EXPECT_EQ(d->flexvert_xpos[2], -3); - EXPECT_EQ(d->flexvert_xpos[3*m->nflexvert-3], 3); - EXPECT_EQ(d->flexvert_xpos[3*m->nflexvert-2], 2); - EXPECT_EQ(d->flexvert_xpos[3*m->nflexvert-1], 1); + EXPECT_EQ(d->flexvert_xpos[3 * m->nflexvert - 3], 3); + EXPECT_EQ(d->flexvert_xpos[3 * m->nflexvert - 2], 2); + EXPECT_EQ(d->flexvert_xpos[3 * m->nflexvert - 1], 1); // bounding box coordinates EXPECT_EQ(m->flex_vert0[0], 0); EXPECT_EQ(m->flex_vert0[1], 0); EXPECT_EQ(m->flex_vert0[2], 0); - 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-1], 1); - - mj_deleteModel(m); - mj_deleteData(d); + 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 - 1], 1); } TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) { @@ -339,8 +331,8 @@ TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) { )"; - mjModel* m1 = LoadModelFromString(xml_selfcoll, error.data(), error.size()); - EXPECT_THAT(m1, IsNull()) << error.data(); + MjModelPtr m1 = LoadModelFromString(xml_selfcoll, error.data(), error.size()); + EXPECT_THAT(m1.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("trilinear interpolation cannot do self-collision")); static constexpr char xml_internal[] = R"( @@ -352,8 +344,8 @@ TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) { )"; - mjModel* m2 = LoadModelFromString(xml_internal, error.data(), error.size()); - EXPECT_THAT(m2, IsNull()) << error.data(); + MjModelPtr m2 = LoadModelFromString(xml_internal, error.data(), error.size()); + EXPECT_THAT(m2.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("trilinear interpolation cannot do internal")); } @@ -370,10 +362,11 @@ TEST_F(UserFlexTest, TrilinearInterpolation) { )"; std::array error; - mjModel* m1 = LoadModelFromString(xml_trilinear, error.data(), error.size()); - ASSERT_THAT(m1, NotNull()) << error.data(); - mjData* d1 = mj_makeData(m1); - mj_step(m1, d1); + MjModelPtr m1 = + LoadModelFromString(xml_trilinear, error.data(), error.size()); + ASSERT_THAT(m1.get(), NotNull()) << error.data(); + MjDataPtr d1 = MakeData(m1); + mj_step(m1.get(), d1.get()); static constexpr char xml_linear[] = R"( @@ -385,13 +378,13 @@ TEST_F(UserFlexTest, TrilinearInterpolation) { )"; - mjModel* m2 = LoadModelFromString(xml_linear, error.data(), error.size()); - ASSERT_THAT(m2, NotNull()) << error.data(); - mjData* d2 = mj_makeData(m2); - mj_step(m2, d2); + MjModelPtr m2 = LoadModelFromString(xml_linear, error.data(), error.size()); + ASSERT_THAT(m2.get(), NotNull()) << error.data(); + MjDataPtr d2 = MakeData(m2); + mj_step(m2.get(), d2.get()); EXPECT_EQ(m1->nflexvert, m2->nflexvert); - for (int i = 0; i < 3*m1->nflexvert; ++i) { + for (int i = 0; i < 3 * m1->nflexvert; ++i) { EXPECT_NEAR(m1->flex_vert[i], d2->flexvert_xpos[i], 1e-7); EXPECT_NEAR(m1->flex_vert0[i], m2->flex_vert0[i], 1e-7); EXPECT_NEAR(d1->flexvert_xpos[i], d2->flexvert_xpos[i], 1e-7); @@ -409,11 +402,11 @@ TEST_F(UserFlexTest, TrilinearInterpolation) { } EXPECT_EQ(m1->body_mass[i], m2->body_mass[i]); for (int j = 0; j < 2; ++j) { - EXPECT_EQ(m1->body_invweight0[i*2+j], m2->body_invweight0[i*2+j]); + EXPECT_EQ(m1->body_invweight0[i * 2 + j], m2->body_invweight0[i * 2 + j]); } for (int j = 0; j < 10; ++j) { - EXPECT_NEAR(d1->cinert[10*i+j], d2->cinert[i*10+j], 1e-5) << i; - EXPECT_NEAR(d1->crb[10*i+j], d2->crb[i*10+j], 1e-5) << i; + EXPECT_NEAR(d1->cinert[10 * i + j], d2->cinert[i * 10 + j], 1e-5) << i; + EXPECT_NEAR(d1->crb[10 * i + j], d2->crb[i * 10 + j], 1e-5) << i; } } @@ -436,18 +429,13 @@ TEST_F(UserFlexTest, TrilinearInterpolation) { } } - EXPECT_EQ(d1->nefc, 4*(d1->contact[0].dim-1)*2); - EXPECT_EQ(d2->nefc, 4*(d2->contact[0].dim-1)*2); + EXPECT_EQ(d1->nefc, 4 * (d1->contact[0].dim - 1) * 2); + EXPECT_EQ(d2->nefc, 4 * (d2->contact[0].dim - 1) * 2); EXPECT_EQ(d1->nJ, d2->nJ); for (int i = 0; i < d1->nefc; ++i) { EXPECT_EQ(d1->efc_diagA[i], d2->efc_diagA[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) { @@ -466,22 +454,20 @@ TEST_F(UserFlexTest, StiffnessMatrix) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); EXPECT_NE(m->flex_stiffness[m->flex_stiffnessadr[0]], 0); EXPECT_EQ(m->nflexnode, 8); // constants are in the kernel mjtNum ones[24], zeros[24], res[24]; - for (int i = 0; i < 3*m->nflexnode; ++i) { + for (int i = 0; i < 3 * m->nflexnode; ++i) { zeros[i] = 0; ones[i] = 1; } mju_mulMatVec(res, m->flex_stiffness + m->flex_stiffnessadr[0], ones, 3 * m->nflexnode, 3 * m->nflexnode); EXPECT_THAT(res, Pointwise(MjNear(1e-8, 1e-4), zeros)); - - mj_deleteModel(m); } TEST_F(UserFlexTest, StiffnessCacheDiffersByGeometry) { @@ -510,19 +496,18 @@ TEST_F(UserFlexTest, StiffnessCacheDiffersByGeometry) { )"; - mjModel* m_small = LoadModelFromString(xml_small, error.data(), error.size()); - ASSERT_THAT(m_small, NotNull()) << error.data(); + MjModelPtr m_small = + 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()); - ASSERT_THAT(m_large, NotNull()) << error.data(); + MjModelPtr m_large = + 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 EXPECT_EQ(m_small->nflexnode, m_large->nflexnode); EXPECT_NE(m_small->flex_stiffness[m_small->flex_stiffnessadr[0]], m_large->flex_stiffness[m_large->flex_stiffnessadr[0]]); - - mj_deleteModel(m_small); - mj_deleteModel(m_large); } TEST_F(UserFlexTest, LoadTexture) { @@ -552,8 +537,8 @@ TEST_F(UserFlexTest, LoadMSHBinary_41_Success) { EXPECT_EQ(m->nflexelem, 24); EXPECT_EQ(m->flex_dim[0], 3); mj_step(m, d); - mj_deleteModel(m); mj_deleteData(d); + mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHBinary_22_Success) { @@ -567,8 +552,8 @@ TEST_F(UserFlexTest, LoadMSHBinary_22_Success) { EXPECT_EQ(m->nflexelem, 24); EXPECT_EQ(m->flex_dim[0], 3); mj_step(m, d); - mj_deleteModel(m); mj_deleteData(d); + mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHSurfaceBinary_41_Success) { @@ -585,18 +570,18 @@ TEST_F(UserFlexTest, LoadMSHSurfaceBinary_41_Success) { EXPECT_EQ(m->flex_dim[0], 2); // first node x y z - EXPECT_EQ(d->flexvert_xpos[0], -0.5 ); - EXPECT_EQ(d->flexvert_xpos[1], -0.5 ); - EXPECT_EQ(d->flexvert_xpos[2], 0 ); + EXPECT_EQ(d->flexvert_xpos[0], -0.5); + EXPECT_EQ(d->flexvert_xpos[1], -0.5); + EXPECT_EQ(d->flexvert_xpos[2], 0); // first element - EXPECT_EQ(m->flex_elem[0], 9-1 ); - EXPECT_EQ(m->flex_elem[1], 4-1 ); - EXPECT_EQ(m->flex_elem[2], 3-1 ); + EXPECT_EQ(m->flex_elem[0], 9 - 1); + EXPECT_EQ(m->flex_elem[1], 4 - 1); + EXPECT_EQ(m->flex_elem[2], 3 - 1); mj_step(m, d); - mj_deleteModel(m); mj_deleteData(d); + mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHSurfaceBinary_22_Success) { @@ -613,18 +598,18 @@ TEST_F(UserFlexTest, LoadMSHSurfaceBinary_22_Success) { EXPECT_EQ(m->flex_dim[0], 2); // first node x y z - EXPECT_EQ(d->flexvert_xpos[0], -0.5 ); - EXPECT_EQ(d->flexvert_xpos[1], -0.5 ); - EXPECT_EQ(d->flexvert_xpos[2], 0 ); + EXPECT_EQ(d->flexvert_xpos[0], -0.5); + EXPECT_EQ(d->flexvert_xpos[1], -0.5); + EXPECT_EQ(d->flexvert_xpos[2], 0); // first element - EXPECT_EQ(m->flex_elem[0], 9-1 ); - EXPECT_EQ(m->flex_elem[1], 4-1 ); - EXPECT_EQ(m->flex_elem[2], 3-1 ); + EXPECT_EQ(m->flex_elem[0], 9 - 1); + EXPECT_EQ(m->flex_elem[1], 4 - 1); + EXPECT_EQ(m->flex_elem[2], 3 - 1); mj_step(m, d); - mj_deleteModel(m); mj_deleteData(d); + mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHBinaryFTETWILD_22_Success) { @@ -638,8 +623,8 @@ TEST_F(UserFlexTest, LoadMSHBinaryFTETWILD_22_Success) { EXPECT_EQ(m->nflexelem, 1073); EXPECT_EQ(m->flex_dim[0], 3); mj_step(m, d); - mj_deleteModel(m); mj_deleteData(d); + mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHASCII_41_Success) { @@ -653,8 +638,8 @@ TEST_F(UserFlexTest, LoadMSHASCII_41_Success) { EXPECT_EQ(m->nflexelem, 24); EXPECT_EQ(m->flex_dim[0], 3); mj_step(m, d); - mj_deleteModel(m); mj_deleteData(d); + mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHASCII_22_Success) { @@ -668,8 +653,8 @@ TEST_F(UserFlexTest, LoadMSHASCII_22_Success) { EXPECT_EQ(m->nflexelem, 24); EXPECT_EQ(m->flex_dim[0], 3); mj_step(m, d); - mj_deleteModel(m); mj_deleteData(d); + mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHSurfaceASCII_41_Success) { @@ -686,18 +671,18 @@ TEST_F(UserFlexTest, LoadMSHSurfaceASCII_41_Success) { EXPECT_EQ(m->flex_dim[0], 2); // first node x y z - EXPECT_EQ(d->flexvert_xpos[0], -0.5 ); - EXPECT_EQ(d->flexvert_xpos[1], -0.5 ); - EXPECT_EQ(d->flexvert_xpos[2], 0 ); + EXPECT_EQ(d->flexvert_xpos[0], -0.5); + EXPECT_EQ(d->flexvert_xpos[1], -0.5); + EXPECT_EQ(d->flexvert_xpos[2], 0); // first element - EXPECT_EQ(m->flex_elem[0], 9-1 ); - EXPECT_EQ(m->flex_elem[1], 4-1 ); - EXPECT_EQ(m->flex_elem[2], 3-1 ); + EXPECT_EQ(m->flex_elem[0], 9 - 1); + EXPECT_EQ(m->flex_elem[1], 4 - 1); + EXPECT_EQ(m->flex_elem[2], 3 - 1); mj_step(m, d); - mj_deleteModel(m); mj_deleteData(d); + mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHSurfaceASCII_22_Success) { @@ -714,18 +699,18 @@ TEST_F(UserFlexTest, LoadMSHSurfaceASCII_22_Success) { EXPECT_EQ(m->flex_dim[0], 2); // first node x y z - EXPECT_EQ(d->flexvert_xpos[0], -0.5 ); - EXPECT_EQ(d->flexvert_xpos[1], -0.5 ); - EXPECT_EQ(d->flexvert_xpos[2], 0 ); + EXPECT_EQ(d->flexvert_xpos[0], -0.5); + EXPECT_EQ(d->flexvert_xpos[1], -0.5); + EXPECT_EQ(d->flexvert_xpos[2], 0); // first element - EXPECT_EQ(m->flex_elem[0], 9-1 ); - EXPECT_EQ(m->flex_elem[1], 4-1 ); - EXPECT_EQ(m->flex_elem[2], 3-1 ); + EXPECT_EQ(m->flex_elem[0], 9 - 1); + EXPECT_EQ(m->flex_elem[1], 4 - 1); + EXPECT_EQ(m->flex_elem[2], 3 - 1); mj_step(m, d); - mj_deleteModel(m); mj_deleteData(d); + mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHASCIIFTETWILD_22_Success) { @@ -739,70 +724,69 @@ TEST_F(UserFlexTest, LoadMSHASCIIFTETWILD_22_Success) { EXPECT_EQ(m->nflexelem, 1070); EXPECT_EQ(m->flex_dim[0], 3); mj_step(m, d); - mj_deleteModel(m); mj_deleteData(d); + mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHASCII_41_MissingNodeHeader_Fail) { - const std::string xml_path = - GetTestDataFilePath( - "user/testdata/malformed_cube_41_ascii_missing_node_header.xml"); + const std::string xml_path = GetTestDataFilePath( + "user/testdata/malformed_cube_41_ascii_missing_node_header.xml"); std::array error; mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); - EXPECT_THAT(error.data(), HasSubstr( - "XML Error: Error: All nodes must be in single block")); + EXPECT_THAT(error.data(), + HasSubstr("XML Error: Error: All nodes must be in single block")); mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHASCII_41_MissingNodeIndex_Fail) { - const std::string xml_path = - GetTestDataFilePath( - "user/testdata/malformed_cube_41_ascii_missing_node_index.xml"); + const std::string xml_path = GetTestDataFilePath( + "user/testdata/malformed_cube_41_ascii_missing_node_index.xml"); std::array error; mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); - EXPECT_THAT(error.data(), HasSubstr( - "XML Error: Error: Node tags must be sequential")); + EXPECT_THAT(error.data(), + HasSubstr("XML Error: Error: Node tags must be sequential")); mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHASCII_41_MissingElementHeader_Fail) { - const std::string xml_path = - GetTestDataFilePath( - "user/testdata/malformed_cube_41_ascii_missing_element_header.xml"); + const std::string xml_path = GetTestDataFilePath( + "user/testdata/malformed_cube_41_ascii_missing_element_header.xml"); std::array error; mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); - EXPECT_THAT(error.data(), HasSubstr( - "XML Error: Error: All elements must be in single block")); + EXPECT_THAT( + error.data(), + HasSubstr("XML Error: Error: All elements must be in single block")); mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHASCII_41_MissingElement_Fail) { - const std::string xml_path = - GetTestDataFilePath( - "user/testdata/malformed_cube_41_ascii_missing_element.xml"); + const std::string xml_path = GetTestDataFilePath( + "user/testdata/malformed_cube_41_ascii_missing_element.xml"); std::array error; mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); - EXPECT_THAT(error.data(), HasSubstr( - "XML Error: Error: Error reading Elements")); + EXPECT_THAT(error.data(), + HasSubstr("XML Error: Error: Error reading Elements")); mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHASCII_41_MismatchBetweenMaxNodesAndNodesInBlock_Fail) { - const std::string xml_path = - GetTestDataFilePath( - "user/testdata/malformed_cube_41_ascii_mismatch_between_max_nodes_and_nodes_in_block.xml"); + const std::string xml_path = GetTestDataFilePath( + "user/testdata/" + "malformed_cube_41_ascii_mismatch_between_max_nodes_and_nodes_in_block." + "xml"); std::array error; mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); - EXPECT_THAT(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")); + EXPECT_THAT( + 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); } TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumNodes_Fail) { - const std::string xml_path = - GetTestDataFilePath( - "user/testdata/malformed_cube_22_ascii_missing_num_nodes.xml"); + const std::string xml_path = GetTestDataFilePath( + "user/testdata/malformed_cube_22_ascii_missing_num_nodes.xml"); std::array error; mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); // TODO(mohammadhamid): Replace with an assertion about the error message. For @@ -812,20 +796,18 @@ TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumNodes_Fail) { } TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNode_Fail) { - const std::string xml_path = - GetTestDataFilePath( - "user/testdata/malformed_cube_22_ascii_missing_node.xml"); + const std::string xml_path = GetTestDataFilePath( + "user/testdata/malformed_cube_22_ascii_missing_node.xml"); std::array error; mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); - EXPECT_THAT(error.data(), HasSubstr( - "XML Error: Error: Error reading node tags")); + EXPECT_THAT(error.data(), + HasSubstr("XML Error: Error: Error reading node tags")); mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumElements_Fail) { - const std::string xml_path = - GetTestDataFilePath( - "user/testdata/malformed_shark_22_ascii_missing_num_elements.xml"); + const std::string xml_path = GetTestDataFilePath( + "user/testdata/malformed_shark_22_ascii_missing_num_elements.xml"); std::array error; mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); // TODO(mohammadhamid): Replace with an assertion about the error message. For @@ -835,20 +817,18 @@ TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumElements_Fail) { } TEST_F(UserFlexTest, LoadMSHASCII_22_MissingElement_Fail) { - const std::string xml_path = - GetTestDataFilePath( - "user/testdata/malformed_cube_22_ascii_missing_element.xml"); + const std::string xml_path = GetTestDataFilePath( + "user/testdata/malformed_cube_22_ascii_missing_element.xml"); std::array error; mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); - EXPECT_THAT(error.data(), HasSubstr( - "XML Error: Error: Error reading Elements")); + EXPECT_THAT(error.data(), + HasSubstr("XML Error: Error: Error reading Elements")); mj_deleteModel(m); } TEST_F(UserFlexTest, LoadMSHASCII_dim_missing_in_xml) { - const std::string xml_path = - GetTestDataFilePath( - "user/testdata/cube_22_ascii_vol_gmshApp_missing_dim.xml"); + const std::string xml_path = GetTestDataFilePath( + "user/testdata/cube_22_ascii_vol_gmshApp_missing_dim.xml"); std::array error; mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); EXPECT_EQ(m->flex_dim[0], 3); @@ -871,9 +851,8 @@ TEST_F(UserFlexTest, TrilinearUnusedVertices_Crash) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, testing::NotNull()) << error.data(); - mj_deleteModel(m); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), testing::NotNull()) << error.data(); } TEST_F(UserFlexTest, MeshNodePinning) { @@ -891,8 +870,8 @@ TEST_F(UserFlexTest, MeshNodePinning) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, testing::NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), testing::NotNull()) << error.data(); // Verify that node 0 (corner) is pinned // 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. // Model has 1 world body + flex bodies. EXPECT_EQ(m->nbody, 1 + 7); // 1 world + 7 flex nodes (1 pinned) - - mj_deleteModel(m); } TEST_F(UserFlexTest, FlexcompMeshLoadsFromVFS) { // read cube.stl from testdata into a buffer - const std::string stl_path = - GetTestDataFilePath("user/testdata/cube.stl"); + const std::string stl_path = GetTestDataFilePath("user/testdata/cube.stl"); FILE* f = fopen(stl_path.c_str(), "rb"); ASSERT_THAT(f, NotNull()) << "Could not open " << stl_path; fseek(f, 0, SEEK_END); @@ -938,12 +914,10 @@ TEST_F(UserFlexTest, FlexcompMeshLoadsFromVFS) { // cleanup std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size(), &vfs); - ASSERT_THAT(m, NotNull()) << error.data(); - mjData* d = mj_makeData(m); - mj_step(m, d); - mj_deleteData(d); - mj_deleteModel(m); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size(), &vfs); + ASSERT_THAT(m.get(), NotNull()) << error.data(); + MjDataPtr d = MakeData(m); + mj_step(m.get(), d.get()); mj_deleteVFS(&vfs); } @@ -986,22 +960,20 @@ TEST_F(UserFlexTest, FlexAttachConstraintPreserved) { // First verify the standalone flex model has constraints std::array error; - mjModel* m_standalone = + MjModelPtr m_standalone = LoadModelFromString(flex_xml, error.data(), error.size(), vfs.get()); - ASSERT_THAT(m_standalone, NotNull()) << error.data(); - mjData* d_standalone = mj_makeData(m_standalone); - mj_forward(m_standalone, d_standalone); + ASSERT_THAT(m_standalone.get(), NotNull()) << error.data(); + MjDataPtr d_standalone = MakeData(m_standalone); + mj_forward(m_standalone.get(), d_standalone.get()); int standalone_neq = m_standalone->neq; 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 - mjModel* m_attached = + MjModelPtr m_attached = LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get()); - ASSERT_THAT(m_attached, NotNull()) << error.data(); - mjData* d_attached = mj_makeData(m_attached); - mj_forward(m_attached, d_attached); + ASSERT_THAT(m_attached.get(), NotNull()) << error.data(); + MjDataPtr d_attached = MakeData(m_attached); + mj_forward(m_attached.get(), d_attached.get()); // THE BUG: flex constraints disappear when attached EXPECT_GT(m_attached->neq, 0) @@ -1009,8 +981,6 @@ TEST_F(UserFlexTest, FlexAttachConstraintPreserved) { EXPECT_EQ(m_attached->neq, standalone_neq) << "Attached flex should have same number of constraints as standalone"; - mj_deleteData(d_attached); - mj_deleteModel(m_attached); mj_deleteVFS(vfs.get()); } @@ -1024,11 +994,10 @@ TEST_F(UserFlexTest, FlexNoConstraintsWarning) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("no equality constraints or passive forces")); - mj_deleteModel(m); } TEST_F(UserFlexTest, EmptyCellNodePinning) { @@ -1046,8 +1015,8 @@ TEST_F(UserFlexTest, EmptyCellNodePinning) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); // A 2x2x2 grid with trilinear order has (2+1)^3 = 27 node positions. int nadr = m->flex_nodeadr[0]; @@ -1065,20 +1034,16 @@ TEST_F(UserFlexTest, EmptyCellNodePinning) { EXPECT_EQ(pinned, 0); // Verify simulation works - mjData* d = mj_makeData(m); + MjDataPtr d = MakeData(m); 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) { // Load bunny_multicell.xml which has a 3x3x3 grid. // The bunny mesh only occupies some cells, so many nodes should be pinned. - const std::string xml_path = - GetModelPath("flex/bunny_multicell.xml"); + const std::string xml_path = GetModelPath("flex/bunny_multicell.xml"); std::array error; mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); ASSERT_THAT(m, NotNull()) << error.data(); @@ -1152,8 +1117,8 @@ TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); // 3x1x1 quadratic grid: (3*2+1) * (1*2+1) * (1*2+1) = 7*3*3 = 63 nodes int nadr = m->flex_nodeadr[0]; @@ -1161,7 +1126,7 @@ TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) { EXPECT_EQ(nnode, 63); // 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); int pinned = 0; for (int n = nadr; n < nadr + nnode; n++) { @@ -1175,8 +1140,7 @@ TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) { // That's 4 * 3 * 3 = 36 nodes. EXPECT_EQ(pinned, 36); - mj_deleteData(mj_makeData(m)); - mj_deleteModel(m); + mj_deleteData(mj_makeData(m.get())); } TEST_F(UserFlexTest, EmptyCellDetectsElements) { @@ -1208,8 +1172,8 @@ TEST_F(UserFlexTest, EmptyCellDetectsElements) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); // 6x6x6 trilinear grid: (6+1)^3 = 343 nodes int nadr = m->flex_nodeadr[0]; @@ -1217,7 +1181,7 @@ TEST_F(UserFlexTest, EmptyCellDetectsElements) { ASSERT_EQ(nnode, 343); // 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); int pinned = 0; for (int n = nadr; n < nadr + nnode; n++) { @@ -1233,8 +1197,7 @@ TEST_F(UserFlexTest, EmptyCellDetectsElements) { // No cells are empty → 0 nodes pinned. EXPECT_EQ(pinned, 0); - mj_deleteData(mj_makeData(m)); - mj_deleteModel(m); + mj_deleteData(mj_makeData(m.get())); } TEST_F(UserFlexTest, TotalMassTrilinear) { @@ -1249,8 +1212,8 @@ TEST_F(UserFlexTest, TotalMassTrilinear) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); double total_mass = 0; for (int i = 1; i < m->nbody; ++i) { @@ -1258,7 +1221,6 @@ TEST_F(UserFlexTest, TotalMassTrilinear) { } EXPECT_NEAR(total_mass, 1.5, 1e-5); - mj_deleteModel(m); } TEST_F(UserFlexTest, TotalMassQuadratic) { @@ -1273,8 +1235,8 @@ TEST_F(UserFlexTest, TotalMassQuadratic) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); double total_mass = 0; for (int i = 1; i < m->nbody; ++i) { @@ -1282,7 +1244,6 @@ TEST_F(UserFlexTest, TotalMassQuadratic) { } EXPECT_NEAR(total_mass, 2.0, 1e-5); - mj_deleteModel(m); } TEST_F(UserFlexTest, Dof2d) { @@ -1298,7 +1259,8 @@ TEST_F(UserFlexTest, Dof2d) { )"; - // 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"( @@ -1313,14 +1275,14 @@ TEST_F(UserFlexTest, Dof2d) { std::array error; // load 2d model - mjModel* m_2d = LoadModelFromString(xml_2d, error.data(), error.size()); - ASSERT_THAT(m_2d, NotNull()) << error.data(); - mjData* d_2d = mj_makeData(m_2d); + MjModelPtr m_2d = LoadModelFromString(xml_2d, error.data(), error.size()); + ASSERT_THAT(m_2d.get(), NotNull()) << error.data(); + MjDataPtr d_2d = MakeData(m_2d); // load full model - mjModel* m_full = LoadModelFromString(xml_full, error.data(), error.size()); - ASSERT_THAT(m_full, NotNull()) << error.data(); - mjData* d_full = mj_makeData(m_full); + MjModelPtr m_full = LoadModelFromString(xml_full, error.data(), error.size()); + ASSERT_THAT(m_full.get(), NotNull()) << error.data(); + MjDataPtr d_full = MakeData(m_full); // verify DOF counts 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 for (int i = 0; i < 10; i++) { - mj_step(m_2d, d_2d); - mj_step(m_full, d_full); + mj_step(m_2d.get(), d_2d.get()); + 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) { @@ -1380,11 +1337,12 @@ TEST_F(UserFlexTest, Vert0RotationInvariant) { )"; std::array error; - mjModel* m1 = LoadModelFromString(xml_unrotated, error.data(), error.size()); - ASSERT_THAT(m1, NotNull()) << error.data(); + MjModelPtr m1 = + LoadModelFromString(xml_unrotated, error.data(), error.size()); + ASSERT_THAT(m1.get(), NotNull()) << error.data(); - mjModel* m2 = LoadModelFromString(xml_rotated, error.data(), error.size()); - ASSERT_THAT(m2, NotNull()) << error.data(); + MjModelPtr m2 = LoadModelFromString(xml_rotated, error.data(), error.size()); + ASSERT_THAT(m2.get(), NotNull()) << error.data(); // same number of vertices 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) << "vert0 mismatch at index " << i; } - - mj_deleteModel(m1); - mj_deleteModel(m2); } TEST_F(UserFlexTest, Load1DFlexFromOBJ) { @@ -1413,4 +1368,3 @@ TEST_F(UserFlexTest, Load1DFlexFromOBJ) { } // namespace } // namespace mujoco - diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 328f9d87..b5025515 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -37,16 +37,12 @@ namespace { using MjCMeshTest = MujocoTest; -static const char* const kMeshPath = - "user/testdata/mesh.xml"; +static const char* const kMeshPath = "user/testdata/mesh.xml"; static const char* const kDuplicateVerticesPath = "user/testdata/duplicate_vertices.xml"; -static const char* const kCubePath = - "user/testdata/cube.xml"; -static const char* const kCubeCompletePath = - "user/testdata/cube_complete.obj"; -static const char* const kTorusPath = - "user/testdata/torus.xml"; +static const char* const kCubePath = "user/testdata/cube.xml"; +static const char* const kCubeCompletePath = "user/testdata/cube_complete.obj"; +static const char* const kTorusPath = "user/testdata/torus.xml"; static const char* const kTorusMaxhullVertPath = "user/testdata/torus_maxhullvert.xml"; static const char* const kTorusDefaultMaxhullVertPath = @@ -57,18 +53,14 @@ static const char* const kConvexInertiaPath = "user/testdata/inertia_convex.xml"; static const char* const kConcaveInertiaPath = "user/testdata/inertia_concave.xml"; -static const char* const kShellInertiaPath = - "user/testdata/inertia_shell.xml"; -static const char* const kTorusQuadsPath = - "user/testdata/torus_quads.xml"; +static const char* const kShellInertiaPath = "user/testdata/inertia_shell.xml"; +static const char* const kTorusQuadsPath = "user/testdata/torus_quads.xml"; static const char* const kTexturedTorusPath = "user/testdata/textured_torus.xml"; -static const char* const kDuplicateOBJPath = - "user/testdata/duplicate.xml"; +static const char* const kDuplicateOBJPath = "user/testdata/duplicate.xml"; static const char* const kMalformedFaceOBJPath = "user/testdata/malformed_face.xml"; -static const char* const kCubeSkinPath = - "user/testdata/cube_skin.xml"; +static const char* const kCubeSkinPath = "user/testdata/cube_skin.xml"; static const char* const kNoDecoderForMeshErrorMsh = "no decoder found for mesh"; @@ -94,13 +86,8 @@ TEST_F(MjCMeshTest, UnknownMeshFormat) { )"; - std::vector invalid_names = { - "noextension", - "anobj", - "f", - "mesh.exe", - "file%s" - }; + std::vector invalid_names = {"noextension", "anobj", "f", + "mesh.exe", "file%s"}; mjVFS vfs; mj_defaultVFS(&vfs); @@ -108,9 +95,9 @@ TEST_F(MjCMeshTest, UnknownMeshFormat) { mj_addBufferVFS(&vfs, name.c_str(), nullptr, 0); std::string xml = absl::StrFormat(xml_format, name); std::array error; - mjModel* model = + MjModelPtr model = 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; EXPECT_THAT(error.data(), HasSubstr(kNoDecoderForMeshErrorMsh)); EXPECT_THAT(error.data(), HasSubstr(name)); @@ -141,8 +128,8 @@ TEST_F(MjCMeshTest, LoadMSHWithVFS) { mj_defaultVFS(vfs.get()); // should fallback to OS filesystem - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -167,8 +154,8 @@ TEST_F(MjCMeshTest, LoadOBJWithVFS) { mj_defaultVFS(vfs.get()); // should fallback to OS filesystem - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -193,8 +180,8 @@ TEST_F(MjCMeshTest, LoadSTLWithVFS) { mj_defaultVFS(vfs.get()); // should fallback to OS filesystem - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -221,8 +208,8 @@ TEST_F(MjCMeshTest, LoadMSHWithContentType) { mj_defaultVFS(vfs.get()); // should try opening the file (not found obviously) - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -247,8 +234,8 @@ TEST_F(MjCMeshTest, LoadOBJWithContentType) { mj_defaultVFS(vfs.get()); // should try opening the file (not found obviously) - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -273,8 +260,8 @@ TEST_F(MjCMeshTest, LoadSTLWithContentType) { mj_defaultVFS(vfs.get()); // should try opening the file (not found obviously) - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -300,8 +287,8 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeError) { mj_addBufferVFS(&vfs, "some_file", nullptr, 0); // should error with unknown file type - mjModel* model = LoadModelFromString(xml, error, error_sz, &vfs); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, &vfs); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr(kNoDecoderForMeshErrorMsh)); mj_deleteVFS(&vfs); } @@ -327,8 +314,8 @@ TEST_F(MjCMeshTest, LoadMSHWithInvalidContentType) { mj_addBufferVFS(&vfs, "some_file", nullptr, 0); // should error with unknown file type - mjModel* model = LoadModelFromString(xml, error, error_sz, &vfs); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, &vfs); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr(kNoDecoderForMeshErrorMsh)); mj_deleteVFS(&vfs); } @@ -353,8 +340,8 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeParam) { mj_defaultVFS(vfs.get()); // should try opening the file (not found obviously) - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -438,8 +425,7 @@ TEST_F(MjCMeshTest, SaveMeshOnce) { std::array error; mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); // Confirm the mesh file is loaded and stored in paths - EXPECT_EQ( - std::string(&model->paths[model->mesh_pathadr[0]]), "cube.obj"); + EXPECT_EQ(std::string(&model->paths[model->mesh_pathadr[0]]), "cube.obj"); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, Not(testing::HasSubstr("vertex"))); mj_deleteModel(model); @@ -457,9 +443,8 @@ TEST_F(MjCMeshTest, TinyMeshLoads) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; } // ------------- test max hull vert ------------------------------------------- @@ -498,9 +483,8 @@ TEST_F(MjCMeshTest, FaceNormalAutogenerated) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; } // ------------- test inertia ------------------------------------------------- @@ -521,9 +505,8 @@ TEST_F(MjCMeshTest, SmallInertiaLoads) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; } TEST_F(MjCMeshTest, TinyInertiaFails) { @@ -586,11 +569,13 @@ TEST_F(MjCMeshTest, FlippedFaceFailsExactInertia) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, testing::IsNull()); - EXPECT_THAT(error.data(), HasSubstr( - "Error: faces of mesh 'example_mesh' have inconsistent orientation. " - "Please check the faces containing the vertices 1 and 2.")); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), testing::IsNull()); + EXPECT_THAT( + error.data(), + HasSubstr( + "Error: faces of mesh 'example_mesh' have inconsistent orientation. " + "Please check the faces containing the vertices 1 and 2.")); } void CheckTetrahedronWasRescaled(mjModel* model) { @@ -598,12 +583,12 @@ void CheckTetrahedronWasRescaled(mjModel* model) { // with vertices (0, 0, 0), (1, 0, 0), (0, 2, 0), (0, 0, 3) // after mesh preprocessing is performed std::vector vert = { - -0.51610732078552246, -0.57402724027633667, -0.5283237099647522, - 0.42337465286254883, -0.90627568960189819, -0.61189728975296021, - 0.065528042614459991, 1.2306677103042603, -1.1645441055297852, - 0.027204651385545731, 0.24963514506816864, 2.3047652244567871}; + -0.51610732078552246, -0.57402724027633667, -0.5283237099647522, + 0.42337465286254883, -0.90627568960189819, -0.61189728975296021, + 0.065528042614459991, 1.2306677103042603, -1.1645441055297852, + 0.027204651385545731, 0.24963514506816864, 2.3047652244567871}; mjtNum tolerance = std::numeric_limits::epsilon(); - for (int i=0; i < 12; ++i) { + for (int i = 0; i < 12; ++i) { EXPECT_NEAR(model->mesh_vert[i], vert[i], tolerance); } } @@ -622,10 +607,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedWorld) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - CheckTetrahedronWasRescaled(model); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + CheckTetrahedronWasRescaled(model.get()); } TEST_F(MjCMeshTest, FlippedFaceAllowedNoMass) { @@ -644,10 +628,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNoMass) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - CheckTetrahedronWasRescaled(model); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + CheckTetrahedronWasRescaled(model.get()); } TEST_F(MjCMeshTest, FlippedFaceAllowedInertial) { @@ -667,10 +650,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedInertial) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - CheckTetrahedronWasRescaled(model); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + CheckTetrahedronWasRescaled(model.get()); } TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) { @@ -689,10 +671,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - CheckTetrahedronWasRescaled(model); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; + CheckTetrahedronWasRescaled(model.get()); } TEST_F(MjCMeshTest, AreaTooSmall) { @@ -711,8 +692,8 @@ TEST_F(MjCMeshTest, AreaTooSmall) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small")); } @@ -732,10 +713,9 @@ TEST_F(MjCMeshTest, VolumeTooSmall) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, testing::IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), testing::IsNull()); EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small")); - mj_deleteModel(model); } TEST_F(MjCMeshTest, VisualVolumeTooSmall) { @@ -759,10 +739,9 @@ TEST_F(MjCMeshTest, VisualVolumeTooSmall) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, testing::IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), testing::IsNull()); EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small")); - mj_deleteModel(model); } TEST_F(MjCMeshTest, VisualVolumeSmallAllowedShell) { @@ -786,12 +765,11 @@ TEST_F(MjCMeshTest, VisualVolumeSmallAllowedShell) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_LE(mju_abs(model->geom_size[0]), 1); EXPECT_LE(mju_abs(model->geom_size[1]), 1); EXPECT_LE(mju_abs(model->geom_size[2]), 1); - mj_deleteModel(model); } TEST_F(MjCMeshTest, VolumeSmallAllowedShell) { @@ -810,12 +788,11 @@ TEST_F(MjCMeshTest, VolumeSmallAllowedShell) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_LE(mju_abs(model->geom_size[0]), 1); EXPECT_LE(mju_abs(model->geom_size[1]), 1); EXPECT_LE(mju_abs(model->geom_size[2]), 1); - mj_deleteModel(model); } TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) { @@ -829,8 +806,8 @@ TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, testing::IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), testing::IsNull()); EXPECT_THAT(error.data(), HasSubstr("2d elasticity requires positive thickness")); } @@ -847,9 +824,8 @@ TEST_F(MjCMeshTest, InterpolatedFlexSupportsBendElasticityWithWarning) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, testing::NotNull()) << error.data(); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), testing::NotNull()) << error.data(); } TEST_F(MjCMeshTest, InterpolatedFlexSupportsBothElasticityWithWarning) { @@ -864,9 +840,8 @@ TEST_F(MjCMeshTest, InterpolatedFlexSupportsBothElasticityWithWarning) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, testing::NotNull()) << error.data(); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), testing::NotNull()) << error.data(); } TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) { @@ -880,8 +855,8 @@ TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, testing::IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), testing::IsNull()); EXPECT_THAT(error.data(), HasSubstr("2d elasticity requires 2d flex")); } @@ -908,23 +883,24 @@ TEST_F(MjCMeshTest, VolumeNegativeThrowsError) { for (int nmesh : {3, 16, 17, 50}) { std::string mesh_definitions = ""; - for (int i = 1; i < nmesh+1; i++) { + for (int i = 1; i < nmesh + 1; i++) { mesh_definitions += absl::StrFormat(bad_mesh, i); } std::string geom_definitions = ""; - for (int i = 1; i < nmesh+1; i++) { + for (int i = 1; i < nmesh + 1; i++) { geom_definitions += absl::StrFormat(geom, i); } std::string xml_str = xml; absl::StrReplaceAll({{"MESH_DEFINITIONS", mesh_definitions}, - {"GEOM_DEFINITIONS", geom_definitions}}, &xml_str); + {"GEOM_DEFINITIONS", geom_definitions}}, + &xml_str); std::array error; - mjModel* model = LoadModelFromString(xml_str.c_str(), - error.data(), error.size()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = + LoadModelFromString(xml_str.c_str(), error.data(), error.size()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("mesh volume is negative")); } } @@ -964,22 +940,22 @@ TEST_F(MjCMeshTest, ExactConcaveInertia) { mjtNum m_hole = .9 * .8 * .8 * density; mjtNum m_cube = 1. * density; mjtNum m_concave_cube = m_cube - m_hole; - mjtNum I_cube = m_cube/6.; + mjtNum I_cube = m_cube / 6.; // due to the asymmetric hole, the com position has changed // so we need to use https://en.wikipedia.org/wiki/Parallel_axis_theorem mjtNum d_cube = .5 - model->body_ipos[5]; mjtNum d_hole = .55 - model->body_ipos[5]; - mjtNum I1 = I_cube - m_hole*(.8*.8 + .8*.8)/12; - mjtNum I2 = I_cube - m_hole*(.8*.8 + .9*.9)/12 - + m_cube*d_cube*d_cube - m_hole*d_hole*d_hole; + mjtNum I1 = I_cube - m_hole * (.8 * .8 + .8 * .8) / 12; + mjtNum I2 = I_cube - m_hole * (.8 * .8 + .9 * .9) / 12 + + 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[2], m_concave_cube, kMaxAbsErr); EXPECT_NEAR(model->body_mass[3], m_concave_cube, kMaxAbsErr); EXPECT_NEAR(model->body_mass[4], m_concave_cube, kMaxAbsErr); for (int i = 3; i < 15; i += 3) { EXPECT_NEAR(model->body_inertia[i], I1, kMaxAbsErr); - EXPECT_NEAR(model->body_inertia[i+1], I2, kMaxAbsErr); - EXPECT_NEAR(model->body_inertia[i+2], I2, kMaxAbsErr); + EXPECT_NEAR(model->body_inertia[i + 1], I2, kMaxAbsErr); + EXPECT_NEAR(model->body_inertia[i + 2], I2, kMaxAbsErr); } mj_deleteModel(model); } @@ -990,7 +966,7 @@ TEST_F(MjCMeshTest, ExactConvexInertia) { mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); // https://en.wikipedia.org/wiki/List_of_moments_of_inertia mjtNum m_solid_cube = 1.; - mjtNum I_solid_cube = 1./6. * m_solid_cube; + mjtNum I_solid_cube = 1. / 6. * m_solid_cube; EXPECT_LE(mju_abs(model->body_mass[1] - m_solid_cube), kMaxAbsErr); EXPECT_LE(mju_abs(model->body_mass[2] - m_solid_cube), kMaxAbsErr); for (int i = 3; i < 9; i++) { @@ -1005,7 +981,7 @@ TEST_F(MjCMeshTest, ExactShellInertia) { mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); // see https://en.wikipedia.org/wiki/List_of_moments_of_inertia mjtNum m_hollow_cube = 6.; - mjtNum I_hollow_cube = 5./18. * m_hollow_cube; + mjtNum I_hollow_cube = 5. / 18. * m_hollow_cube; EXPECT_LE(mju_abs(model->body_mass[1] - m_hollow_cube), kMaxAbsErr); EXPECT_LE(mju_abs(model->body_inertia[3] - I_hollow_cube), kMaxAbsErr); EXPECT_LE(mju_abs(model->body_inertia[4] - I_hollow_cube), kMaxAbsErr); @@ -1026,8 +1002,8 @@ TEST_F(MjCMeshTest, MeshPosQuat) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; // 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 // 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 mjtNum inverse_mesh_pos[3]; mjtNum inverse_mesh_quat[4]; - mju_negPose(inverse_mesh_pos, inverse_mesh_quat, - &model->mesh_pos[0], &model->mesh_quat[0]); + mju_negPose(inverse_mesh_pos, inverse_mesh_quat, &model->mesh_pos[0], + &model->mesh_quat[0]); // apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and // quat. It should match the originally specified values mjtNum recovered_pos[3]; mjtNum recovered_quat[4]; - mju_mulPose(recovered_pos, recovered_quat, - &model->geom_pos[0], &model->geom_quat[0], - inverse_mesh_pos, inverse_mesh_quat); + mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[0], + &model->geom_quat[0], inverse_mesh_pos, inverse_mesh_quat); 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[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)); // same test on the other geom - mju_negPose(inverse_mesh_pos, inverse_mesh_quat, - &model->mesh_pos[0], &model->mesh_quat[0]); - mju_mulPose(recovered_pos, recovered_quat, - &model->geom_pos[3], &model->geom_quat[4], - inverse_mesh_pos, inverse_mesh_quat); + mju_negPose(inverse_mesh_pos, inverse_mesh_quat, &model->mesh_pos[0], + &model->mesh_quat[0]); + mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[3], + &model->geom_quat[4], inverse_mesh_pos, inverse_mesh_quat); 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[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[2], 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) { @@ -1085,8 +1057,8 @@ TEST_F(MjCMeshTest, MeshPosQuatShellInertia) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; // 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 // 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 mjtNum inverse_mesh_pos[3]; mjtNum inverse_mesh_quat[4]; - mju_negPose(inverse_mesh_pos, inverse_mesh_quat, - &model->mesh_pos[0], &model->mesh_quat[0]); + mju_negPose(inverse_mesh_pos, inverse_mesh_quat, &model->mesh_pos[0], + &model->mesh_quat[0]); // apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and // quat. It should match the originally specified values mjtNum recovered_pos[3]; mjtNum recovered_quat[4]; - mju_mulPose(recovered_pos, recovered_quat, - &model->geom_pos[0], &model->geom_quat[0], - inverse_mesh_pos, inverse_mesh_quat); + mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[0], + &model->geom_quat[0], inverse_mesh_pos, inverse_mesh_quat); 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[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)); // same test on the other geom - mju_negPose(inverse_mesh_pos, inverse_mesh_quat, - &model->mesh_pos[0], &model->mesh_quat[0]); - mju_mulPose(recovered_pos, recovered_quat, - &model->geom_pos[3], &model->geom_quat[4], - inverse_mesh_pos, inverse_mesh_quat); + mju_negPose(inverse_mesh_pos, inverse_mesh_quat, &model->mesh_pos[0], + &model->mesh_quat[0]); + mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[3], + &model->geom_quat[4], inverse_mesh_pos, inverse_mesh_quat); 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[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[2], 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) { @@ -1144,12 +1113,11 @@ TEST_F(MjCMeshTest, MeshScale) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1)); EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1)); - mj_deleteModel(model); } TEST_F(MjCMeshTest, NegativeScaleUserMeshCompiles) { @@ -1166,9 +1134,8 @@ TEST_F(MjCMeshTest, NegativeScaleUserMeshCompiles) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; } TEST_F(MjCMeshTest, NegativeScaleUserMeshMatchesPositiveScale) { @@ -1199,22 +1166,19 @@ TEST_F(MjCMeshTest, NegativeScaleUserMeshMatchesPositiveScale) { )"; char error[1024]; - mjModel* pos_model = LoadModelFromString(pos_xml, error, sizeof(error)); - ASSERT_THAT(pos_model, NotNull()) << error; + MjModelPtr pos_model = LoadModelFromString(pos_xml, error, sizeof(error)); + ASSERT_THAT(pos_model.get(), NotNull()) << error; - mjModel* neg_model = LoadModelFromString(neg_xml, error, sizeof(error)); - ASSERT_THAT(neg_model, NotNull()) << error; + MjModelPtr neg_model = LoadModelFromString(neg_xml, error, sizeof(error)); + ASSERT_THAT(neg_model.get(), NotNull()) << error; ASSERT_EQ(pos_model->nmeshface, neg_model->nmeshface); for (int i = 0; i < pos_model->nmeshface; i++) { - EXPECT_EQ(pos_model->mesh_face[3*i + 0], neg_model->mesh_face[3*i + 0]); - 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 + 0], neg_model->mesh_face[3 * i + 0]); + 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]); } - - mj_deleteModel(pos_model); - mj_deleteModel(neg_model); } TEST_F(MjCMeshTest, ShellInertiaTest) { @@ -1231,12 +1195,11 @@ TEST_F(MjCMeshTest, ShellInertiaTest) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1)); EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1)); - mj_deleteModel(model); } // ----------------------------- texcoord ------------------------------------- @@ -1251,9 +1214,8 @@ TEST_F(MjCMeshTest, CreateFaceTexCoord) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; } TEST_F(MjCMeshTest, UseFaceTexCoord) { @@ -1267,21 +1229,20 @@ TEST_F(MjCMeshTest, UseFaceTexCoord) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, 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[ 1]], .2); - EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 2]], .1); - EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 3]], .0); - EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 4]], .1); - EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 5]], .3); - EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 6]], .2); - EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 7]], .0); - EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 8]], .3); - 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[11]], .3); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + 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[1]], .2); + EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[2]], .1); + EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[3]], .0); + EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[4]], .1); + EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[5]], .3); + EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[6]], .2); + EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[7]], .0); + EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[8]], .3); + 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[11]], .3); } TEST_F(MjCMeshTest, MissingTexCoord) { @@ -1294,8 +1255,8 @@ TEST_F(MjCMeshTest, MissingTexCoord) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, testing::IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), testing::IsNull()); EXPECT_THAT(error.data(), HasSubstr("texcoord must be 2*nv")); } @@ -1312,10 +1273,9 @@ TEST_F(MjCMeshTest, NaNConvexHullDisallowed) { MockWarningHandler warning_handler; warning_handler.ExpectWarnings(); std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, testing::IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), testing::IsNull()); EXPECT_THAT(error.data(), HasSubstr("vertex coordinate 0 is not finite")); - mj_deleteModel(model); } TEST_F(MjCMeshTest, InvalidIndexInFace) { @@ -1333,10 +1293,9 @@ TEST_F(MjCMeshTest, InvalidIndexInFace) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("in face 0, vertex index 6 does not exist")); - mj_deleteModel(model); } TEST_F(MjCMeshTest, QhullCache) { @@ -1351,7 +1310,7 @@ TEST_F(MjCMeshTest, QhullCache) { )"; - static constexpr char xml2[] = R"( + static constexpr char xml2[] = R"( @@ -1367,17 +1326,16 @@ TEST_F(MjCMeshTest, QhullCache) { mj_addFileVFS(&vfs, "", GetTestDataFilePath(kCubeCompletePath).c_str()); std::array error; - mjModel* model = LoadModelFromString(xml1, error.data(), error.size(), &vfs); - ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + MjModelPtr model = + 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); - mj_deleteModel(model); - 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); - - mj_deleteModel(model); mj_deleteVFS(&vfs); } @@ -1395,8 +1353,8 @@ TEST_F(MjCMeshTest, ColocatedMeshError) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("colocated")); } @@ -1414,8 +1372,8 @@ TEST_F(MjCMeshTest, CollinearMeshError) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("collinear")); } @@ -1433,8 +1391,8 @@ TEST_F(MjCMeshTest, CoplanarMeshError) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), IsNull()); 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 center2 = aabb2[dim]; const mjtNum half_size2 = aabb2[dim + 3]; - const mjtNum gap = - std::abs(center1 - center2) - (half_size1 + half_size2); + const mjtNum gap = std::abs(center1 - center2) - (half_size1 + half_size2); if (std::abs(gap) < kEps) { touching_dims++; } else if (gap < -kEps) { @@ -1533,15 +1490,15 @@ TEST_F(MjCMeshTest, OctreeIsBalanced) { const int level2 = model->oct_depth[octree_adr + node2_idx]; if (std::abs(level1 - level2) > 1) { if (unbalanced_pairs < 10) { - ADD_FAILURE() - << "Nodes " << node1_idx << " (level " << level1 << ") and " - << node2_idx << " (level " << level2 << ") are not balanced." - << "\nAABB1: center=(" << aabb1[0] << ", " << aabb1[1] << ", " - << aabb1[2] << "), half_size=(" << aabb1[3] << ", " - << aabb1[4] << ", " << aabb1[5] << ")" - << "\nAABB2: center=(" << aabb2[0] << ", " << aabb2[1] << ", " - << aabb2[2] << "), half_size=(" << aabb2[3] << ", " - << aabb2[4] << ", " << aabb2[5] << ")"; + ADD_FAILURE() << "Nodes " << node1_idx << " (level " << level1 + << ") and " << node2_idx << " (level " << level2 + << ") are not balanced." + << "\nAABB1: center=(" << aabb1[0] << ", " << aabb1[1] + << ", " << aabb1[2] << "), half_size=(" << aabb1[3] + << ", " << aabb1[4] << ", " << aabb1[5] << ")" + << "\nAABB2: center=(" << aabb2[0] << ", " << aabb2[1] + << ", " << aabb2[2] << "), half_size=(" << aabb2[3] + << ", " << aabb2[4] << ", " << aabb2[5] << ")"; } unbalanced_pairs++; } @@ -1610,8 +1567,7 @@ TEST_F(MjCMeshTest, OctreeHangingNodeInterpolation) { // decide which node is finer and which is coarser const int finer_node_idx = (level1 > level2) ? node1_idx : node2_idx; - const int coarser_node_idx = - (level1 > level2) ? node2_idx : node1_idx; + const int coarser_node_idx = (level1 > level2) ? node2_idx : node1_idx; const mjtNum* coarser_aabb = &model->oct_aabb[(octree_adr + coarser_node_idx) * 6]; const mjtNum* finer_aabb = @@ -1729,12 +1685,11 @@ TEST_F(MjCMeshTest, OctreeNotComputedForNonSDF) { } mjtNum CubeSDF(mjtNum p[3], mjtNum b[3]) { - mjtNum q[3] = {mju_abs(p[0]) - b[0], - mju_abs(p[1]) - b[1], + mjtNum q[3] = {mju_abs(p[0]) - b[0], mju_abs(p[1]) - b[1], mju_abs(p[2]) - b[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[2], (mjtNum)0), 2)) + + std::pow(std::max(q[1], (mjtNum)0), 2) + + std::pow(std::max(q[2], (mjtNum)0), 2)) + std::min(std::max(q[0], std::max(q[1], q[2])), (mjtNum)0); } @@ -1751,12 +1706,12 @@ TEST_F(MjCMeshTest, OctreeCube) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); EXPECT_EQ(m->noct, 63497); - mjData* d = mj_makeData(m); + MjDataPtr d = MakeData(m); ASSERT_THAT(d, NotNull()); - mj_forward(m, d); + mj_forward(m.get(), d.get()); mjSDF sdf; int instance = 0; 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 size[3] = {1, 1, 1}; 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; } - mj_deleteModel(m); - mj_deleteData(d); } TEST_F(MjCMeshTest, HemisphereSizes) { @@ -1786,15 +1740,14 @@ TEST_F(MjCMeshTest, HemisphereSizes) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); 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[2], 2 * (2 + 1) * (2 + 2) + 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[2], 4 * (2 + 1) * (2 + 2)); - mj_deleteModel(model); } TEST_F(MjCMeshTest, SphereSizes) { @@ -1808,15 +1761,14 @@ TEST_F(MjCMeshTest, SphereSizes) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); 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[2], 2 + 10 * std::pow(4, 2)); 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[2], 20 * std::pow(4, 2)); - mj_deleteModel(model); } TEST_F(MjCMeshTest, MeshMaterial) { @@ -1835,11 +1787,10 @@ TEST_F(MjCMeshTest, MeshMaterial) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->geom_matid[0], 1); EXPECT_EQ(model->geom_matid[1], 0); - mj_deleteModel(model); } } // namespace diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 831d570b..52446a3a 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -95,15 +95,13 @@ TEST_F(UserModelTest, WeldRootID) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), NotNull()); EXPECT_THAT(AsVector(model->body_rootid, model->nbody), ElementsAre(0, 1, 1, 3, 3, 5, 5, 7, 7, 9, 9)); EXPECT_THAT(AsVector(model->body_weldid, model->nbody), ElementsAre(0, 1, 2, 3, 3, 0, 0, 0, 0, 0, 10)); - - mj_deleteModel(model); } TEST_F(UserModelTest, RepeatedNames) { @@ -119,8 +117,8 @@ TEST_F(UserModelTest, RepeatedNames) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("repeated name 'geom1' in geom")); } @@ -143,8 +141,8 @@ TEST_F(UserModelTest, SameFrame) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->geom_sameframe[0], mjSAMEFRAME_NONE); EXPECT_EQ(model->geom_sameframe[1], mjSAMEFRAME_BODY); EXPECT_EQ(model->geom_sameframe[2], mjSAMEFRAME_INERTIA); @@ -152,27 +150,24 @@ TEST_F(UserModelTest, SameFrame) { EXPECT_EQ(model->geom_sameframe[4], mjSAMEFRAME_INERTIAROT); // make data, get geom_xpos - mjData* data = mj_makeData(model); - mj_kinematics(model, data); - auto geom_xpos = AsVector(data->geom_xpos, model->ngeom*3); - auto geom_xmat = AsVector(data->geom_xmat, model->ngeom*9); + MjDataPtr data = MakeData(model); + mj_kinematics(model.get(), data.get()); + auto geom_xpos = AsVector(data->geom_xpos, model->ngeom * 3); + auto geom_xmat = AsVector(data->geom_xmat, model->ngeom * 9); // set all geom_sameframe to 0, call kinematics again for (int i = 0; i < model->ngeom; i++) { model->geom_sameframe[i] = mjSAMEFRAME_NONE; } - mj_resetData(model, data); - mj_kinematics(model, data); - auto geom_xpos2 = AsVector(data->geom_xpos, model->ngeom*3); - auto geom_xmat2 = AsVector(data->geom_xmat, model->ngeom*9); + mj_resetData(model.get(), data.get()); + mj_kinematics(model.get(), data.get()); + auto geom_xpos2 = AsVector(data->geom_xpos, model->ngeom * 3); + auto geom_xmat2 = AsVector(data->geom_xmat, model->ngeom * 9); // expect them to be equal constexpr double eps = 1e-6; EXPECT_THAT(geom_xpos, Pointwise(MjNear(eps, eps), geom_xpos2)); EXPECT_THAT(geom_xmat, Pointwise(MjNear(eps, eps), geom_xmat2)); - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(UserModelTest, ActuatorSparsity) { @@ -194,9 +189,8 @@ TEST_F(UserModelTest, ActuatorSparsity) { )"; - mjModel* m = LoadModelFromString(xml); + MjModelPtr m = LoadModelFromString(xml); ASSERT_EQ(m->nJmom, 2); - mj_deleteModel(m); } TEST_F(UserModelTest, FixedTendonSparsity) { @@ -226,8 +220,8 @@ TEST_F(UserModelTest, FixedTendonSparsity) { )"; - mjModel* m = LoadModelFromString(xml); - ASSERT_THAT(m, NotNull()); + MjModelPtr m = LoadModelFromString(xml); + ASSERT_THAT(m.get(), NotNull()); EXPECT_EQ(m->nJten, 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* colind = m->ten_J_colind + rowadr; EXPECT_THAT(std::vector(colind, colind + 3), ElementsAre(0, 1, 2)); - - mj_deleteModel(m); } TEST_F(UserModelTest, NestedZeroMassBodiesOK) { @@ -259,9 +251,8 @@ TEST_F(UserModelTest, NestedZeroMassBodiesOK) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; } TEST_F(UserModelTest, NestedZeroMassBodiesWithJointOK) { @@ -286,9 +277,8 @@ TEST_F(UserModelTest, NestedZeroMassBodiesWithJointOK) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), NotNull()) << error; } TEST_F(UserModelTest, NestedZeroMassBodiesFail) { @@ -309,14 +299,13 @@ TEST_F(UserModelTest, NestedZeroMassBodiesFail) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT( error, HasSubstr( "mass and inertia of moving bodies must be larger than mjMINVAL")); EXPECT_THAT(error, HasSubstr("Element name 'bad'")); - mj_deleteModel(model); } TEST_F(UserModelTest, ConvexHullForCollisionMeshes) { @@ -338,14 +327,14 @@ TEST_F(UserModelTest, ConvexHullForCollisionMeshes) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + 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 = - mj_name2id(model, mjOBJ_MESH, "mesh_with_hull_contype"); + mj_name2id(model.get(), mjOBJ_MESH, "mesh_with_hull_contype"); 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(with_hull_contype_id, -1); @@ -359,8 +348,6 @@ TEST_F(UserModelTest, ConvexHullForCollisionMeshes) { // mesh_with_hull_conaffinity should have a convex hull. EXPECT_NE(model->mesh_graphadr[with_hull_conaffinity_id], -1); - - mj_deleteModel(model); } TEST_F(UserModelTest, ConvexHullForPairCollisionMeshes) { @@ -380,17 +367,15 @@ TEST_F(UserModelTest, ConvexHullForPairCollisionMeshes) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + 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); // 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); - - mj_deleteModel(model); } // ------------- test automatic inference of nuser_xxx ------------------------- @@ -406,11 +391,10 @@ TEST_F(UserDataTest, AutoNUserBody) { )"; - mjModel* m = LoadModelFromString(xml); + MjModelPtr m = LoadModelFromString(xml); 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, 2), ElementsAre(2, 3, 0)); - mj_deleteModel(m); } TEST_F(UserDataTest, AutoNUserJoint) { @@ -425,11 +409,10 @@ TEST_F(UserDataTest, AutoNUserJoint) { )"; - mjModel* m = LoadModelFromString(xml); + MjModelPtr m = LoadModelFromString(xml); 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, 1), ElementsAre(2, 3, 0)); - mj_deleteModel(m); } TEST_F(UserDataTest, AutoNUserGeom) { @@ -441,11 +424,10 @@ TEST_F(UserDataTest, AutoNUserGeom) { )"; - mjModel* m = LoadModelFromString(xml); + MjModelPtr m = LoadModelFromString(xml); 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, 1), ElementsAre(2, 3, 0)); - mj_deleteModel(m); } TEST_F(UserDataTest, AutoNUserSite) { @@ -457,11 +439,10 @@ TEST_F(UserDataTest, AutoNUserSite) { )"; - mjModel* m = LoadModelFromString(xml); + MjModelPtr m = LoadModelFromString(xml); 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, 1), ElementsAre(2, 3, 0)); - mj_deleteModel(m); } TEST_F(UserDataTest, AutoNUserCamera) { @@ -473,11 +454,10 @@ TEST_F(UserDataTest, AutoNUserCamera) { )"; - mjModel* m = LoadModelFromString(xml); + MjModelPtr m = LoadModelFromString(xml); 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, 1), ElementsAre(2, 3, 0)); - mj_deleteModel(m); } TEST_F(UserDataTest, AutoNUserTendon) { @@ -499,11 +479,10 @@ TEST_F(UserDataTest, AutoNUserTendon) { )"; - mjModel* m = LoadModelFromString(xml); + MjModelPtr m = LoadModelFromString(xml); 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, 1), ElementsAre(2, 3, 0)); - mj_deleteModel(m); } TEST_F(UserDataTest, AutoNUserActuator) { @@ -521,13 +500,12 @@ TEST_F(UserDataTest, AutoNUserActuator) { )"; - mjModel* m = LoadModelFromString(xml); + MjModelPtr m = LoadModelFromString(xml); ASSERT_EQ(m->nuser_actuator, 3); EXPECT_THAT(GetRow(m->actuator_user, m->nuser_actuator, 0), ElementsAre(1, 2, 3)); EXPECT_THAT(GetRow(m->actuator_user, m->nuser_actuator, 1), ElementsAre(2, 3, 0)); - mj_deleteModel(m); } TEST_F(UserDataTest, AutoNUserSensor) { @@ -542,11 +520,10 @@ TEST_F(UserDataTest, AutoNUserSensor) { )"; - mjModel* m = LoadModelFromString(xml); + MjModelPtr m = LoadModelFromString(xml); 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, 1), ElementsAre(2, 3, 0)); - mj_deleteModel(m); } // ------------- test duplicate names ------------------------------------------ @@ -584,10 +561,10 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) { std::string fuse = absl::StrFormat(xml_template, "true"); std::string no_fuse = absl::StrFormat(xml_template, "false"); - mjModel* m_fuse = LoadModelFromString(fuse.c_str()); - mjModel* m_no_fuse = LoadModelFromString(no_fuse.c_str()); - ASSERT_THAT(m_fuse, NotNull()); - ASSERT_THAT(m_no_fuse, NotNull()); + MjModelPtr m_fuse = LoadModelFromString(fuse.c_str()); + MjModelPtr m_no_fuse = LoadModelFromString(no_fuse.c_str()); + ASSERT_THAT(m_fuse.get(), 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_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_fuse->body_bvhnum[1], 3); - mjData* d_fuse = mj_makeData(m_fuse); - mjData* d_no_fuse = mj_makeData(m_no_fuse); + MjDataPtr d_fuse = MakeData(m_fuse); + MjDataPtr d_no_fuse = MakeData(m_no_fuse); - mj_step(m_fuse, d_fuse); - mj_step(m_no_fuse, d_no_fuse); + mj_step(m_fuse.get(), d_fuse.get()); + 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"; - EXPECT_NE(d_fuse->qvel[0], 0); - - mj_deleteData(d_fuse); - mj_deleteData(d_no_fuse); - mj_deleteModel(m_fuse); - mj_deleteModel(m_no_fuse); + EXPECT_NE(d_fuse.get()->qvel[0], 0); } TEST_F(FuseStaticTest, FuseStaticActuatorReferencedBody) { @@ -642,10 +615,9 @@ TEST_F(FuseStaticTest, FuseStaticActuatorReferencedBody) { )"; std::array error; - mjModel* m = LoadModelFromString(xml_template, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml_template, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others"; - mj_deleteModel(m); } TEST_F(FuseStaticTest, FuseStaticLightReferencedBody) { @@ -671,10 +643,9 @@ TEST_F(FuseStaticTest, FuseStaticLightReferencedBody) { )"; std::array error; - mjModel* m = LoadModelFromString(xml_template, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml_template, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others"; - mj_deleteModel(m); } TEST_F(FuseStaticTest, FuseStaticForceSensorReferencedBody) { @@ -704,10 +675,9 @@ TEST_F(FuseStaticTest, FuseStaticForceSensorReferencedBody) { )"; std::array error; - mjModel* m = LoadModelFromString(xml_template, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml_template, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others"; - mj_deleteModel(m); } TEST_F(FuseStaticTest, FuseStaticCameraInBody) { @@ -727,11 +697,10 @@ TEST_F(FuseStaticTest, FuseStaticCameraInBody) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); EXPECT_EQ(m->nbody, 2) << "Static body should be fused"; EXPECT_EQ(m->ncam, 1); - mj_deleteModel(m); } TEST_F(FuseStaticTest, FuseStaticLightInBody) { @@ -750,11 +719,10 @@ TEST_F(FuseStaticTest, FuseStaticLightInBody) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); EXPECT_EQ(m->nbody, 2) << "Static body should be fused"; EXPECT_EQ(m->nlight, 1); - mj_deleteModel(m); } // ------------- test discardvisual -------------------------------------------- @@ -789,13 +757,12 @@ TEST_F(DiscardVisualTest, DiscardVisualKeepsInertia) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), NotNull()) << error.data(); EXPECT_THAT(model->nmesh, 1); 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[5], model->body_inertia[8]); - mj_deleteModel(model); } TEST_F(DiscardVisualTest, DiscardVisualEquivalent) { @@ -818,8 +785,8 @@ TEST_F(DiscardVisualTest, DiscardVisualEquivalent) { EXPECT_THAT(model1->nq, model2->nq); EXPECT_THAT(model1->nmat, 0); EXPECT_THAT(model1->ntex, 0); - EXPECT_THAT(model2->ngeom-model1->ngeom, 3); - EXPECT_THAT(model2->nmesh-model1->nmesh, 2); + EXPECT_THAT(model2->ngeom - model1->ngeom, 3); + EXPECT_THAT(model2->nmesh - model1->nmesh, 2); EXPECT_THAT(model1->npair, model2->npair); EXPECT_THAT(model1->nsensor, model2->nsensor); EXPECT_THAT(model1->nwrap, model2->nwrap); @@ -851,8 +818,8 @@ TEST_F(DiscardVisualTest, DiscardVisualEquivalent) { EXPECT_STREQ(model1->names + adr1, model2->names + adr2); } - mjData *d1 = mj_makeData(model1); - mjData *d2 = mj_makeData(model2); + mjData* d1 = mj_makeData(model1); + mjData* d2 = mj_makeData(model2); for (int i = 0; i < 100; i++) { mj_step(model1, d1); mj_step(model2, d2); @@ -877,8 +844,7 @@ TEST_F(LengthRangeTest, LengthRangeThreading) { size_t error_sz = 1024; std::string field = ""; - static const char* const kLengthrangePath = - "user/testdata/lengthrange.xml"; + static const char* const kLengthrangePath = "user/testdata/lengthrange.xml"; const std::string xml_path1 = GetTestDataFilePath(kLengthrangePath); mjSpec* spec = mj_parseXML(xml_path1.c_str(), 0, error, error_sz); @@ -1022,20 +988,17 @@ TEST_F(MujocoTest, NestedMeshDir) { sizeof(cube)); std::array error; - mjModel* child_model = LoadModelFromString(child_xml, error.data(), - error.size(), vfs.get()); - EXPECT_THAT(child_model, NotNull()) << error.data(); - mj_deleteModel(child_model); + MjModelPtr child_model = + LoadModelFromString(child_xml, error.data(), error.size(), vfs.get()); + EXPECT_THAT(child_model.get(), NotNull()) << error.data(); - mjModel* parent_model = LoadModelFromString(parent_xml, error.data(), - error.size(), vfs.get()); - EXPECT_THAT(parent_model, NotNull()) << error.data(); - mj_deleteModel(parent_model); + MjModelPtr parent_model = + LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get()); + EXPECT_THAT(parent_model.get(), NotNull()) << error.data(); - mjModel* grandparent_model = LoadModelFromString( + MjModelPtr grandparent_model = LoadModelFromString( grandparent_xml, error.data(), error.size(), vfs.get()); - EXPECT_THAT(grandparent_model, NotNull()) << error.data(); - mj_deleteModel(grandparent_model); + EXPECT_THAT(grandparent_model.get(), NotNull()) << error.data(); mj_deleteVFS(vfs.get()); } @@ -1087,8 +1050,8 @@ TEST_F(DelayBufferTest, ActuatorDelayBufferSizes) { )"; - mjModel* m = LoadModelFromString(xml); - ASSERT_THAT(m, NotNull()); + MjModelPtr m = LoadModelFromString(xml); + ASSERT_THAT(m.get(), NotNull()); ASSERT_EQ(m->nu, 3); // 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[1], 0); EXPECT_EQ(m->actuator_historyadr[2], 8); - - mj_deleteModel(m); } } // namespace diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index b084a2ea..b3641569 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -65,8 +65,8 @@ TEST_F(VfsTest, HFieldPngWithVFS) { mj_defaultVFS(vfs.get()); // should fallback to OS filesystem - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -92,8 +92,8 @@ TEST_F(VfsTest, HFieldCustomWithVFS) { mj_defaultVFS(vfs.get()); // should fallback to OS filesystem - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -120,8 +120,8 @@ TEST_F(VfsTest, TexturePngWithVFS) { mj_defaultVFS(vfs.get()); // should fallback to OS filesystem - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -148,8 +148,8 @@ TEST_F(VfsTest, TextureCustomWithVFS) { mj_defaultVFS(vfs.get()); // should fallback to OS filesystem - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -180,8 +180,8 @@ TEST_F(ContentTypeTest, HFieldPngWithContentType) { mj_defaultVFS(vfs.get()); // should try loading the file - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -208,8 +208,8 @@ TEST_F(ContentTypeTest, HFieldCustomWithContentType) { mj_defaultVFS(vfs.get()); // should try loading the file - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -236,8 +236,8 @@ TEST_F(ContentTypeTest, HFieldWithContentTypeError) { mj_defaultVFS(vfs.get()); // should try loading the file - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("unsupported content type: 'image/jpeg'")); mj_deleteVFS(vfs.get()); } @@ -264,8 +264,8 @@ TEST_F(ContentTypeTest, TexturePngWithContentType) { mj_defaultVFS(vfs.get()); // should try loading the file - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -293,8 +293,8 @@ TEST_F(ContentTypeTest, TextureCustomWithContentType) { mj_defaultVFS(vfs.get()); // should try loading the file - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("Error opening file")); mj_deleteVFS(vfs.get()); } @@ -322,8 +322,8 @@ TEST_F(ContentTypeTest, TextureWithContentTypeError) { mj_defaultVFS(vfs.get()); // should try loading the file - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("unsupported content type: 'image/jpeg'")); mj_deleteVFS(vfs.get()); } @@ -345,15 +345,14 @@ TEST_F(ContentTypeTest, TextureLoadPng) { // tiny RGB 2 x 3 PNG file static constexpr unsigned char tiny[] = { - 0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00, 0x0d, - 0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x02, - 0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d, 0x00, 0x00, 0x00, - 0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63, 0x78, 0xc1, 0xc0, 0xc0, - 0xc0, 0xf0, 0xbf, 0xb8, 0xb8, 0x98, 0x81, 0xe1, 0x3f, 0xc3, 0xff, 0xff, - 0xff, 0xc5, 0xc4, 0xc4, 0x00, 0x46, 0xd7, 0x07, 0x7f, 0xd2, 0x52, 0xa1, - 0x41, 0x00, 0x00, 0x00, 0x00, 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60, - 0x82 - }; + 0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00, + 0x0d, 0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x02, 0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d, + 0x00, 0x00, 0x00, 0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63, + 0x78, 0xc1, 0xc0, 0xc0, 0xc0, 0xf0, 0xbf, 0xb8, 0xb8, 0x98, 0x81, + 0xe1, 0x3f, 0xc3, 0xff, 0xff, 0xff, 0xc5, 0xc4, 0xc4, 0x00, 0x46, + 0xd7, 0x07, 0x7f, 0xd2, 0x52, 0xa1, 0x41, 0x00, 0x00, 0x00, 0x00, + 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60, 0x82}; size_t tiny_sz = sizeof(tiny); @@ -366,10 +365,8 @@ TEST_F(ContentTypeTest, TextureLoadPng) { mj_addBufferVFS(vfs.get(), filename, tiny, tiny_sz); // loading the file should be successful - mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); - EXPECT_THAT(model, NotNull()); - - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get()); + EXPECT_THAT(model.get(), NotNull()); mj_deleteVFS(vfs.get()); } @@ -461,8 +458,8 @@ TEST_F(KeyframeTest, BadSize) { )"; char error[1024]; size_t error_sz = 1024; - mjModel* model = LoadModelFromString(xml, error, error_sz); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error, error_sz); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error, HasSubstr("invalid qpos size, expected 0, got 1")); } @@ -481,9 +478,8 @@ TEST_F(RelativeFrameSensorParsingTest, RefTypeNotRequired) { )"; - mjModel* model = LoadModelFromString(xml, 0, 0); - ASSERT_THAT(model, NotNull()); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, 0, 0); + ASSERT_THAT(model.get(), NotNull()); } TEST_F(RelativeFrameSensorParsingTest, ReferenceBodyFound) { @@ -499,11 +495,10 @@ TEST_F(RelativeFrameSensorParsingTest, ReferenceBodyFound) { )"; - mjModel* model = LoadModelFromString(xml, 0, 0); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, 0, 0); + ASSERT_THAT(model.get(), NotNull()); ASSERT_EQ(model->sensor_reftype[0], mjOBJ_XBODY); ASSERT_EQ(model->sensor_refid[0], 1); - mj_deleteModel(model); } TEST_F(RelativeFrameSensorParsingTest, MissingRefname) { @@ -575,8 +570,8 @@ TEST_F(RelativeFrameSensorParsingTest, BadObjName) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("unrecognized name 'alessio' of sensorized object")); EXPECT_THAT(error.data(), HasSubstr("name 'tom'")); @@ -595,8 +590,8 @@ TEST_F(RelativeFrameSensorParsingTest, BadObjRefName) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("unrecognized name 'alessio' of object")); EXPECT_THAT(error.data(), HasSubstr("name 'tom'")); EXPECT_THAT(error.data(), HasSubstr("line 7")); @@ -614,15 +609,14 @@ TEST_F(SensorTest, OjbtypeParsedButNotRequired) { )"; - mjModel* model = LoadModelFromString(xml, 0, 0); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, 0, 0); + ASSERT_THAT(model.get(), NotNull()); EXPECT_EQ(model->sensor_datatype[0], mjDATATYPE_AXIS); EXPECT_EQ(model->sensor_objtype[0], mjOBJ_UNKNOWN); EXPECT_EQ(model->sensor_dim[0], 3); EXPECT_EQ(model->sensor_datatype[1], mjDATATYPE_REAL); EXPECT_EQ(model->sensor_objtype[1], mjOBJ_BODY); EXPECT_EQ(model->sensor_objid[1], 0); - mj_deleteModel(model); } TEST_F(SensorTest, NegativeIntervalError) { @@ -640,8 +634,8 @@ TEST_F(SensorTest, NegativeIntervalError) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("negative interval")); } @@ -660,8 +654,8 @@ TEST_F(SensorTest, PositivePhaseError) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("positive phase")); } @@ -680,8 +674,8 @@ TEST_F(SensorTest, DelayWithoutHistoryError) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("delay > 0 without a history buffer")); } @@ -692,8 +686,8 @@ static const char* const kCapsuleInertiaPath = using MjCGeomTest = MujocoTest; -static constexpr int kSphereBodyId = 1, kCylinderBodyId = 2, - kCapsuleBodyId = 3, kCapsuleGeomId = 2; +static constexpr int kSphereBodyId = 1, kCylinderBodyId = 2, kCapsuleBodyId = 3, + kCapsuleGeomId = 2; TEST_F(MjCGeomTest, CapsuleMass) { const string xml_path = GetTestDataFilePath(kCapsuleInertiaPath); @@ -750,7 +744,8 @@ TEST_F(MjCGeomTest, CapsuleInertiaX) { TEST_F(MjCGeomTest, ShellInertiaSphere) { if constexpr (sizeof(mjtNum) == sizeof(float)) { - GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision"; + GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which " + "vanish in float32 precision"; } static constexpr char xml[] = R"( @@ -772,8 +767,8 @@ TEST_F(MjCGeomTest, ShellInertiaSphere) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); // radius mjtNum r = 1.5; @@ -806,13 +801,12 @@ TEST_F(MjCGeomTest, ShellInertiaSphere) { EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], kInertiaTol); EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], kInertiaTol); EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], kInertiaTol); - - mj_deleteModel(m); } TEST_F(MjCGeomTest, ShellInertiaCapsule) { if constexpr (sizeof(mjtNum) == sizeof(float)) { - GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision"; + GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which " + "vanish in float32 precision"; } static constexpr char xml[] = R"( @@ -834,8 +828,8 @@ TEST_F(MjCGeomTest, ShellInertiaCapsule) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); // dimensions mjtNum r = 0.1; @@ -890,13 +884,12 @@ TEST_F(MjCGeomTest, ShellInertiaCapsule) { EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], kInertiaTol); EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], kInertiaTol); EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], kInertiaTol); - - mj_deleteModel(m); } TEST_F(MjCGeomTest, ShellInertiaCylinder) { if constexpr (sizeof(mjtNum) == sizeof(float)) { - GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision"; + GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which " + "vanish in float32 precision"; } static constexpr char xml[] = R"( @@ -918,8 +911,8 @@ TEST_F(MjCGeomTest, ShellInertiaCylinder) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); // dimensions mjtNum r = 0.1; @@ -968,13 +961,12 @@ TEST_F(MjCGeomTest, ShellInertiaCylinder) { EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], kInertiaTol); EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], kInertiaTol); EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], kInertiaTol); - - mj_deleteModel(m); } TEST_F(MjCGeomTest, ShellInertiaEllipsoid) { if constexpr (sizeof(mjtNum) == sizeof(float)) { - GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision"; + GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which " + "vanish in float32 precision"; } // test special case of ellipsoid with dimensions: a = b = c // TODO(taylorhowell): add test for ellipsoid with dimensions: a != b != c @@ -998,8 +990,8 @@ TEST_F(MjCGeomTest, ShellInertiaEllipsoid) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); // dimensions mjtNum r = 0.1; @@ -1034,13 +1026,12 @@ TEST_F(MjCGeomTest, ShellInertiaEllipsoid) { EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], 10 * kInertiaTol); EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], 10 * kInertiaTol); EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], 10 * kInertiaTol); - - mj_deleteModel(m); } TEST_F(MjCGeomTest, ShellInertiaBox) { if constexpr (sizeof(mjtNum) == sizeof(float)) { - GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision"; + GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which " + "vanish in float32 precision"; } static constexpr char xml[] = R"( @@ -1062,8 +1053,8 @@ TEST_F(MjCGeomTest, ShellInertiaBox) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); // dimensions mjtNum dx = 0.1; @@ -1133,8 +1124,6 @@ TEST_F(MjCGeomTest, ShellInertiaBox) { EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], kInertiaTol); EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], kInertiaTol); EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], kInertiaTol); - - mj_deleteModel(m); } // ------------- test inertiagrouprange ---------------------------------------- @@ -1150,9 +1139,8 @@ TEST_F(MjCGeomTest, IgnoreGeomOutsideInertiagrouprange) { )"; - mjModel* m = LoadModelFromString(xml, nullptr, 0); + MjModelPtr m = LoadModelFromString(xml, nullptr, 0); EXPECT_THAT(m->body_mass[1], 0); - mj_deleteModel(m); } TEST_F(MjCGeomTest, IgnoreBadGeomOutsideInertiagrouprange) { @@ -1171,9 +1159,8 @@ TEST_F(MjCGeomTest, IgnoreBadGeomOutsideInertiagrouprange) { )"; - mjModel* m = LoadModelFromString(xml, nullptr, 0); + MjModelPtr m = LoadModelFromString(xml, nullptr, 0); EXPECT_THAT(m->body_mass[1], 0); - mj_deleteModel(m); } // ------------- test invalid size values -------------------------------------- @@ -1192,8 +1179,8 @@ TEST_F(MjCGeomTest, NanSize) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, testing::IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), testing::IsNull()); ASSERT_THAT(error.data(), HasSubstr("nan")); EXPECT_THAT(error.data(), HasSubstr("line 5")); } @@ -1217,11 +1204,10 @@ TEST_F(MjCGeomTest, BadMeshZeroMassDensityDoesntError) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->body_mass[1], 0); EXPECT_EQ(model->body_mass[2], 0); - mj_deleteModel(model); } // ------------- test joints -------------------------------------------------- @@ -1235,8 +1221,8 @@ TEST_F(MjCJointTest, AlignFree) { mjSpec* s = mj_parseXML(xml_path.c_str(), nullptr, err.data(), err.size()); ASSERT_THAT(s, NotNull()) << err.data(); s->compiler.alignfree = 1; // auto-aligned free joint - mjModel* m = mj_compile(s, nullptr); - ASSERT_THAT(m, NotNull()); + MjModelPtr m(mj_compile(s, nullptr)); + ASSERT_THAT(m.get(), NotNull()); // with alignfree the body has sameframe and simple and all dof are simple EXPECT_EQ(m->body_sameframe[1], 1); @@ -1245,20 +1231,20 @@ TEST_F(MjCJointTest, AlignFree) { // make unaligned model s->compiler.alignfree = 0; // unaligned free joint - mjModel* m_u = mj_compile(s, nullptr); - ASSERT_THAT(m_u, NotNull()); + MjModelPtr m_u(mj_compile(s, nullptr)); + ASSERT_THAT(m_u.get(), NotNull()); // no sameframe or simple EXPECT_EQ(m_u->body_sameframe[1], 0); EXPECT_EQ(m_u->body_simple[1], 0); // make datas for both models - mjData* d = mj_makeData(m); - mjData* d_u = mj_makeData(m_u); + MjDataPtr d = MakeData(m); + MjDataPtr d_u = MakeData(m_u); // call mj_forward - mj_forward(m, d); - mj_forward(m_u, d_u); + mj_forward(m.get(), d.get()); + mj_forward(m_u.get(), d_u.get()); // expect x-frames (sensors) to match to very high precision double eps = MjTol(1e-10, 1e-6); @@ -1276,17 +1262,17 @@ TEST_F(MjCJointTest, AlignFree) { m->opt.timestep = m_u->opt.timestep = 1e-4; m->opt.integrator = m_u->opt.integrator = mjINT_RK4; while (d->time < 1) { - mj_step(m, d); - mj_step(m_u, d_u); + mj_step(m.get(), d.get()); + mj_step(m_u.get(), d_u.get()); } // expect qpos to be significantly different, since the semantics are changed - mj_markStack(d); + mj_markStack(d.get()); int nq = m->nq; - mjtNum* dqpos = mj_stackAllocNum(d, nq); + mjtNum* dqpos = mj_stackAllocNum(d.get(), nq); mju_sub(dqpos, d->qpos, d_u->qpos, nq); EXPECT_GT(mju_norm(dqpos, nq), 1.0); - mj_freeStack(d); + mj_freeStack(d.get()); // expect x-frames to match to reasonable precision eps = MjTol(1e-5, 1e-2); @@ -1298,14 +1284,9 @@ TEST_F(MjCJointTest, AlignFree) { EXPECT_THAT(AsVector(d->light_xdir, 3), Pointwise(MjNear(eps, eps), AsVector(d_u->light_xdir, 3))); - mj_deleteData(d_u); - mj_deleteData(d); - mj_deleteModel(m_u); - mj_deleteModel(m); mj_deleteSpec(s); } - // ------------- test height fields -------------------------------------------- using MjCHFieldTest = MujocoTest; @@ -1347,10 +1328,8 @@ TEST_F(MjCTextureTest, TexturesLoad) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << error.data(); - - mj_deleteModel(m); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), NotNull()) << error.data(); } // ------------- test quaternion normalization---------------------------------- @@ -1370,12 +1349,15 @@ TEST_F(QuatNorm, QuatNotNormalized) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(AsVector(m->body_quat+4, 4), ElementsAre(1./5, 2./5, 2./5, 4./5)); - EXPECT_THAT(AsVector(m->geom_quat, 4), ElementsAre(1./5, 2./5, 2./5, 4./5)); - EXPECT_THAT(AsVector(m->site_quat, 4), ElementsAre(1./5, 2./5, 2./5, 4./5)); - EXPECT_THAT(AsVector(m->cam_quat, 4), ElementsAre(1./5, 2./5, 2./5, 4./5)); - mj_deleteModel(m); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(AsVector(m->body_quat + 4, 4), + ElementsAre(1. / 5, 2. / 5, 2. / 5, 4. / 5)); + EXPECT_THAT(AsVector(m->geom_quat, 4), + ElementsAre(1. / 5, 2. / 5, 2. / 5, 4. / 5)); + EXPECT_THAT(AsVector(m->site_quat, 4), + ElementsAre(1. / 5, 2. / 5, 2. / 5, 4. / 5)); + EXPECT_THAT(AsVector(m->cam_quat, 4), + ElementsAre(1. / 5, 2. / 5, 2. / 5, 4. / 5)); } // ------------- test camera specifications ------------------------------------ @@ -1394,11 +1376,10 @@ TEST_F(CameraSpecTest, FovyLimits) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("fovy too large")); EXPECT_THAT(error.data(), HasSubstr("line 6")); - mj_deleteModel(m); } TEST_F(CameraSpecTest, DuplicatedFocalIgnorePixel) { @@ -1417,11 +1398,10 @@ TEST_F(CameraSpecTest, DuplicatedFocalIgnorePixel) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), NotNull()) << error.data(); EXPECT_NEAR(m->cam_intrinsic[0], 5e-4, 1e-6); // focal length in meters (x) EXPECT_NEAR(m->cam_intrinsic[1], 5e-4, 1e-6); // focal length in meters (y) - mj_deleteModel(m); } TEST_F(CameraSpecTest, FovyFromResolution) { @@ -1440,14 +1420,13 @@ TEST_F(CameraSpecTest, FovyFromResolution) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), NotNull()) << error.data(); EXPECT_NEAR(m->cam_fovy[0], 41.112, 1e-3); EXPECT_NEAR(m->cam_intrinsic[0], 8e-3, 1e-6); // focal length in meters (x) EXPECT_NEAR(m->cam_intrinsic[1], 8e-3, 1e-6); // focal length in meters (y) EXPECT_EQ(m->cam_intrinsic[2], 0); // principal point in meters (x) EXPECT_EQ(m->cam_intrinsic[3], 0); // principal point in meters (y) - mj_deleteModel(m); } TEST_F(CameraSpecTest, FovyFromResolutionPixel) { @@ -1466,14 +1445,13 @@ TEST_F(CameraSpecTest, FovyFromResolutionPixel) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), NotNull()) << error.data(); EXPECT_NEAR(m->cam_fovy[0], 41.112, 1e-3); EXPECT_NEAR(m->cam_intrinsic[0], 8e-3, 1e-6); // focal length in meters (x) EXPECT_NEAR(m->cam_intrinsic[1], 8e-3, 1e-6); // focal length in meters (y) EXPECT_EQ(m->cam_intrinsic[2], 0); // principal point in meters (x) EXPECT_EQ(m->cam_intrinsic[3], 0); // principal point in meters (y) - mj_deleteModel(m); } TEST_F(CameraSpecTest, ParentTargetingNull) { @@ -1486,18 +1464,14 @@ TEST_F(CameraSpecTest, ParentTargetingNull) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()) << error.data(); - mjData* data = mj_makeData(model); - mj_forward(model, data); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), NotNull()) << error.data(); + MjDataPtr data = MakeData(model); + mj_forward(model.get(), data.get()); // orientation can't be decided so we get an arbitrary but valid xmat // (camera pointed towards the negative x-axis) EXPECT_THAT(AsVector(data->cam_xmat, 9), - ElementsAre(0, 0, 1, - 1, 0, 0, - 0, 1, 0)); - mj_deleteData(data); - mj_deleteModel(model); + ElementsAre(0, 0, 1, 1, 0, 0, 0, 1, 0)); } TEST_F(CameraSpecTest, ParentTargeting) { @@ -1510,16 +1484,14 @@ TEST_F(CameraSpecTest, ParentTargeting) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()) << error.data(); - mjData* data = mj_makeData(model); - mj_forward(model, data); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), NotNull()) << error.data(); + MjDataPtr data = MakeData(model); + mj_forward(model.get(), data.get()); // expect negative z-axis to point from camera to world EXPECT_FLOAT_EQ(data->cam_xmat[2], mju_sqrt(0.5)); EXPECT_FLOAT_EQ(data->cam_xmat[5], mju_sqrt(0.5)); EXPECT_FLOAT_EQ(data->cam_xmat[8], 0); - mj_deleteData(data); - mj_deleteModel(model); } // ------------- test actuator order ------------------------------------------- @@ -1555,10 +1527,10 @@ TEST_F(ActuatorTest, ActuatorOrderDoesntMatter) { )"; - mjModel* model1 = LoadModelFromString(xml1, nullptr, 0); - mjData* data1 = mj_makeData(model1); - mjModel* model2 = LoadModelFromString(xml2, nullptr, 0); - mjData* data2 = mj_makeData(model2); + MjModelPtr model1 = LoadModelFromString(xml1, nullptr, 0); + MjDataPtr data1 = MakeData(model1); + MjModelPtr model2 = LoadModelFromString(xml2, nullptr, 0); + MjDataPtr data2 = MakeData(model2); // check activation indexing EXPECT_EQ(model1->actuator_actadr[0], 0); @@ -1570,23 +1542,18 @@ TEST_F(ActuatorTest, ActuatorOrderDoesntMatter) { while (data1->time < 1) { data1->ctrl[0] = data1->time; data1->ctrl[1] = -data1->time; - mj_step(model1, data1); + mj_step(model1.get(), data1.get()); } while (data2->time < 1) { data2->ctrl[0] = -data2->time; data2->ctrl[1] = data2->time; - mj_step(model2, data2); + mj_step(model2.get(), data2.get()); } // expect states to match exactly EXPECT_EQ(data1->qpos[0], data2->qpos[0]); EXPECT_EQ(data1->qvel[0], data2->qvel[0]); EXPECT_EQ(data1->act[0], data2->act[0]); - - mj_deleteData(data2); - mj_deleteModel(model2); - mj_deleteData(data1); - mj_deleteModel(model1); } // ------------- test inheritrange attribute ---------------------------------- @@ -1609,8 +1576,8 @@ TEST_F(InheritrangeTest, ErrorIfTargetMissingRange) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("target 'jnt' has no range defined")); } @@ -1630,15 +1597,12 @@ TEST_F(InheritrangeTest, WorksForDegrees) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_MJTNUM_EQ(model->actuator_ctrlrange[0], mjPI / 2); EXPECT_MJTNUM_EQ(model->actuator_ctrlrange[1], mjPI); - - mj_deleteModel(model); } - // ------------- test actlimited and actrange fields --------------------------- using ActRangeTest = MujocoTest; @@ -1657,12 +1621,10 @@ TEST_F(ActRangeTest, ActRangeParsed) { )"; - mjModel* m = LoadModelFromString(xml, nullptr, 0); + MjModelPtr m = LoadModelFromString(xml, nullptr, 0); EXPECT_EQ(m->actuator_actlimited[0], 1); EXPECT_EQ(m->actuator_actrange[0], -1); EXPECT_EQ(m->actuator_actrange[1], 1.5); - - mj_deleteModel(m); } TEST_F(ActRangeTest, ActRangeBad) { @@ -1680,8 +1642,8 @@ TEST_F(ActRangeTest, ActRangeBad) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("invalid actrange")); EXPECT_THAT(error.data(), HasSubstr("line 10")); } @@ -1701,8 +1663,8 @@ TEST_F(ActRangeTest, ActRangeUndefined) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("invalid actrange")); EXPECT_THAT(error.data(), HasSubstr("line 10")); } @@ -1722,8 +1684,8 @@ TEST_F(ActRangeTest, ActRangeNoDyntype) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("actrange specified but dyntype is 'none'")); } @@ -1751,7 +1713,7 @@ TEST_F(ActRangeTest, ActRangeDefaultsPropagate) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); // first actuator EXPECT_THAT(model->actuator_actlimited[0], 1); @@ -1762,8 +1724,6 @@ TEST_F(ActRangeTest, ActRangeDefaultsPropagate) { EXPECT_THAT(model->actuator_actlimited[1], 0); EXPECT_THAT(model->actuator_actrange[2], 2); EXPECT_THAT(model->actuator_actrange[3], 3); - - mj_deleteModel(model); } // ---------------------------- test actdim ------------------------------------ @@ -1785,9 +1745,9 @@ TEST_F(ActDimTest, BiggerThanOneOnlyForUser) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("actdim > 1 is only allowed for dyntype 'user'")); } @@ -1807,9 +1767,9 @@ TEST_F(ActDimTest, NonzeroNotAllowedInStateless) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("invalid actdim 1 in stateless")); EXPECT_THAT(error.data(), HasSubstr("line 10")); } @@ -1829,9 +1789,9 @@ TEST_F(ActDimTest, ZeroNotAllowedInStateful) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("invalid actdim 0 in stateful")); EXPECT_THAT(error.data(), HasSubstr("line 10")); } @@ -1850,8 +1810,8 @@ TEST_F(UserDataTest, NBodyTooSmall) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_body")); EXPECT_THAT(error.data(), HasSubstr("line 5")); } @@ -1869,8 +1829,8 @@ TEST_F(UserDataTest, NJointTooSmall) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_jnt")); EXPECT_THAT(error.data(), HasSubstr("line 7")); } @@ -1885,8 +1845,8 @@ TEST_F(UserDataTest, NGeomTooSmall) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_geom")); EXPECT_THAT(error.data(), HasSubstr("line 5")); } @@ -1901,8 +1861,8 @@ TEST_F(UserDataTest, NSiteTooSmall) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_site")); EXPECT_THAT(error.data(), HasSubstr("line 5")); } @@ -1917,8 +1877,8 @@ TEST_F(UserDataTest, NCameraTooSmall) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_cam")); EXPECT_THAT(error.data(), HasSubstr("line 5")); } @@ -1940,8 +1900,8 @@ TEST_F(UserDataTest, NTendonTooSmall) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_tendon")); EXPECT_THAT(error.data(), HasSubstr("line 9")); } @@ -1962,8 +1922,8 @@ TEST_F(UserDataTest, NActuatorTooSmall) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_actuator")); EXPECT_THAT(error.data(), HasSubstr("line 11")); } @@ -1981,8 +1941,8 @@ TEST_F(UserDataTest, NSensorTooSmall) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_sensor")); EXPECT_THAT(error.data(), HasSubstr("line 8")); } @@ -2024,8 +1984,8 @@ TEST_F(LimitedTest, ErrorIfLimitedMissingOnJoint) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("limited")); EXPECT_THAT(error.data(), HasSubstr("line 6")); } @@ -2044,11 +2004,10 @@ TEST_F(LimitedTest, ExplicitLimitedFalseIsOk) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->jnt_limited[0], 0); - mj_deleteModel(model); } TEST_F(LimitedTest, ErrorIfLimitedMissingOnTendon) { @@ -2073,8 +2032,8 @@ TEST_F(LimitedTest, ErrorIfLimitedMissingOnTendon) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("limited")); EXPECT_THAT(error.data(), HasSubstr("tendon")); EXPECT_THAT(error.data(), HasSubstr("line 13")); @@ -2097,8 +2056,8 @@ TEST_F(LimitedTest, ErrorIfForceLimitedMissingOnActuator) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("forcelimited")); EXPECT_THAT(error.data(), HasSubstr("forcerange")); EXPECT_THAT(error.data(), HasSubstr("actuator")); @@ -2127,8 +2086,8 @@ TEST_F(TendonTest, SiteBetweenPulleyNotAllowed) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("needs a neighbor that is not a pulley")); EXPECT_THAT(error.data(), HasSubstr("line 9")); } @@ -2159,22 +2118,22 @@ TEST_F(TendonTest, ActuatorForceRangeNotAllowed) { std::string xml0 = xml; std::string range0 = "-2 -1"; xml0.replace(rng_ind, str_replace.length(), range0); - mjModel* m0 = LoadModelFromString(xml0.c_str(), error.data(), error.size()); - EXPECT_THAT(m0, IsNull()); + MjModelPtr m0 = LoadModelFromString(xml0.c_str(), error.data(), error.size()); + EXPECT_THAT(m0.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("invalid actuatorfrcrange in tendon")); std::string xml1 = xml; std::string range1 = "1 2"; xml1.replace(rng_ind, str_replace.length(), range1); - mjModel* m1 = LoadModelFromString(xml1.c_str(), error.data(), error.size()); - EXPECT_THAT(m1, IsNull()); + MjModelPtr m1 = LoadModelFromString(xml1.c_str(), error.data(), error.size()); + EXPECT_THAT(m1.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("invalid actuatorfrcrange in tendon")); std::string xml2 = xml; std::string range2 = "1 0"; xml2.replace(rng_ind, str_replace.length(), range2); - mjModel* m2 = LoadModelFromString(xml2.c_str(), error.data(), error.size()); - EXPECT_THAT(m2, IsNull()); + MjModelPtr m2 = LoadModelFromString(xml2.c_str(), error.data(), error.size()); + EXPECT_THAT(m2.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("invalid actuatorfrcrange in tendon")); } @@ -2203,11 +2162,10 @@ TEST_F(SpringrangeTest, DefaultsPropagate) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_MJTNUM_EQ(model->tendon_lengthspring[0], .2); EXPECT_MJTNUM_EQ(model->tendon_lengthspring[1], .5); - mj_deleteModel(model); } TEST_F(SpringrangeTest, InvalidRange) { @@ -2227,8 +2185,8 @@ TEST_F(SpringrangeTest, InvalidRange) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("invalid springlength in tendon")); EXPECT_THAT(error.data(), HasSubstr("line 9")); } @@ -2278,8 +2236,8 @@ TEST_F(UserObjectsTest, Frame) { )"; const mjtNum eps = MjTol(1e-14, 1e-5); std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, testing::NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), testing::NotNull()) << error.data(); EXPECT_EQ(m->nbody, 5); // geom quat transformed to euler = 0 0 50 @@ -2295,7 +2253,7 @@ TEST_F(UserObjectsTest, Frame) { EXPECT_NEAR(m->geom_quat[7], .5, eps); // geom pos transformed from 0 1 0 to 0 2 0 - EXPECT_EQ(m->geom_pos[ 9], 0); + EXPECT_EQ(m->geom_pos[9], 0); EXPECT_EQ(m->geom_pos[10], 2); EXPECT_EQ(m->geom_pos[11], 0); @@ -2310,12 +2268,12 @@ TEST_F(UserObjectsTest, Frame) { EXPECT_EQ(m->geom_pos[14], 1); // joint axis transformed to 0 -1 0 - EXPECT_NEAR(m->jnt_axis[0], 0, eps); + EXPECT_NEAR(m->jnt_axis[0], 0, eps); EXPECT_NEAR(m->jnt_axis[1], -1, eps); - EXPECT_NEAR(m->jnt_axis[2], 0, eps); + EXPECT_NEAR(m->jnt_axis[2], 0, eps); - mjData* d = mj_makeData(m); - mj_kinematics(m, d); + MjDataPtr d = MakeData(m); + mj_kinematics(m.get(), d.get()); // body and frame equivalence geom 2 vs 6 EXPECT_NEAR(d->geom_xpos[6], d->geom_xpos[18], eps); @@ -2325,9 +2283,6 @@ TEST_F(UserObjectsTest, Frame) { EXPECT_NEAR(d->geom_xmat[19], d->geom_xmat[55], eps); EXPECT_NEAR(d->geom_xmat[20], d->geom_xmat[56], eps); EXPECT_NEAR(d->geom_xmat[21], d->geom_xmat[57], eps); - - mj_deleteModel(m); - mj_deleteData(d); } TEST_F(UserObjectsTest, FrameTransformsLight) { @@ -2341,8 +2296,8 @@ TEST_F(UserObjectsTest, FrameTransformsLight) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), NotNull()) << error.data(); EXPECT_EQ(m->nlight, 1); const mjtNum eps = MjTol(1e-14, 1e-5); @@ -2353,8 +2308,6 @@ TEST_F(UserObjectsTest, FrameTransformsLight) { EXPECT_NEAR(m->light_dir[0], 0, eps); EXPECT_NEAR(m->light_dir[1], 0, eps); EXPECT_NEAR(m->light_dir[2], -1, eps); - - mj_deleteModel(m); } TEST_F(ContentTypeTest, ImageLightsReferenceTexture) { @@ -2372,15 +2325,13 @@ TEST_F(ContentTypeTest, ImageLightsReferenceTexture) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, NotNull()); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), NotNull()); EXPECT_EQ(m->ntex, 1); EXPECT_EQ(m->nlight, 1); EXPECT_THAT(m->light_texid[0], 0); - mj_deleteModel(m); } - // ------------- test bvh ------------------------------------------------------ TEST_F(UserObjectsTest, RobustBVH) { static constexpr char xml1[] = R"( @@ -2414,19 +2365,16 @@ TEST_F(UserObjectsTest, RobustBVH) { )"; std::array error; - mjModel* m1 = LoadModelFromString(xml1, error.data(), error.size()); - EXPECT_THAT(m1, NotNull()) << error.data(); + MjModelPtr m1 = LoadModelFromString(xml1, error.data(), error.size()); + EXPECT_THAT(m1.get(), NotNull()) << error.data(); - mjModel* m2 = LoadModelFromString(xml2, error.data(), error.size()); - EXPECT_THAT(m2, NotNull()) << error.data(); + MjModelPtr m2 = LoadModelFromString(xml2, error.data(), error.size()); + EXPECT_THAT(m2.get(), NotNull()) << error.data(); EXPECT_EQ(m1->nbvh, m2->nbvh); for (int i = 0; i < m1->nbvh; i++) { EXPECT_EQ(m1->bvh_nodeid[i], m2->bvh_nodeid[i]); } - - mj_deleteModel(m1); - mj_deleteModel(m2); } // ------------- test equality compilation ------------------------------------- @@ -2453,17 +2401,15 @@ TEST_F(UserObjectsTest, BadConnect) { // good model using body semantic string xml = base.replace(pos, len, ""); char error[1024]; - mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 1, 0, 0, 2)); - mj_deleteModel(m); // good model using site semantic xml = base.replace(pos, len, ""); m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + ASSERT_THAT(m.get(), NotNull()) << error; EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 0, 0, 0, 0)); - mj_deleteModel(m); char error_missing[] = "either both body1 and anchor must be defined," @@ -2472,7 +2418,7 @@ TEST_F(UserObjectsTest, BadConnect) { // bad model (missing anchor) xml = base.replace(pos, len, ""); m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(m, IsNull()); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(error, HasSubstr(error_missing)); char error_mixed[] = @@ -2482,7 +2428,7 @@ TEST_F(UserObjectsTest, BadConnect) { // bad model (mixing body and site) xml = base.replace(pos, len, ""); m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(m, IsNull()); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(error, HasSubstr(error_mixed)); // load spec with no constraints @@ -2497,16 +2443,15 @@ TEST_F(UserObjectsTest, BadConnect) { mjs_setString(equality->name2, "1"); // expect compilation to fail - m = mj_compile(s, nullptr); - ASSERT_THAT(m, IsNull()); + m.reset(mj_compile(s, nullptr)); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(mjs_getError(s), HasSubstr("connect constraint supports only sites and bodies")); // set objtype, expect compilation to succeed equality->objtype = mjOBJ_SITE; - m = mj_compile(s, nullptr); - ASSERT_THAT(m, NotNull()) << mjs_getError(s); - mj_deleteModel(m); + m.reset(mj_compile(s, nullptr)); + ASSERT_THAT(m.get(), NotNull()) << mjs_getError(s); mj_deleteSpec(s); } @@ -2533,19 +2478,17 @@ TEST_F(UserObjectsTest, BadWeld) { string xml = base.replace(pos, len, ""); char error[1024]; - mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; EXPECT_THAT(AsVector(m->eq_data, 10), ElementsAre(0, 0, 1, 0, 0, 0, 1, 0, 0, 0)); - mj_deleteModel(m); // good model using site semantic xml = base.replace(pos, len, ""); m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + ASSERT_THAT(m.get(), NotNull()) << error; EXPECT_THAT(AsVector(m->eq_data, 10), ElementsAre(0, 1, 0, 0, 0, 0, 0, 0, 0, 0)); - mj_deleteModel(m); char error_mixed[] = "body and site semantics cannot be mixed" @@ -2554,25 +2497,25 @@ TEST_F(UserObjectsTest, BadWeld) { // bad model (mixing body and site) xml = base.replace(pos, len, ""); m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(m, IsNull()); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(error, HasSubstr(error_mixed)); // bad model (mixing site and anchor) xml = base.replace(pos, len, ""); m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(m, IsNull()); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(error, HasSubstr(error_mixed)); // bad model (mixing site and relpose) xml = base.replace(pos, len, ""); m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(m, IsNull()); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(error, HasSubstr(error_mixed)); // bad model (body and site semantics are valid, but both are specified) xml = base.replace(pos, len, ""); m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(m, IsNull()); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(error, HasSubstr(error_mixed)); char error_underspecified[] = @@ -2582,13 +2525,13 @@ TEST_F(UserObjectsTest, BadWeld) { // bad model (underspecified body semantics) xml = base.replace(pos, len, ""); m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(m, IsNull()); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(error, HasSubstr(error_underspecified)); // bad model (underspecified site semantics) xml = base.replace(pos, len, ""); m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(m, IsNull()); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(error, HasSubstr(error_underspecified)); // load spec with no constraints @@ -2603,16 +2546,15 @@ TEST_F(UserObjectsTest, BadWeld) { mjs_setString(equality->name2, "1"); // expect compilation to fail - m = mj_compile(s, nullptr); - ASSERT_THAT(m, IsNull()); + m.reset(mj_compile(s, nullptr)); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(mjs_getError(s), HasSubstr("weld constraint supports only sites and bodies")); // set objtype, expect compilation to succeed equality->objtype = mjOBJ_SITE; - m = mj_compile(s, nullptr); - ASSERT_THAT(m, NotNull()) << mjs_getError(s); - mj_deleteModel(m); + m.reset(mj_compile(s, nullptr)); + ASSERT_THAT(m.get(), NotNull()) << mjs_getError(s); mj_deleteSpec(s); } @@ -2632,8 +2574,8 @@ TEST_F(UserObjectsTest, Inertial) { )"; char error[1024]; - mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; + MjModelPtr m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(m.get(), NotNull()) << error; EXPECT_EQ(m->body_mass[1], 1); EXPECT_THAT(AsVector(m->body_ipos + 3, 3), ElementsAre(2, 3, 4)); EXPECT_THAT(AsVector(m->body_inertia + 3, 3), ElementsAre(4, 5, 6)); @@ -2647,7 +2589,6 @@ TEST_F(UserObjectsTest, Inertial) { EXPECT_EQ(m->body_mass[2], 2); EXPECT_THAT(AsVector(m->body_ipos + 6, 3), ElementsAre(1, 2, 3)); EXPECT_THAT(AsVector(m->body_inertia + 6, 3), ElementsAre(4, 3, 2)); - mj_deleteModel(m); string bad_xml1 = R"( @@ -2659,7 +2600,7 @@ TEST_F(UserObjectsTest, Inertial) { )"; m = LoadModelFromString(bad_xml1.c_str(), error, sizeof(error)); - ASSERT_THAT(m, IsNull()); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(error, HasSubstr("fullinertia and diagonal inertia cannot both")); string bad_xml2 = R"( @@ -2672,7 +2613,7 @@ TEST_F(UserObjectsTest, Inertial) { )"; m = LoadModelFromString(bad_xml2.c_str(), error, sizeof(error)); - ASSERT_THAT(m, IsNull()); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(error, HasSubstr("fullinertia and inertial orientation cannot")); } @@ -2691,12 +2632,10 @@ TEST_F(UserObjectsTest, ZeroMass) { )"; char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - EXPECT_THAT(model, NotNull()) << error; - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + EXPECT_THAT(model.get(), NotNull()) << error; } - // ------------- test Octree SDF computation ----------------------------------- using OctreeSDFTest = MujocoTest; @@ -2714,9 +2653,9 @@ TEST_F(OctreeSDFTest, SphereSDF) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); EXPECT_GT(model->nmesh, 0); @@ -2733,7 +2672,7 @@ TEST_F(OctreeSDFTest, SphereSDF) { // Analytic SDF for unit sphere: distance = |p| - 1 auto analyticSdf = [](const mjtNum* p) -> double { - return mju_sqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2]) - 1.0; + return mju_sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]) - 1.0; }; int sign_errors = 0; @@ -2745,7 +2684,7 @@ TEST_F(OctreeSDFTest, SphereSDF) { for (double y = -2.0; y <= 2.0; y += 0.5) { for (double z = -2.0; z <= 2.0; z += 0.5) { mjtNum p[3] = {(mjtNum)x, (mjtNum)y, (mjtNum)z}; - double sdf_dist = mjc_distance(model, data, &sdf, p); + double sdf_dist = mjc_distance(model.get(), data.get(), &sdf, p); double gt_dist = analyticSdf(p); if ((sdf_dist < 0) != (gt_dist < 0)) { @@ -2764,9 +2703,6 @@ TEST_F(OctreeSDFTest, SphereSDF) { EXPECT_LT(sign_errors, total_points / 200) << "No more than 0.5% of points should have sign errors"; EXPECT_LT(rmse, 0.11) << "RMSE should be less than 0.11"; - - mj_deleteData(data); - mj_deleteModel(model); } TEST_F(OctreeSDFTest, TorusSDF) { @@ -2782,9 +2718,9 @@ TEST_F(OctreeSDFTest, TorusSDF) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); + MjDataPtr data = MakeData(model); ASSERT_THAT(data, NotNull()); EXPECT_GT(model->nmesh, 0); @@ -2803,9 +2739,9 @@ TEST_F(OctreeSDFTest, TorusSDF) { // projection of p onto circle of radius R, and r is minor radius. // R=1, r=0.3 auto analyticSdf = [](const mjtNum* p) -> double { - double xy = mju_sqrt(p[0]*p[0] + p[1]*p[1]); + double xy = mju_sqrt(p[0] * p[0] + p[1] * p[1]); double vec[2] = {xy - 1.0, p[2]}; - return mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1]) - 0.3; + return mju_sqrt(vec[0] * vec[0] + vec[1] * vec[1]) - 0.3; }; int sign_errors = 0; @@ -2817,7 +2753,7 @@ TEST_F(OctreeSDFTest, TorusSDF) { for (mjtNum y = -2.0; y <= 2.0; y += 0.5) { for (mjtNum z = -2.0; z <= 2.0; z += 0.5) { mjtNum p[3] = {x, y, z}; - double sdf_dist = mjc_distance(model, data, &sdf, p); + double sdf_dist = mjc_distance(model.get(), data.get(), &sdf, p); double gt_dist = analyticSdf(p); if ((sdf_dist < 0) != (gt_dist < 0)) { @@ -2836,9 +2772,6 @@ TEST_F(OctreeSDFTest, TorusSDF) { EXPECT_LT(sign_errors, total_points / 20) << "No more than 5% of points should have sign errors"; EXPECT_LT(rmse, 0.52) << "RMSE should be close to 0.516"; - - mj_deleteData(data); - mj_deleteModel(model); } // ------------- test actuator damping/armature validation --------------------- @@ -2861,8 +2794,8 @@ TEST_F(ActuatorTransmissionTest, DampingRejectsSiteTransmission) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("damping requires joint or tendon")); } @@ -2882,8 +2815,8 @@ TEST_F(ActuatorTransmissionTest, ArmatureRejectsSiteTransmission) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("armature requires joint or tendon")); } diff --git a/test/xml/xml_api_test.cc b/test/xml/xml_api_test.cc index 4207dd16..b4da2f7e 100644 --- a/test/xml/xml_api_test.cc +++ b/test/xml/xml_api_test.cc @@ -14,6 +14,8 @@ // Tests for xml/xml_api.cc. +#include "src/xml/xml_api.h" + #include #include #include @@ -23,18 +25,17 @@ #include #include #include -#include #include -#include "src/xml/xml_api.h" +#include #include "test/fixture.h" namespace mujoco { namespace { +using ::testing::HasSubstr; using ::testing::IsNull; using ::testing::NotNull; using ::testing::StartsWith; -using ::testing::HasSubstr; static constexpr char xml[] = R"( @@ -57,62 +58,56 @@ using LoadXmlTest = MujocoTest; TEST_F(LoadXmlTest, EmptyModel) { static constexpr char xml[] = ""; - mjModel* model = LoadModelFromString(xml, 0, 0); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, 0, 0); + ASSERT_THAT(model.get(), NotNull()); EXPECT_EQ(model->nq, 0); EXPECT_EQ(model->nv, 0); EXPECT_EQ(model->nu, 0); EXPECT_EQ(model->na, 0); EXPECT_EQ(model->nbody, 1); // worldbody exists even in empty model - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); EXPECT_THAT(data, NotNull()); - mj_step(model, data); - mj_deleteData(data); - mj_deleteModel(model); + mj_step(model.get(), data.get()); } TEST_F(LoadXmlTest, InvalidXmlFailsToLoad) { static constexpr char invalid_xml[] = " error; - mjModel* model = LoadModelFromString(invalid_xml, error.data(), error.size()); - EXPECT_THAT(model, IsNull()) << "Expected model loading to fail."; + MjModelPtr model = + LoadModelFromString(invalid_xml, error.data(), error.size()); + EXPECT_THAT(model.get(), IsNull()) << "Expected model loading to fail."; EXPECT_GT(std::strlen(error.data()), 0); - if (model) { - mj_deleteModel(model); + if (model.get()) { } } TEST_F(LoadXmlTest, MultipleBodies) { std::array 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); - mjData* data = mj_makeData(model); + MjDataPtr data = MakeData(model); EXPECT_THAT(data, NotNull()); - mj_step(model, data); - mj_deleteData(data); - mj_deleteModel(model); + mj_step(model.get(), data.get()); } using SaveLastXmlTest = MujocoTest; TEST_F(SaveLastXmlTest, EmptyModel) { static constexpr char xml[] = ""; - mjModel* model = LoadModelFromString(xml, 0, 0); - mjData* data = mj_makeData(model); + MjModelPtr model = LoadModelFromString(xml, 0, 0); + MjDataPtr data = MakeData(model); std::array error; error.data()[0] = '\0'; 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(" out; - EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), - error.data(), error.size()), 273); + EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), error.data(), + error.size()), + 273); EXPECT_STREQ(error.data(), "Output string too short, should be at least 274"); mj_deleteSpec(spec); @@ -156,10 +152,13 @@ TEST_F(MujocoTest, SaveXml) { std::array out; 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_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()); EXPECT_THAT(saved_spec, NotNull()) << "Invalid saved spec: " << error.data(); @@ -197,7 +196,9 @@ TEST_F(MujocoTest, SaveXmlWithDefaultMesh) { std::array out; 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()); EXPECT_THAT(saved_spec, NotNull()) << "Invalid saved spec: " << error.data(); @@ -215,9 +216,8 @@ TEST_F(MujocoTest, SaveXmlWithDefaultMesh) { TEST_F(MujocoTest, FreeLastXml) { static constexpr char xml[] = ""; - mjModel* model = LoadModelFromString(xml, 0, 0); - ASSERT_THAT(model, NotNull()); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, 0, 0); + ASSERT_THAT(model.get(), NotNull()); ASSERT_NE(mj_saveLastXML(nullptr, nullptr, nullptr, 0), 0); mj_freeLastXML(); ASSERT_EQ(mj_saveLastXML(nullptr, nullptr, nullptr, 0), 0); diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 3f848ee9..8d840604 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -59,8 +59,8 @@ TEST_F(XMLReaderTest, UniqueElementTest) { )"; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("unique element 'flag' found 2 times")); } @@ -139,10 +139,9 @@ TEST_F(XMLReaderTest, MemorySize) { )"; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->narena, 512); - mj_deleteModel(model); } { static constexpr char xml[] = R"( @@ -150,10 +149,9 @@ TEST_F(XMLReaderTest, MemorySize) { )"; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->narena, 1024); - mj_deleteModel(model); } { static constexpr char xml[] = R"( @@ -161,10 +159,9 @@ TEST_F(XMLReaderTest, MemorySize) { )"; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->narena, 10240); - mj_deleteModel(model); } { static constexpr char xml[] = R"( @@ -172,10 +169,9 @@ TEST_F(XMLReaderTest, MemorySize) { )"; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->narena, 4*1024*1024); - mj_deleteModel(model); } { static constexpr char xml[] = R"( @@ -183,10 +179,9 @@ TEST_F(XMLReaderTest, MemorySize) { )"; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->narena, 1024*1024*1024); - mj_deleteModel(model); } { static constexpr char xml[] = R"( @@ -194,10 +189,9 @@ TEST_F(XMLReaderTest, MemorySize) { )"; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->narena, 1024*1024*1024); - mj_deleteModel(model); } } @@ -209,8 +203,8 @@ TEST_F(XMLReaderTest, InvalidMemorySize) { )"; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); } { static constexpr char xml[] = R"( @@ -218,8 +212,8 @@ TEST_F(XMLReaderTest, InvalidMemorySize) { )"; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); } { static constexpr char xml[] = R"( @@ -227,8 +221,8 @@ TEST_F(XMLReaderTest, InvalidMemorySize) { )"; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); } { static constexpr char xml[] = R"( @@ -236,8 +230,8 @@ TEST_F(XMLReaderTest, InvalidMemorySize) { )"; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); } } @@ -248,8 +242,8 @@ TEST_F(XMLReaderTest, InvalidNUserBody) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_body")); EXPECT_THAT(error.data(), HasSubstr("line 3")); } @@ -261,8 +255,8 @@ TEST_F(XMLReaderTest, InvalidNUserJoint) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_jnt")); EXPECT_THAT(error.data(), HasSubstr("line 3")); } @@ -274,8 +268,8 @@ TEST_F(XMLReaderTest, InvalidNUserGeom) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_geom")); EXPECT_THAT(error.data(), HasSubstr("line 3")); } @@ -287,8 +281,8 @@ TEST_F(XMLReaderTest, InvalidNUserSite) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_site")); EXPECT_THAT(error.data(), HasSubstr("line 3")); } @@ -300,8 +294,8 @@ TEST_F(XMLReaderTest, InvalidNUserCamera) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_cam")); EXPECT_THAT(error.data(), HasSubstr("line 3")); } @@ -313,8 +307,8 @@ TEST_F(XMLReaderTest, InvalidNUserTendon) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_tendon")); EXPECT_THAT(error.data(), HasSubstr("line 3")); } @@ -326,8 +320,8 @@ TEST_F(XMLReaderTest, InvalidNUserActuator) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_actuator")); EXPECT_THAT(error.data(), HasSubstr("line 3")); } @@ -339,8 +333,8 @@ TEST_F(XMLReaderTest, InvalidNUserSensor) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("nuser_sensor")); EXPECT_THAT(error.data(), HasSubstr("line 3")); } @@ -357,15 +351,14 @@ TEST_F(XMLReaderTest, CanParseInf) { )"; const double inf = std::numeric_limits::infinity(); - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); EXPECT_EQ(model->geom_pos[0], 0.5); EXPECT_EQ(model->geom_pos[1], -inf); EXPECT_THAT(model->geom_pos[2], inf); EXPECT_EQ(model->geom_pos[3], inf); EXPECT_EQ(model->geom_pos[4], -inf); EXPECT_EQ(model->geom_pos[5], inf); - mj_deleteModel(model); } TEST_F(XMLReaderTest, CanParseNanAndRaisesWarning) { @@ -382,15 +375,14 @@ TEST_F(XMLReaderTest, CanParseNanAndRaisesWarning) { std::array error; MockWarningHandler warning_handler; warning_handler.ExpectWarnings("XML contains a 'NaN'"); - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); EXPECT_THAT(model->geom_pos[0], IsNan()); EXPECT_THAT(model->geom_pos[1], IsNan()); EXPECT_THAT(model->geom_pos[2], IsNan()); EXPECT_EQ(model->geom_pos[3], 1); EXPECT_EQ(model->geom_pos[4], 0); EXPECT_THAT(model->geom_pos[5], IsNan()); - mj_deleteModel(model); } TEST_F(XMLReaderTest, InvalidArrayElement) { @@ -404,8 +396,8 @@ TEST_F(XMLReaderTest, InvalidArrayElement) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("bad format in attribute 'axisangle'")); EXPECT_THAT(error.data(), HasSubstr("line 5")); } @@ -421,8 +413,8 @@ TEST_F(XMLReaderTest, InvalidArrayLength) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("has too much data")); EXPECT_THAT(error.data(), HasSubstr("line 5")); } @@ -438,8 +430,8 @@ TEST_F(XMLReaderTest, InvalidQuaternion) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("zero quaternion is not allowed")); EXPECT_THAT(error.data(), HasSubstr("line 5")); } @@ -455,8 +447,8 @@ TEST_F(XMLReaderTest, InvalidNumber) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("bad format in attribute 'axisangle'")); EXPECT_THAT(error.data(), HasSubstr("line 5")); } @@ -470,8 +462,8 @@ TEST_F(XMLReaderTest, InvalidNumberRange) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("number is too large in attribute 'face'")); EXPECT_THAT(error.data(), HasSubstr("line 4")); @@ -489,8 +481,8 @@ TEST_F(XMLReaderTest, InvalidNumberOfAttributes) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("size 2 must be positive")); EXPECT_THAT(error.data(), HasSubstr("line 6")); } @@ -506,9 +498,8 @@ TEST_F(XMLReaderTest, AllowsSpaces) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); } TEST_F(XMLReaderTest, InvalidDoubleOrientation) { @@ -530,9 +521,8 @@ TEST_F(XMLReaderTest, InvalidDoubleOrientation) { if (orient1 == orient2) continue; std::string xml = prefix + field + orient1 + orient2 + suffix; std::array error; - mjModel* model = - LoadModelFromString(xml.c_str(), error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml.c_str(), error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT( error.data(), HasSubstr("multiple orientation specifiers are not allowed")); @@ -565,13 +555,12 @@ TEST_F(XMLReaderTest, ClassOverridesChildclass) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->geom_size[3*0], 2); EXPECT_EQ(model->geom_size[3*1], 3); EXPECT_EQ(model->geom_size[3*2], 2); EXPECT_EQ(model->geom_size[3*3], 3); - mj_deleteModel(model); } TEST_F(XMLReaderTest, RepeatedDefaultName) { @@ -593,8 +582,8 @@ TEST_F(XMLReaderTest, RepeatedDefaultName) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("repeated default class name")); } @@ -614,8 +603,8 @@ TEST_F(XMLReaderTest, InvalidDefaultClassName) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), AllOf(HasSubstr("unknown default class name 'invalid'"), HasSubstr("Element 'geom'"), HasSubstr("line 10"))); @@ -635,8 +624,8 @@ TEST_F(XMLReaderTest, InvalidTopDefaultClassName) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("top-level default class 'main' cannot be renamed")); } @@ -655,9 +644,8 @@ TEST_F(XMLReaderTest, ValidTopDefaultClassName) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); } // ------------------------ test including ------------------------------------- @@ -720,12 +708,11 @@ TEST_F(XMLReaderTest, IncludeTest) { mj_addBufferVFS(vfs.get(), "model3.xml", xml3, sizeof(xml3)); std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size(), vfs.get()); - ASSERT_THAT(model, NotNull()) << error.data(); - EXPECT_EQ(mj_name2id(model, mjOBJ_GEOM, "ball"), 2); - EXPECT_EQ(mj_name2id(model, mjOBJ_GEOM, "another_box"), 3); - mj_deleteModel(model); + ASSERT_THAT(model.get(), NotNull()) << error.data(); + EXPECT_EQ(mj_name2id(model.get(), mjOBJ_GEOM, "ball"), 2); + EXPECT_EQ(mj_name2id(model.get(), mjOBJ_GEOM, "another_box"), 3); mj_deleteVFS(vfs.get()); } @@ -741,10 +728,9 @@ TEST_F(XMLReaderTest, IncludeChildTest) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("Include element cannot have children")); - mj_deleteModel(model); } TEST_F(XMLReaderTest, IncludeSameFileTest) { @@ -765,11 +751,10 @@ TEST_F(XMLReaderTest, IncludeSameFileTest) { mj_addBufferVFS(vfs.get(), "model1.xml", xml1, sizeof(xml1)); std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size(), + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size(), vfs.get()); - ASSERT_THAT(model, IsNull()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("File 'model1.xml' already included")); - mj_deleteModel(model); mj_deleteVFS(vfs.get()); } @@ -946,8 +931,8 @@ TEST_F(XMLReaderTest, MaterialTextureFailTest) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("A material with a texture attribute " "cannot have layer sub-elements")); } @@ -969,10 +954,9 @@ TEST_F(XMLReaderTest, LargeTextureTest) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()); - mj_deleteModel(model); + EXPECT_THAT(model.get(), NotNull()); } TEST_F(XMLReaderTest, LargeTextureAddressTest) { @@ -992,12 +976,11 @@ TEST_F(XMLReaderTest, LargeTextureAddressTest) { )"; std::array 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->ntex, 2); EXPECT_GT(model->tex_adr[1], INT32_MAX); - mj_deleteModel(model); } TEST_F(XMLReaderTest, IncludeAssetsTest) { @@ -1203,8 +1186,8 @@ TEST_F(XMLReaderTest, ParsePolycoef) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), NotNull()) << error.data(); EXPECT_THAT(AsVector(m->eq_data + 0*mjNEQDATA, 5), ElementsAre(0, 1, 0, 0, 0)); EXPECT_THAT(AsVector(m->eq_data + 1*mjNEQDATA, 5), @@ -1213,7 +1196,6 @@ TEST_F(XMLReaderTest, ParsePolycoef) { ElementsAre(3, 4, 0, 0, 0)); EXPECT_THAT(AsVector(m->eq_data + 3*mjNEQDATA, 5), ElementsAre(5, 6, 7, 8, 9)); - mj_deleteModel(m); } TEST_F(XMLReaderTest, TendonArmature) { @@ -1243,13 +1225,12 @@ TEST_F(XMLReaderTest, TendonArmature) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), NotNull()) << error.data(); EXPECT_EQ(m->ntendon, 3); EXPECT_MJTNUM_EQ(m->tendon_armature[0], 1.5); EXPECT_MJTNUM_EQ(m->tendon_armature[1], 2.5); EXPECT_MJTNUM_EQ(m->tendon_armature[2], 0); - mj_deleteModel(m); } TEST_F(XMLReaderTest, TendonArmatureNegative) { @@ -1269,8 +1250,8 @@ TEST_F(XMLReaderTest, TendonArmatureNegative) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("tendon armature cannot be negative")); } @@ -1293,8 +1274,8 @@ TEST_F(XMLReaderTest, TendonArmatureGeomWrap) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("geom wrapping not supported")); } @@ -1338,14 +1319,13 @@ TEST_F(XMLReaderTest, ParseFrame) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), NotNull()) << error.data(); EXPECT_THAT(m->geom_size[ 0], .5); EXPECT_THAT(m->geom_size[ 3], .6); EXPECT_THAT(m->geom_size[ 6], .1); EXPECT_THAT(m->geom_size[ 9], .2); EXPECT_THAT(m->geom_size[12], .3); - mj_deleteModel(m); } TEST_F(XMLReaderTest, DuplicateFrameName) { @@ -1363,8 +1343,8 @@ TEST_F(XMLReaderTest, DuplicateFrameName) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()) << error.data(); EXPECT_THAT(error.data(), HasSubstr("repeated name 'frame1'")); } @@ -1406,8 +1386,8 @@ TEST_F(XMLReaderTest, ParseReplicate) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, testing::NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), testing::NotNull()) << error.data(); EXPECT_THAT(m->ngeom, 105); EXPECT_THAT(m->nsensor, 4); EXPECT_THAT(m->nbody, 102); @@ -1467,8 +1447,6 @@ TEST_F(XMLReaderTest, ParseReplicate) { EXPECT_THAT(m->key_qpos[i*m->nq+j], i == j ? 1 : 0) << i << " " << j; } } - - mj_deleteModel(m); } TEST_F(XMLReaderTest, ParseReplicatePartialReference) { @@ -1506,8 +1484,8 @@ TEST_F(XMLReaderTest, ParseReplicatePartialReference) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, testing::NotNull()) << error.data(); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), testing::NotNull()) << error.data(); EXPECT_THAT(m->nbody, 3); EXPECT_THAT(m->ngeom, 3); @@ -1515,8 +1493,6 @@ TEST_F(XMLReaderTest, ParseReplicatePartialReference) { EXPECT_THAT(m->nexclude, 2); EXPECT_THAT(m->ntendon, 2); EXPECT_THAT(m->nsensor, 2); - - mj_deleteModel(m); } TEST_F(XMLReaderTest, ParseReplicateDefaultPropagate) { @@ -1860,21 +1836,17 @@ TEST_F(XMLReaderTest, AttachSpecAssets) { mj_addBufferVFS(vfs.get(), "xml_child.xml", xml_child, sizeof(xml_child)); std::array er; - mjModel* model = - LoadModelFromString(xml_parent, er.data(), er.size(), vfs.get()); - EXPECT_THAT(model, NotNull()) << er.data(); + MjModelPtr model = LoadModelFromString(xml_parent, er.data(), er.size(), vfs.get()); + EXPECT_THAT(model.get(), NotNull()) << er.data(); - mjModel* expected = LoadModelFromString(xml_expected, er.data(), er.size()); - EXPECT_THAT(expected, NotNull()) << er.data(); + MjModelPtr expected = LoadModelFromString(xml_expected, er.data(), er.size()); + EXPECT_THAT(expected.get(), NotNull()) << er.data(); mjtNum tol = 0; std::string field = ""; - EXPECT_LE(CompareModel(model, expected, field), tol) + EXPECT_LE(CompareModel(model.get(), expected.get(), field), tol) << "Expected and attached models are different!\n" << "Different field: " << field << '\n';; - - mj_deleteModel(model); - mj_deleteModel(expected); mj_deleteVFS(vfs.get()); } @@ -1918,10 +1890,9 @@ TEST_F(XMLReaderTest, InvalidAttach) { mj_addBufferVFS(vfs.get(), "child.xml", xml_child, sizeof(xml_child)); std::array er; - mjModel* model = - LoadModelFromString(xml_parent, er.data(), er.size(), vfs.get()); + MjModelPtr model = LoadModelFromString(xml_parent, er.data(), er.size(), vfs.get()); - EXPECT_THAT(model, IsNull()) << er.data(); + EXPECT_THAT(model.get(), IsNull()) << er.data(); EXPECT_THAT(er.data(), HasSubstr("repeated name '_actuator' in actuator")); EXPECT_THAT(er.data(), HasSubstr("Element 'attach'")); mj_deleteVFS(vfs.get()); @@ -2028,10 +1999,8 @@ TEST_F(XMLReaderTest, ResizeKeyframeAfterParsing) { mj_addBufferVFS(vfs.get(), "child.xml", child_xml, sizeof(child_xml)); std::array error; - mjModel* m = - LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get()); - EXPECT_THAT(m, NotNull()) << error.data(); - mj_deleteModel(m); + MjModelPtr m = LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get()); + EXPECT_THAT(m.get(), NotNull()) << error.data(); mj_deleteVFS(vfs.get()); } @@ -2049,8 +2018,8 @@ TEST_F(XMLReaderTest, CameraInvalidFovyAndSensorsize) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("either 'fovy' or 'sensorsize'")); EXPECT_THAT(error.data(), HasSubstr("line 6")); } @@ -2067,8 +2036,8 @@ TEST_F(XMLReaderTest, CameraPrincipalRequiresSensorsize) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("focal/principal require sensorsize")); EXPECT_THAT(error.data(), HasSubstr("line 6")); } @@ -2085,8 +2054,8 @@ TEST_F(XMLReaderTest, CameraSensorsizeRequiresResolution) { )"; std::array error; - mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, IsNull()); + MjModelPtr m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("requires positive resolution")); EXPECT_THAT(error.data(), HasSubstr("line 6")); } @@ -2104,8 +2073,8 @@ TEST_F(XMLReaderTest, InvalidInertialOrientation) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT( error.data(), HasSubstr( @@ -2126,9 +2095,8 @@ TEST_F(XMLReaderTest, ReadShellParameter) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); } // ----------------------- test builtin mesh parsing --------------------------- @@ -2145,9 +2113,8 @@ TEST_F(XMLReaderTest, ReadWedgeMesh) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); } TEST_F(XMLReaderTest, BuiltinAndFile) { @@ -2163,11 +2130,10 @@ TEST_F(XMLReaderTest, BuiltinAndFile) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("builtin mesh cannot be used with user vertex data")); - mj_deleteModel(model); } TEST_F(XMLReaderTest, MakePlateNoParameters) { @@ -2181,10 +2147,9 @@ TEST_F(XMLReaderTest, MakePlateNoParameters) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("required attribute missing: 'params'")); - mj_deleteModel(model); } TEST_F(XMLReaderTest, MakePlateTooFewParameters) { @@ -2198,12 +2163,11 @@ TEST_F(XMLReaderTest, MakePlateTooFewParameters) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT( error.data(), HasSubstr("Plate builtin mesh type requires 2 parameters")); - mj_deleteModel(model); } TEST_F(XMLReaderTest, MakePlateInvalidParameters) { @@ -2217,10 +2181,9 @@ TEST_F(XMLReaderTest, MakePlateInvalidParameters) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("resolutions must be positive")); - mj_deleteModel(model); } // ----------------------- test skin parsing -------------------------------- @@ -2301,15 +2264,14 @@ TEST_F(XMLReaderTest, ReadsSkinGroups) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); - int flexid1 = mj_name2id(model, mjOBJ_FLEX, "B0"); - int flexid2 = mj_name2id(model, mjOBJ_FLEX, "B1"); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); + int flexid1 = mj_name2id(model.get(), mjOBJ_FLEX, "B0"); + int flexid2 = mj_name2id(model.get(), mjOBJ_FLEX, "B1"); EXPECT_THAT(model->flex_group[flexid1], 2); EXPECT_THAT(model->skin_group[0], 4); EXPECT_THAT(model->flex_group[flexid2], 4); EXPECT_THAT(model->skin_group[1], 2); - mj_deleteModel(model); } TEST_F(XMLReaderTest, InvalidSkinGroup) { @@ -2326,8 +2288,8 @@ TEST_F(XMLReaderTest, InvalidSkinGroup) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT( error.data(), HasSubstr("skin group must be between 0 and 5\nElement 'skin', line 7")); @@ -2352,16 +2314,14 @@ TEST_F(XMLReaderTest, Orthographic) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->vis.global.orthographic, 1); EXPECT_EQ(model->cam_projection[0], mjPROJ_ORTHOGRAPHIC); EXPECT_EQ(model->cam_projection[1], mjPROJ_ORTHOGRAPHIC); EXPECT_EQ(model->cam_fovy[0], 1); EXPECT_EQ(model->cam_fovy[1], 2); - - mj_deleteModel(model); } // ------------- test height-field parsing ------------------------------------- @@ -2377,15 +2337,14 @@ TEST_F(HfieldParsingTest, NoData) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->hfield_nrow[0], 4); EXPECT_EQ(model->hfield_ncol[0], 3); EXPECT_EQ(model->hfield_size[0], 0.5); EXPECT_EQ(model->hfield_size[1], 0.5); EXPECT_EQ(model->hfield_size[2], 1); EXPECT_MJTNUM_EQ(model->hfield_size[3], 0.1); - mj_deleteModel(model); } TEST_F(HfieldParsingTest, HasDataBadSize) { @@ -2400,8 +2359,8 @@ TEST_F(HfieldParsingTest, HasDataBadSize) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("data length must match nrow*ncol")); EXPECT_THAT(error.data(), HasSubstr("line 4")); } @@ -2418,8 +2377,8 @@ TEST_F(HfieldParsingTest, HasData) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->hfield_nrow[0], 3); EXPECT_EQ(model->hfield_ncol[0], 2); EXPECT_EQ(model->hfield_size[0], 0.5); @@ -2438,7 +2397,6 @@ TEST_F(HfieldParsingTest, HasData) { EXPECT_THAT(model->hfield_data[3], FloatEq((4-offset)/scale)); EXPECT_THAT(model->hfield_data[4], FloatEq((1-offset)/scale)); EXPECT_THAT(model->hfield_data[5], FloatEq((2-offset)/scale)); - mj_deleteModel(model); } @@ -2501,8 +2459,8 @@ TEST_F(ActuatorTest, InvalidActlimited) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("unrecognized attribute")); EXPECT_THAT(error.data(), HasSubstr("line 10")); } @@ -2522,8 +2480,8 @@ TEST_F(ActuatorTest, IncompleteActlimited) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("attribute 'actrange' does not have enough data")); } @@ -2543,10 +2501,9 @@ TEST_F(ActuatorTest, ReadsByte) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); EXPECT_EQ(*(model->actuator_actlimited), true); - mj_deleteModel(model); } // ---------------- test actuator parsing -------------------------------------- @@ -2568,11 +2525,10 @@ TEST_F(ActuatorParseTest, PositionTimeconst) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); ASSERT_NEAR(model->actuator_dynprm[0], 2.0, 1e-6); EXPECT_THAT(model->actuator_dyntype[0], Eq(mjDYN_FILTEREXACT)); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, PositionTimeconstInheritrange) { @@ -2590,9 +2546,8 @@ TEST_F(ActuatorParseTest, PositionTimeconstInheritrange) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); } TEST_F(ActuatorParseTest, PositionTimeconstDefault) { @@ -2613,11 +2568,10 @@ TEST_F(ActuatorParseTest, PositionTimeconstDefault) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); ASSERT_NEAR(model->actuator_dynprm[0], 1.0, 1e-6); EXPECT_THAT(model->actuator_dyntype[0], Eq(mjDYN_FILTEREXACT)); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, PositionTimeconstDefaultOverride) { @@ -2638,11 +2592,10 @@ TEST_F(ActuatorParseTest, PositionTimeconstDefaultOverride) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); EXPECT_FALSE(model->actuator_dynprm[0]); EXPECT_THAT(model->actuator_dyntype[0], Eq(mjDYN_NONE)); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, ReadsDamper) { @@ -2661,13 +2614,12 @@ TEST_F(ActuatorParseTest, ReadsDamper) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); EXPECT_THAT(model->actuator_gaintype[0], Eq(mjGAIN_AFFINE)); EXPECT_THAT(model->actuator_gaintype[1], Eq(mjGAIN_AFFINE)); EXPECT_THAT(model->actuator_biastype[0], Eq(mjBIAS_NONE)); EXPECT_THAT(model->actuator_biastype[1], Eq(mjBIAS_NONE)); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DamperRequiresPositiveDamping) { @@ -2685,8 +2637,8 @@ TEST_F(ActuatorParseTest, DamperRequiresPositiveDamping) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("damping coefficient cannot be negative")); } @@ -2706,8 +2658,8 @@ TEST_F(ActuatorParseTest, DamperRequiresControlRange) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("invalid control range")); EXPECT_THAT(error.data(), HasSubstr("line 10")); } @@ -2727,8 +2679,8 @@ TEST_F(ActuatorParseTest, DamperPositiveControlRange) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("control range cannot be negative")); EXPECT_THAT(error.data(), HasSubstr("line 10")); } @@ -2749,11 +2701,10 @@ TEST_F(ActuatorParseTest, ReadsPositionIntvelKv) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); EXPECT_THAT(model->actuator_biasprm[0*mjNBIAS + 2], Eq(-2.0)); EXPECT_THAT(model->actuator_biasprm[1*mjNBIAS + 2], Eq(-3.0)); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, RequirePositiveKv) { @@ -2771,8 +2722,8 @@ TEST_F(ActuatorParseTest, RequirePositiveKv) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("kv cannot be negative")); EXPECT_THAT(error.data(), HasSubstr("line 10")); } @@ -2805,8 +2756,8 @@ TEST_F(ActuatorParseTest, PositionIntvelocityVelocityDefaultsPropagate) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->actuator_gainprm[0*mjNGAIN + 0], 3.0); EXPECT_EQ(model->actuator_biasprm[0*mjNBIAS + 1], -3.0); EXPECT_EQ(model->actuator_biasprm[0*mjNBIAS + 2], -4.0); @@ -2828,7 +2779,6 @@ TEST_F(ActuatorParseTest, PositionIntvelocityVelocityDefaultsPropagate) { EXPECT_EQ(model->actuator_ctrlrange[0*2 + 1], 3.0); EXPECT_EQ(model->actuator_actrange[1*2 + 0], 0.5); EXPECT_EQ(model->actuator_actrange[1*2 + 1], 1.5); - mj_deleteModel(model); } @@ -2849,8 +2799,8 @@ TEST_F(ActuatorParseTest, IntvelocityCheckEquivalence) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); // same actlimited EXPECT_EQ(model->actuator_actlimited[0], 1); EXPECT_EQ(model->actuator_actlimited[1], 1); @@ -2878,7 +2828,6 @@ TEST_F(ActuatorParseTest, IntvelocityCheckEquivalence) { EXPECT_MJTNUM_EQ(model->actuator_actrange[0 + 1], 1.57); EXPECT_MJTNUM_EQ(model->actuator_actrange[0 + 2], -1.57); EXPECT_MJTNUM_EQ(model->actuator_actrange[0 + 3], 1.57); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, IntvelocityCheckDefaultsIfNotSpecified) { @@ -2896,15 +2845,14 @@ TEST_F(ActuatorParseTest, IntvelocityCheckDefaultsIfNotSpecified) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); // check that by default kp = 1 EXPECT_DOUBLE_EQ(model->actuator_gainprm[0], 1.0); // check that biasprm is (0, -1, 0) EXPECT_DOUBLE_EQ(model->actuator_biasprm[0], 0.0); EXPECT_DOUBLE_EQ(model->actuator_biasprm[1], -1.0); EXPECT_DOUBLE_EQ(model->actuator_biasprm[2], 0.0); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, IntvelocityNoActrangeThrowsError) { @@ -2922,8 +2870,8 @@ TEST_F(ActuatorParseTest, IntvelocityNoActrangeThrowsError) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("invalid actrange for actuator")); EXPECT_THAT(error.data(), HasSubstr("line 10")); } @@ -2951,15 +2899,14 @@ TEST_F(ActuatorParseTest, IntvelocityDefaultsPropagate) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_DOUBLE_EQ(model->actuator_gainprm[0], 5); EXPECT_DOUBLE_EQ(model->actuator_gainprm[mjNGAIN], 1); EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 0], 0); EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 1], 1); EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 2], -1); EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 3], 1); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, AdhesionDefaultsPropagate) { @@ -2979,11 +2926,10 @@ TEST_F(ActuatorParseTest, AdhesionDefaultsPropagate) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); EXPECT_EQ(model->actuator_ctrlrange[0], 0); EXPECT_EQ(model->actuator_ctrlrange[1], 3); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, ErrorBadAdhesionDefaults) { @@ -3003,8 +2949,8 @@ TEST_F(ActuatorParseTest, ErrorBadAdhesionDefaults) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("adhesion control range cannot be negative")); } @@ -3028,11 +2974,10 @@ TEST_F(ActuatorParseTest, DampersDontRequireRange) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); EXPECT_EQ(model->actuator_ctrlrange[0], 0); EXPECT_EQ(model->actuator_ctrlrange[1], 2); - mj_deleteModel(model); } // adhesion actuators inherit from general defaults @@ -3055,8 +3000,8 @@ TEST_F(ActuatorParseTest, AdhesionInheritsFromGeneral) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); // expect that gainprm was inherited from the general default EXPECT_EQ(model->actuator_gainprm[0], 5); @@ -3064,7 +3009,6 @@ TEST_F(ActuatorParseTest, AdhesionInheritsFromGeneral) { EXPECT_EQ(model->actuator_dynprm[0], 123); // expect that dyntype was inherited from the general default EXPECT_EQ(model->actuator_dyntype[0], mjDYN_FILTER); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DCMotorBasicParsing) { @@ -3082,8 +3026,8 @@ TEST_F(ActuatorParseTest, DCMotorBasicParsing) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->actuator_dyntype[0], mjDYN_DCMOTOR); EXPECT_EQ(model->actuator_gaintype[0], mjGAIN_DCMOTOR); EXPECT_EQ(model->actuator_biastype[0], mjBIAS_DCMOTOR); @@ -3093,7 +3037,6 @@ TEST_F(ActuatorParseTest, DCMotorBasicParsing) { EXPECT_MJTNUM_EQ(model->actuator_dampingpoly[0], 2.0); EXPECT_MJTNUM_EQ(model->actuator_dampingpoly[1], 3.0); EXPECT_MJTNUM_EQ(model->actuator_armature[0], 0.1); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DCMotorNominalDerivation) { @@ -3116,8 +3059,8 @@ TEST_F(ActuatorParseTest, DCMotorNominalDerivation) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); // actuator 0: B = 0, Ke = vn/omega0 { @@ -3144,8 +3087,6 @@ TEST_F(ActuatorParseTest, DCMotorNominalDerivation) { EXPECT_MJTNUM_EQ(model->actuator_gainprm[2*mjNGAIN + 0], R); EXPECT_MJTNUM_EQ(model->actuator_gainprm[2*mjNGAIN + 1], Ke); } - - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DCMotorSaturation) { @@ -3164,12 +3105,11 @@ TEST_F(ActuatorParseTest, DCMotorSaturation) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->actuator_forcelimited[0], 1); EXPECT_MJTNUM_EQ(model->actuator_forcerange[0], -1.5); EXPECT_MJTNUM_EQ(model->actuator_forcerange[1], 1.5); - mj_deleteModel(model); } @@ -3192,8 +3132,8 @@ TEST_F(ActuatorParseTest, DCMotorInheritedDefaults) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); // check motorconst and resistance are overridden by instance EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], 0.05); @@ -3216,8 +3156,6 @@ TEST_F(ActuatorParseTest, DCMotorInheritedDefaults) { // check inductance (te) in dynprm[0] EXPECT_MJTNUM_EQ(model->actuator_dynprm[0], 0.01); - - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DCMotorControllerFull) { @@ -3236,8 +3174,8 @@ TEST_F(ActuatorParseTest, DCMotorControllerFull) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_MJTNUM_EQ(model->actuator_gainprm[4], 1.0); EXPECT_MJTNUM_EQ(model->actuator_gainprm[5], 2.0); @@ -3245,8 +3183,6 @@ TEST_F(ActuatorParseTest, DCMotorControllerFull) { EXPECT_MJTNUM_EQ(model->actuator_dynprm[7], 4.0); EXPECT_MJTNUM_EQ(model->actuator_dynprm[8], 5.0); EXPECT_MJTNUM_EQ(model->actuator_gainprm[7], 6.0); - - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) { @@ -3265,15 +3201,14 @@ TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_MJTNUM_EQ(model->actuator_dynprm[5], 100); EXPECT_MJTNUM_EQ(model->actuator_dynprm[6], 1); EXPECT_MJTNUM_EQ(model->actuator_damping[0], 0.01); EXPECT_MJTNUM_EQ(model->actuator_biasprm[3], 0.5); EXPECT_MJTNUM_EQ(model->actuator_biasprm[4], 0.7); EXPECT_MJTNUM_EQ(model->actuator_biasprm[5], 10); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DCMotorLuGreInheritedDefaults) { @@ -3294,10 +3229,9 @@ TEST_F(ActuatorParseTest, DCMotorLuGreInheritedDefaults) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_MJTNUM_EQ(model->actuator_damping[0], 0.01); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DCMotorActdimStateless) { @@ -3315,11 +3249,10 @@ TEST_F(ActuatorParseTest, DCMotorActdimStateless) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->actuator_actnum[0], 0); EXPECT_EQ(model->actuator_actadr[0], -1); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DCMotorActdimCurrentOnly) { @@ -3338,11 +3271,10 @@ TEST_F(ActuatorParseTest, DCMotorActdimCurrentOnly) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->actuator_actnum[0], 1); EXPECT_MJTNUM_EQ(model->actuator_dynprm[0], 0.001 / 2.0); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DCMotorActdimThermalOnly) { @@ -3361,13 +3293,12 @@ TEST_F(ActuatorParseTest, DCMotorActdimThermalOnly) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->actuator_actnum[0], 1); EXPECT_MJTNUM_EQ(model->actuator_dynprm[2], 10); EXPECT_MJTNUM_EQ(model->actuator_dynprm[3], 5); EXPECT_MJTNUM_EQ(model->actuator_dynprm[4], 25); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DCMotorActdimLuGreOnly) { @@ -3386,11 +3317,10 @@ TEST_F(ActuatorParseTest, DCMotorActdimLuGreOnly) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->actuator_actnum[0], 1); EXPECT_MJTNUM_EQ(model->actuator_dynprm[5], 100); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DCMotorActdimAllThree) { @@ -3411,10 +3341,9 @@ TEST_F(ActuatorParseTest, DCMotorActdimAllThree) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->actuator_actnum[0], 3); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DCMotorMissingKError) { @@ -3432,8 +3361,8 @@ TEST_F(ActuatorParseTest, DCMotorMissingKError) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("motor constant K must be positive")); } @@ -3455,11 +3384,10 @@ TEST_F(ActuatorParseTest, DCMotorDefaultsPropagate) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_MJTNUM_EQ(model->actuator_gainprm[0], 1.5); EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], 0.03); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, DCMotorMotorconstGeometricMean) { @@ -3478,13 +3406,12 @@ TEST_F(ActuatorParseTest, DCMotorMotorconstGeometricMean) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); double K = std::sqrt(0.03 * 0.05); EXPECT_MJTNUM_EQ(model->actuator_gainprm[0], 2.0); EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], K); EXPECT_MJTNUM_EQ(model->actuator_gainprm[mjNGAIN + 1], 0.03); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, ActdimDefaultsPropagate) { @@ -3505,12 +3432,11 @@ TEST_F(ActuatorParseTest, ActdimDefaultsPropagate) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); // expect that actdim was inherited from the general default EXPECT_EQ(model->actuator_actnum[0], 2); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, MusclesParseSmoothdyn) { @@ -3529,11 +3455,10 @@ TEST_F(ActuatorParseTest, MusclesParseSmoothdyn) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->actuator_dynprm[2], 0.0); EXPECT_MJTNUM_EQ(model->actuator_dynprm[mjNDYN + 2], 0.4); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, MusclesSmoothdynNegative) { @@ -3551,8 +3476,8 @@ TEST_F(ActuatorParseTest, MusclesSmoothdynNegative) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("muscle tausmooth cannot be negative")); EXPECT_THAT(error.data(), HasSubstr("line 10")); } @@ -3565,10 +3490,9 @@ TEST_F(ActuatorParseTest, GroupDisable) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()); EXPECT_EQ(model->opt.disableactuator, (1<<0) + (1<<3) + (1<<8)); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, GroupDisableNegative) { @@ -3578,8 +3502,8 @@ TEST_F(ActuatorParseTest, GroupDisableNegative) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("must be non-negative")); EXPECT_THAT(error.data(), HasSubstr("line 3")); } @@ -3591,8 +3515,8 @@ TEST_F(ActuatorParseTest, GroupDisableTooBig) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("cannot exceed 30")); EXPECT_THAT(error.data(), HasSubstr("line 3")); } @@ -3610,8 +3534,8 @@ TEST_F(SensorParseTest, UserObjTypeNoName) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("objtype 'site' given but")); EXPECT_THAT(error.data(), HasSubstr("line 4")); } @@ -3625,8 +3549,8 @@ TEST_F(SensorParseTest, UserObjNameNoType) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("objname 'kevin' given but")); EXPECT_THAT(error.data(), HasSubstr("line 4")); } @@ -3642,12 +3566,11 @@ TEST_F(SensorParseTest, UserNeedstageAcc) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->sensor_needstage[0], mjSTAGE_VEL); EXPECT_EQ(model->sensor_needstage[1], mjSTAGE_ACC); EXPECT_EQ(model->sensor_needstage[2], mjSTAGE_POS); - mj_deleteModel(model); } // ------------- test general parsing ------------------------------------------ @@ -3661,8 +3584,8 @@ TEST_F(XMLReaderTest, ZnearZeroNotAllowed) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("znear must be strictly positive")); EXPECT_THAT(error.data(), HasSubstr("line 4")); } @@ -3676,8 +3599,8 @@ TEST_F(XMLReaderTest, ZnearNegativeNotAllowed) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("znear must be strictly positive")); EXPECT_THAT(error.data(), HasSubstr("line 4")); } @@ -3689,8 +3612,8 @@ TEST_F(XMLReaderTest, ExtentZeroNotAllowed) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("extent must be strictly positive")); EXPECT_THAT(error.data(), HasSubstr("line 3")); } @@ -3702,8 +3625,8 @@ TEST_F(XMLReaderTest, ExtentNegativeNotAllowed) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("extent must be strictly positive")); EXPECT_THAT(error.data(), HasSubstr("line 3")); } @@ -3724,12 +3647,11 @@ TEST_F(XMLReaderTest, LightRadius) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_FLOAT_EQ(model->light_bulbradius[0], 0.02); EXPECT_FLOAT_EQ(model->light_bulbradius[1], 1); EXPECT_FLOAT_EQ(model->light_bulbradius[2], 2); - mj_deleteModel(model); } TEST_F(XMLReaderTest, CameraOutput) { @@ -3743,14 +3665,13 @@ TEST_F(XMLReaderTest, CameraOutput) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->ncam, 3); EXPECT_EQ(model->cam_output[0], mjCAMOUT_RGB); // default EXPECT_EQ(model->cam_output[1], mjCAMOUT_DEPTH); EXPECT_EQ(model->cam_output[2], mjCAMOUT_RGB | mjCAMOUT_NORMAL | mjCAMOUT_SEG); - mj_deleteModel(model); } TEST_F(XMLReaderTest, CameraOutputDefault) { @@ -3769,13 +3690,12 @@ TEST_F(XMLReaderTest, CameraOutputDefault) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->ncam, 3); EXPECT_EQ(model->cam_output[0], mjCAMOUT_RGB); // main default EXPECT_EQ(model->cam_output[1], mjCAMOUT_DEPTH | mjCAMOUT_DIST); EXPECT_EQ(model->cam_output[2], mjCAMOUT_NORMAL); - mj_deleteModel(model); } // ------------- test delay attribute parsing ---------------------------------- @@ -3800,8 +3720,8 @@ TEST_F(ActuatorParseTest, ActuatorDelayParsed) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); ASSERT_EQ(model->nu, 3); // actuator_history[2*i] = nsample, actuator_history[2*i+1] = interp EXPECT_EQ(model->actuator_history[0], 0); // jnt1 nsample @@ -3813,7 +3733,6 @@ TEST_F(ActuatorParseTest, ActuatorDelayParsed) { EXPECT_EQ(model->actuator_historyadr[0], -1); EXPECT_EQ(model->actuator_historyadr[1], 0); EXPECT_EQ(model->actuator_historyadr[2], 8); // 2+2*3 = 8 - mj_deleteModel(model); } TEST_F(ActuatorParseTest, ActuatorDelayDefault) { @@ -3837,14 +3756,13 @@ TEST_F(ActuatorParseTest, ActuatorDelayDefault) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); ASSERT_EQ(model->nu, 2); EXPECT_EQ(model->actuator_history[0], 5); EXPECT_EQ(model->actuator_history[2], 0); EXPECT_EQ(model->actuator_historyadr[0], 0); EXPECT_EQ(model->actuator_historyadr[1], -1); - mj_deleteModel(model); } TEST_F(ActuatorParseTest, ActuatorDelayRequiresHistory) { @@ -3862,8 +3780,8 @@ TEST_F(ActuatorParseTest, ActuatorDelayRequiresHistory) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, IsNull()); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), IsNull()); EXPECT_THAT(error.data(), HasSubstr("setting delay > 0 without a history buffer")); } @@ -3885,11 +3803,10 @@ TEST_F(ActuatorParseTest, DampingArmatureDefaultsPropagate) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); EXPECT_EQ(model->actuator_damping[0], 3); EXPECT_EQ(model->actuator_armature[0], 0.5); - mj_deleteModel(model); } TEST_F(XMLReaderTest, AttachConflictXMLWarning) { diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index c8e2386e..3cc634a0 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -48,11 +48,10 @@ static const char* const kNonRgbTextureXMLPath = TEST_F(XMLWriterTest, EmptyModel) { static constexpr char xml[] = ""; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("default"))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, SavesMemory) { @@ -62,11 +61,10 @@ TEST_F(XMLWriterTest, SavesMemory) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("memory=\"1023\"")); - mj_deleteModel(model); } { static constexpr char xml[] = R"( @@ -74,11 +72,10 @@ TEST_F(XMLWriterTest, SavesMemory) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("memory=\"1K\"")); - mj_deleteModel(model); } { static constexpr char xml[] = R"( @@ -86,11 +83,10 @@ TEST_F(XMLWriterTest, SavesMemory) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("memory=\"4K\"")); - mj_deleteModel(model); } { static constexpr char xml[] = R"( @@ -98,11 +94,10 @@ TEST_F(XMLWriterTest, SavesMemory) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("memory=\"1M\"")); - mj_deleteModel(model); } { static constexpr char xml[] = R"( @@ -110,11 +105,10 @@ TEST_F(XMLWriterTest, SavesMemory) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("memory=\"1023K\"")); - mj_deleteModel(model); } } @@ -126,10 +120,9 @@ TEST_F(XMLWriterTest, SavesDisableSensor) { )"; - mjModel* model = LoadModelFromString(xml); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("sensor=\"disable\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, SavesInertial) { @@ -143,10 +136,9 @@ TEST_F(XMLWriterTest, SavesInertial) { )"; - mjModel* model = LoadModelFromString(xml); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("mass=\"1\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, EmptyUserSensor) { @@ -157,14 +149,13 @@ TEST_F(XMLWriterTest, EmptyUserSensor) { )"; - mjModel* model = LoadModelFromString(xml); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("dim=\"2\"")); EXPECT_THAT(saved_xml, HasSubstr("needstage=\"vel\"")); EXPECT_THAT(saved_xml, HasSubstr("datatype=\"real\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("objtype"))); EXPECT_THAT(saved_xml, Not(HasSubstr("objname"))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsEmptyClasses) { @@ -183,12 +174,11 @@ TEST_F(XMLWriterTest, KeepsEmptyClasses) { )"; - mjModel* model = LoadModelFromString(xml); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("default class=\"regular\"")); EXPECT_THAT(saved_xml, HasSubstr("default class=\"empty_referenced\"")); EXPECT_THAT(saved_xml, HasSubstr("default class=\"empty_unreferenced\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsExplicitInertial) { @@ -202,10 +192,9 @@ TEST_F(XMLWriterTest, KeepsExplicitInertial) { )"; - mjModel* model = LoadModelFromString(xml); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr(" )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("inertial"))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsBoundMassInertia) { @@ -231,12 +219,11 @@ TEST_F(XMLWriterTest, KeepsBoundMassInertia) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("boundmass=\"0.1\"")); EXPECT_THAT(saved_xml, HasSubstr("boundinertia=\"0.2\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, DropsZeroBoundMassInertia) { @@ -245,12 +232,11 @@ TEST_F(XMLWriterTest, DropsZeroBoundMassInertia) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("boundmass"))); EXPECT_THAT(saved_xml, Not(HasSubstr("boundinertia"))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, DropsInertialIfFromGeom) { @@ -265,11 +251,10 @@ TEST_F(XMLWriterTest, DropsInertialIfFromGeom) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("inertial"))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsAutoLimitsFalse) { @@ -278,11 +263,10 @@ TEST_F(XMLWriterTest, KeepsAutoLimitsFalse) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("autolimits=\"false\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, DoesNotKeepInferredJointLimited) { @@ -297,13 +281,12 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredJointLimited) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("range=\"-1 1\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("limited=\"true\""))); EXPECT_THAT(saved_xml, Not(HasSubstr("autolimits=\"true\""))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsExplicitJointLimited) { @@ -318,13 +301,12 @@ TEST_F(XMLWriterTest, KeepsExplicitJointLimited) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("autolimits=\"true\""))); EXPECT_THAT(saved_xml, HasSubstr("range=\"-1 1\"")); EXPECT_THAT(saved_xml, HasSubstr("limited=\"true\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsJointLimitedFalseIfAutoLimits) { @@ -339,11 +321,10 @@ TEST_F(XMLWriterTest, KeepsJointLimitedFalseIfAutoLimits) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("limited=\"false\" range=\"-1 1\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, DoesNotKeepInferredTendonLimited) { @@ -366,13 +347,12 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredTendonLimited) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("autolimits=\"true\""))); EXPECT_THAT(saved_xml, HasSubstr("range=\"-1 1\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("limited=\"true\""))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsExplicitTendonLimitedIfAutoLimits) { @@ -395,13 +375,12 @@ TEST_F(XMLWriterTest, KeepsExplicitTendonLimitedIfAutoLimits) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("autolimits=\"true\""))); EXPECT_THAT(saved_xml, HasSubstr("range=\"-1 1\"")); EXPECT_THAT(saved_xml, HasSubstr("limited=\"true\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsTendonLimitedFalseIfAutoLimits) { @@ -424,11 +403,10 @@ TEST_F(XMLWriterTest, KeepsTendonLimitedFalseIfAutoLimits) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("limited=\"false\" range=\"-1 1\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, DoesNotKeepInferredActlimited) { @@ -446,13 +424,12 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredActlimited) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("autolimits=\"true\""))); EXPECT_THAT(saved_xml, HasSubstr("actrange=\"-1 1\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("actlimited=\"true\""))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsExplicitActlimitedIfAutoLimits) { @@ -470,13 +447,12 @@ TEST_F(XMLWriterTest, KeepsExplicitActlimitedIfAutoLimits) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("autolimits=\"true\""))); EXPECT_THAT(saved_xml, HasSubstr("actrange=\"-1 1\"")); EXPECT_THAT(saved_xml, HasSubstr("actlimited=\"true\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsActlimitedFalse) { @@ -494,11 +470,10 @@ TEST_F(XMLWriterTest, KeepsActlimitedFalse) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("actlimited=\"false\" actrange=\"-1 1\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, DoesNotKeepInferredCtrllimited) { @@ -516,12 +491,11 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredCtrllimited) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("ctrlrange=\"-1 1\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("ctrllimited=\"true\""))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsExplicitCtrllimitedIfAutoLimits) { @@ -539,12 +513,11 @@ TEST_F(XMLWriterTest, KeepsExplicitCtrllimitedIfAutoLimits) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("ctrlrange=\"-1 1\"")); EXPECT_THAT(saved_xml, HasSubstr("ctrllimited=\"true\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsCtrllimitedFalse) { @@ -561,11 +534,10 @@ TEST_F(XMLWriterTest, KeepsCtrllimitedFalse) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("ctrllimited=\"false\" ctrlrange=\"-1 1\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, DoesNotKeepInferredForcelimited) { @@ -583,13 +555,12 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredForcelimited) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("autolimits=\"true\""))); EXPECT_THAT(saved_xml, HasSubstr("forcerange=\"-1 1\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("forcelimited=\"true\""))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsExplicitForcelimited) { @@ -606,13 +577,12 @@ TEST_F(XMLWriterTest, KeepsExplicitForcelimited) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("autolimits=\"true\""))); EXPECT_THAT(saved_xml, HasSubstr("forcerange=\"-1 1\"")); EXPECT_THAT(saved_xml, HasSubstr("forcelimited=\"true\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, KeepsForcelimitedFalse) { @@ -629,12 +599,11 @@ TEST_F(XMLWriterTest, KeepsForcelimitedFalse) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("forcelimited=\"false\" forcerange=\"-1 1\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, WritesGravComp) { @@ -646,12 +615,11 @@ TEST_F(XMLWriterTest, WritesGravComp) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("gravcomp=\"0.25\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("gravcomp=\"0\""))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, WritesBodyPos) { @@ -663,12 +631,11 @@ TEST_F(XMLWriterTest, WritesBodyPos) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("pos=\"1 2 3\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("pos=\"0 0 0\""))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, UndefinedMassDensity) { @@ -681,12 +648,11 @@ TEST_F(XMLWriterTest, UndefinedMassDensity) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("density"))); EXPECT_THAT(saved_xml, Not(HasSubstr("mass"))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, WritesDefaults) { @@ -702,12 +668,11 @@ TEST_F(XMLWriterTest, WritesDefaults) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("mass"))); EXPECT_THAT(saved_xml, HasSubstr("")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, WritesActuatorDefaults) { @@ -733,13 +698,12 @@ TEST_F(XMLWriterTest, WritesActuatorDefaults) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("mass"))); EXPECT_THAT(saved_xml, HasSubstr( " error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()) << error.data(); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model.get(), NotNull()) << error.data(); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_STREQ(saved_xml.c_str(), xml_expected); - mj_deleteModel(model); } TEST_F(XMLWriterTest, WritesDensity) { @@ -826,12 +789,11 @@ TEST_F(XMLWriterTest, WritesDensity) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("density=\"100\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("mass"))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, WritesMass) { @@ -844,12 +806,11 @@ TEST_F(XMLWriterTest, WritesMass) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("density"))); EXPECT_THAT(saved_xml, HasSubstr("mass=\"0.1\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, ZeroMass) { @@ -862,12 +823,11 @@ TEST_F(XMLWriterTest, ZeroMass) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("density"))); EXPECT_THAT(saved_xml, HasSubstr("mass=\"0\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, OverwritesDensity) { @@ -880,12 +840,11 @@ TEST_F(XMLWriterTest, OverwritesDensity) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("density"))); EXPECT_THAT(saved_xml, HasSubstr("mass=\"100\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, SaveDefaultMass) { @@ -906,11 +865,10 @@ TEST_F(XMLWriterTest, SaveDefaultMass) { )"; - mjModel* model = LoadModelFromString(xml); - EXPECT_THAT(model, NotNull()); - std::string content = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + EXPECT_THAT(model.get(), NotNull()); + std::string content = SaveAndReadXml(model.get()); EXPECT_THAT(content, HasSubstr("mass=\"1\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, UsesTwoSpaces) { @@ -920,12 +878,11 @@ TEST_F(XMLWriterTest, UsesTwoSpaces) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr(" ")); EXPECT_THAT(saved_xml, Not(HasSubstr(" "))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, WritesSkin) { @@ -968,18 +925,15 @@ TEST_F(XMLWriterTest, WritesSkin) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); EXPECT_THAT(model->nskin, 1); char error[1024]; - mjModel* mtemp = LoadModelFromString(SaveAndReadXml(model), + MjModelPtr mtemp = LoadModelFromString(SaveAndReadXml(model.get()), error, sizeof(error)); - ASSERT_THAT(mtemp, NotNull()) << error; + ASSERT_THAT(mtemp.get(), NotNull()) << error; EXPECT_THAT(mtemp->nskin, 1); - - mj_deleteModel(model); - mj_deleteModel(mtemp); } TEST_F(XMLWriterTest, WritesHfield) { @@ -994,8 +948,8 @@ TEST_F(XMLWriterTest, WritesHfield) { )"; // load model - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); int size = model->hfield_nrow[0]*model->hfield_ncol[0]; EXPECT_EQ(size, 6); @@ -1004,12 +958,11 @@ TEST_F(XMLWriterTest, WritesHfield) { ElementsAre(.8, 1, .4, .6, 0, .2)); // save and read, compare data - mjModel* mtemp = LoadModelFromString(SaveAndReadXml(model)); - ASSERT_THAT(mtemp, NotNull()); + MjModelPtr mtemp = LoadModelFromString(SaveAndReadXml(model.get())); + ASSERT_THAT(mtemp.get(), NotNull()); for (int i = 0; i < size; ++i) { EXPECT_THAT(mtemp->hfield_data[i], FloatEq(model->hfield_data[i])); } - mj_deleteModel(mtemp); // modify data, save read and compare model->hfield_data[0] = 0.25; @@ -1019,14 +972,11 @@ TEST_F(XMLWriterTest, WritesHfield) { model->hfield_data[4] = 1.0; model->hfield_data[5] = 0.0; - mtemp = LoadModelFromString(SaveAndReadXml(model)); - ASSERT_THAT(mtemp, NotNull()); + mtemp = LoadModelFromString(SaveAndReadXml(model.get())); + ASSERT_THAT(mtemp.get(), NotNull()); for (int i = 0; i < size; ++i) { EXPECT_THAT(mtemp->hfield_data[i], FloatEq(model->hfield_data[i])); } - mj_deleteModel(mtemp); - - mj_deleteModel(model); } TEST_F(XMLWriterTest, WritesLight) { @@ -1044,18 +994,15 @@ TEST_F(XMLWriterTest, WritesLight) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); // save and read, compare data - mjModel* mtemp = LoadModelFromString(SaveAndReadXml(model)); + MjModelPtr mtemp = LoadModelFromString(SaveAndReadXml(model.get())); EXPECT_EQ(mtemp->nlight, 3); EXPECT_FLOAT_EQ(mtemp->light_bulbradius[0], 0.02); EXPECT_FLOAT_EQ(mtemp->light_bulbradius[1], 1); EXPECT_FLOAT_EQ(mtemp->light_bulbradius[2], 2); - - mj_deleteModel(mtemp); - mj_deleteModel(model); } TEST_F(XMLWriterTest, WritesMaterial) { @@ -1073,19 +1020,16 @@ TEST_F(XMLWriterTest, WritesMaterial) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); // save and read, compare data - mjModel* mtemp = LoadModelFromString(SaveAndReadXml(model)); + MjModelPtr mtemp = LoadModelFromString(SaveAndReadXml(model.get())); EXPECT_EQ(mtemp->nmat, 2); EXPECT_EQ(mtemp->mat_metallic[0], 2); EXPECT_EQ(mtemp->mat_metallic[1], 4); EXPECT_EQ(mtemp->mat_roughness[0], 3); EXPECT_EQ(mtemp->mat_roughness[1], 5); - - mj_deleteModel(mtemp); - mj_deleteModel(model); } TEST_F(XMLWriterTest, SpringlengthOneValue) { @@ -1104,11 +1048,10 @@ TEST_F(XMLWriterTest, SpringlengthOneValue) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("springlength=\"0.5\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, SpringlengthTwoValues) { @@ -1127,11 +1070,10 @@ TEST_F(XMLWriterTest, SpringlengthTwoValues) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("springlength=\"0 0.5\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, Actdim) { @@ -1150,13 +1092,12 @@ TEST_F(XMLWriterTest, Actdim) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("actdim=\"2\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("actdim=\"1\""))); EXPECT_THAT(saved_xml, Not(HasSubstr("actdim=\"0\""))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, ActdimDefaults) { @@ -1176,11 +1117,10 @@ TEST_F(XMLWriterTest, ActdimDefaults) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("actdim=\"2\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, TrimsDefaults) { @@ -1201,25 +1141,23 @@ TEST_F(XMLWriterTest, TrimsDefaults) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("friction=\"0.9\"")); EXPECT_THAT(saved_xml, HasSubstr("solref=\"0.1\"")); EXPECT_THAT(saved_xml, HasSubstr("solimp=\"0.1 0.2\"")); EXPECT_THAT(saved_xml, HasSubstr("gainprm=\"3\" biasprm=\"0 -3 -4\"")); EXPECT_THAT(saved_xml, HasSubstr("gainprm=\"0 0 -10\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, DoesntSaveGlobal) { static constexpr char xml[] = ""; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("global"))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, InheritrangeSavedAsRange) { @@ -1242,12 +1180,11 @@ TEST_F(XMLWriterTest, InheritrangeSavedAsRange) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("ctrlrange=\"-1 3\"")); EXPECT_THAT(saved_xml, HasSubstr("actrange=\"-1.5 -0.5\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, InheritedInheritrangeSavedAsRange) { @@ -1273,13 +1210,12 @@ TEST_F(XMLWriterTest, InheritedInheritrangeSavedAsRange) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("inheritrange"))); EXPECT_THAT(saved_xml, HasSubstr("ctrlrange=\"-1 3\"")); EXPECT_THAT(saved_xml, HasSubstr("actrange=\"0.5 1.5\"")); - mj_deleteModel(model); } // check that no precision is lost when saving XMLs with FullFloatPrecision @@ -1291,12 +1227,12 @@ TEST_F(XMLWriterTest, SetPrecision) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); // save to XML and re-load, expect to lose precision - mjModel* model_lo = LoadModelFromString(SaveAndReadXml(model)); - ASSERT_THAT(model_lo, NotNull()); + MjModelPtr model_lo = LoadModelFromString(SaveAndReadXml(model.get())); + ASSERT_THAT(model_lo.get(), NotNull()); EXPECT_EQ(model->geom_size[1], model_lo->geom_size[1]); EXPECT_NE(model->geom_size[2], model_lo->geom_size[2]); @@ -1304,12 +1240,9 @@ TEST_F(XMLWriterTest, SetPrecision) { // save to XML and reload with FullFloatPrecision // expect to maintain precision FullFloatPrecision increase_precision; - mjModel* model_hi = LoadModelFromString(SaveAndReadXml(model)); + MjModelPtr model_hi = LoadModelFromString(SaveAndReadXml(model.get())); EXPECT_EQ(model->geom_size[2], model_hi->geom_size[2]); - mj_deleteModel(model_hi); } - mj_deleteModel(model_lo); - mj_deleteModel(model); } class XMLWriterLocaleTest : public MujocoTest { @@ -1341,11 +1274,10 @@ TEST_F(XMLWriterLocaleTest, IgnoresLocale) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("0.1 1.23 2.345")); - mj_deleteModel(model); // Test that MuJoCo doesn't override locales for subsequent calls. char formatted[8]; @@ -1381,8 +1313,8 @@ TEST_F(DecompilerTest, SavesStatistics) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); model->stat.meansize = 7; model->stat.extent = 8; model->stat.center[0] = 9; @@ -1390,13 +1322,12 @@ TEST_F(DecompilerTest, SavesStatistics) { model->stat.center[2] = 11; model->stat.meanmass = 12; model->stat.meaninertia = 13; - std::string saved_xml = SaveAndReadXml(model); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("meansize=\"7\"")); EXPECT_THAT(saved_xml, HasSubstr("extent=\"8\"")); EXPECT_THAT(saved_xml, HasSubstr("center=\"9 10 11\"")); EXPECT_THAT(saved_xml, HasSubstr("meanmass=\"12\"")); EXPECT_THAT(saved_xml, HasSubstr("meaninertia=\"13\"")); - mj_deleteModel(model); } TEST_F(DecompilerTest, SaveAndReadXml) { @@ -1418,11 +1349,11 @@ TEST_F(DecompilerTest, SaveAndReadXml) { )"; std::array error; - mjModel* m1 = LoadModelFromString(xml1, error.data(), error.size()); - ASSERT_THAT(m1, NotNull()) << error.data(); + MjModelPtr m1 = LoadModelFromString(xml1, error.data(), error.size()); + ASSERT_THAT(m1.get(), NotNull()) << error.data(); m1->geom_size[0] = 10; m1->geom_size[3] = 20; - std::string saved_xml = SaveAndReadXml(m1); + std::string saved_xml = SaveAndReadXml(m1.get()); EXPECT_THAT(saved_xml, HasSubstr("geom size=\"10\"")); EXPECT_THAT(saved_xml, HasSubstr("geom size=\"20\"")); @@ -1439,7 +1370,7 @@ TEST_F(DecompilerTest, SaveAndReadXml) { m2->geom_size[0] = .1; m2->geom_size[3] = .2; m2->geom_size[6] = .3; - EXPECT_EQ(mj_copyBack(spec, m1), 0); + EXPECT_EQ(mj_copyBack(spec, m1.get()), 0); EXPECT_THAT(mjs_getError(spec), HasSubstr("CopyBack")); EXPECT_EQ(mj_copyBack(spec, m2), 1); std::string saved_xml2 = SaveAndReadXml(spec); @@ -1454,7 +1385,6 @@ TEST_F(DecompilerTest, SaveAndReadXml) { EXPECT_THAT(saved_xml3, Not(HasSubstr("geom size=\"0.3\""))); mj_deleteSpec(spec); - mj_deleteModel(m1); mj_deleteModel(m2); } @@ -1468,16 +1398,15 @@ TEST_F(DecompilerTest, DoesntSaveInferredStatistics) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("meansize"))); EXPECT_THAT(saved_xml, Not(HasSubstr("meanmass"))); EXPECT_THAT(saved_xml, Not(HasSubstr("meaninertia"))); EXPECT_THAT(saved_xml, Not(HasSubstr("center"))); EXPECT_THAT(saved_xml, Not(HasSubstr("extent"))); EXPECT_THAT(saved_xml, Not(HasSubstr("statistic"))); - mj_deleteModel(model); } TEST_F(DecompilerTest, VeryLargeNumbers) { @@ -1494,14 +1423,13 @@ TEST_F(DecompilerTest, VeryLargeNumbers) { )"; std::array error; - mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); + std::string saved_xml = SaveAndReadXml(model.get()); // note, focal is float and loses precision 16777217 -> 16777216 EXPECT_THAT(saved_xml, HasSubstr("focal=\"16777216 1\"")); EXPECT_THAT(saved_xml, HasSubstr("pos=\"1e+20 0 0\"")); EXPECT_THAT(saved_xml, HasSubstr("range=\"-1e+10 1e+10\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, ExpandAttach) { @@ -1541,12 +1469,11 @@ TEST_F(XMLWriterTest, ExpandAttach) { mj_addBufferVFS(vfs.get(), "b.xml", xml_child, sizeof(xml_child)); std::array er; - mjModel* m = LoadModelFromString(xml_parent, er.data(), er.size(), vfs.get()); - ASSERT_THAT(m, NotNull()) << er.data(); + MjModelPtr m = LoadModelFromString(xml_parent, er.data(), er.size(), vfs.get()); + ASSERT_THAT(m.get(), NotNull()) << er.data(); - std::string saved_xml = SaveAndReadXml(m); + std::string saved_xml = SaveAndReadXml(m.get()); EXPECT_THAT(saved_xml, HasSubstr("class=\"bb\"")); - mj_deleteModel(m); mj_deleteVFS(vfs.get()); } @@ -1559,14 +1486,13 @@ TEST_F(XMLWriterTest, WritesCameraOutput) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); // default output="rgb" should not be written EXPECT_THAT(saved_xml, Not(HasSubstr("output=\"rgb\""))); // non-default output should be written EXPECT_THAT(saved_xml, HasSubstr("output=\"depth normal\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, WritesCameraOutputDefault) { @@ -1584,18 +1510,16 @@ TEST_F(XMLWriterTest, WritesCameraOutputDefault) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); // save and reload to verify round-trip - mjModel* mtemp = LoadModelFromString(saved_xml); - ASSERT_THAT(mtemp, NotNull()); + MjModelPtr mtemp = LoadModelFromString(saved_xml); + ASSERT_THAT(mtemp.get(), NotNull()); EXPECT_EQ(mtemp->ncam, 3); EXPECT_EQ(mtemp->cam_output[0], mjCAMOUT_RGB); EXPECT_EQ(mtemp->cam_output[1], mjCAMOUT_DEPTH | mjCAMOUT_DIST); EXPECT_EQ(mtemp->cam_output[2], mjCAMOUT_SEG); - mj_deleteModel(mtemp); - mj_deleteModel(model); } TEST_F(XMLWriterTest, WritesSensorDelayIntervalHistory) { @@ -1612,14 +1536,13 @@ TEST_F(XMLWriterTest, WritesSensorDelayIntervalHistory) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("delay=\"0.05\"")); EXPECT_THAT(saved_xml, HasSubstr("interval=\"0.1 -0.02\"")); EXPECT_THAT(saved_xml, HasSubstr("nsample=\"20\"")); EXPECT_THAT(saved_xml, HasSubstr("interp=\"linear\"")); - mj_deleteModel(model); } TEST_F(XMLWriterTest, DoesNotWriteDefaultSensorAttributes) { @@ -1636,14 +1559,13 @@ TEST_F(XMLWriterTest, DoesNotWriteDefaultSensorAttributes) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, Not(HasSubstr("delay="))); EXPECT_THAT(saved_xml, Not(HasSubstr("interval="))); EXPECT_THAT(saved_xml, Not(HasSubstr("nsample="))); EXPECT_THAT(saved_xml, Not(HasSubstr("interp="))); - mj_deleteModel(model); } TEST_F(XMLWriterTest, WritesActuatorDelayHistory) { @@ -1660,13 +1582,12 @@ TEST_F(XMLWriterTest, WritesActuatorDelayHistory) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); - std::string saved_xml = SaveAndReadXml(model); + MjModelPtr model = LoadModelFromString(xml); + ASSERT_THAT(model.get(), NotNull()); + std::string saved_xml = SaveAndReadXml(model.get()); EXPECT_THAT(saved_xml, HasSubstr("delay=\"0.03\"")); EXPECT_THAT(saved_xml, HasSubstr("nsample=\"15\"")); EXPECT_THAT(saved_xml, HasSubstr("interp=\"cubic\"")); - mj_deleteModel(model); } } // namespace diff --git a/test/xml/xml_urdf_test.cc b/test/xml/xml_urdf_test.cc index f029ba4d..7a134b8e 100644 --- a/test/xml/xml_urdf_test.cc +++ b/test/xml/xml_urdf_test.cc @@ -45,9 +45,8 @@ TEST_F(MujocoTest, ReadsCapsule) { )"; std::array error; - mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); - mj_deleteModel(model); + MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); } TEST_F(MujocoTest, ReadsGeomNames) { @@ -74,19 +73,17 @@ TEST_F(MujocoTest, ReadsGeomNames) { )"; std::array error; - mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); // 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); 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); EXPECT_EQ(model->geom_type[visual_sphere_id], mjtGeom::mjGEOM_SPHERE); - - mj_deleteModel(model); } TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesCollisionBeforeVisual) { @@ -113,16 +110,14 @@ TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesCollisionBeforeVisual) { )"; std::array error; - mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); // Check the collision geom gets its name from the URDF. The visual sphere // 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); EXPECT_EQ(model->geom_type[collision_box_id], mjtGeom::mjGEOM_BOX); - - mj_deleteModel(model); } TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesVisualBeforeCollision) { @@ -149,16 +144,14 @@ TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesVisualBeforeCollision) { )"; std::array error; - mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); // Check the visual geom gets its name from the URDF. The collision geom // 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); EXPECT_EQ(model->geom_type[visual_sphere_id], mjtGeom::mjGEOM_SPHERE); - - mj_deleteModel(model); } TEST_F(MujocoTest, ReadsJointTypes) { @@ -226,8 +219,8 @@ TEST_F(MujocoTest, ReadsJointTypes) { )"; std::array error; - mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); - ASSERT_THAT(model, NotNull()) << error.data(); + MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size()); + ASSERT_THAT(model.get(), NotNull()) << error.data(); constexpr float eps = 1e-6; @@ -241,14 +234,12 @@ TEST_F(MujocoTest, ReadsJointTypes) { {0.0, 0.0, 1.0} }; 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_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 + 2], expected_axis[i][2], eps); } - - mj_deleteModel(model); } TEST_F(MujocoTest, RepeatedMeshName) {