Change LoadModelFromString to return a smart pointer, add MakeData, and update tests to have C++ RAII clean up model and data.
PiperOrigin-RevId: 935980153 Change-Id: I41d25bfab4935494dc984168820cb7cad123cadf
This commit is contained in:
committed by
Copybara-Service
parent
34d142ee50
commit
1490336955
@@ -120,8 +120,7 @@ void BM_CompileManyTextures(benchmark::State& state) {
|
|||||||
|
|
||||||
for (auto s : state) {
|
for (auto s : state) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Reset cache capacity to default value.
|
// Reset cache capacity to default value.
|
||||||
|
|||||||
@@ -72,8 +72,8 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
|
|||||||
g2 = g2new;
|
g2 = g2new;
|
||||||
|
|
||||||
// call low-level box-box collider
|
// call low-level box-box collider
|
||||||
int num = mjc_BoxBox(model, data, precon.data(), g1, g2,
|
int num =
|
||||||
con->includemargin);
|
mjc_BoxBox(model, data, precon.data(), g1, g2, con->includemargin);
|
||||||
|
|
||||||
// allocate and clear arrays marking already matched contacts
|
// allocate and clear arrays marking already matched contacts
|
||||||
mju_zeroInt(match_raw.data(), num);
|
mju_zeroInt(match_raw.data(), num);
|
||||||
@@ -83,8 +83,7 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
|
|||||||
int nmatched = 0;
|
int nmatched = 0;
|
||||||
for (int i = 0; i < num; i++) {
|
for (int i = 0; i < num; i++) {
|
||||||
for (int j = 0; j < data->ncon; j++) {
|
for (int j = 0; j < data->ncon; j++) {
|
||||||
if (!match[j] &&
|
if (!match[j] && precon[i].pos[0] == data->contact[j].pos[0] &&
|
||||||
precon[i].pos[0] == data->contact[j].pos[0] &&
|
|
||||||
precon[i].pos[1] == data->contact[j].pos[1] &&
|
precon[i].pos[1] == data->contact[j].pos[1] &&
|
||||||
precon[i].pos[2] == data->contact[j].pos[2]) {
|
precon[i].pos[2] == data->contact[j].pos[2]) {
|
||||||
match_raw[i] = match[j] = 1;
|
match_raw[i] = match[j] = 1;
|
||||||
@@ -110,17 +109,17 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
|
|||||||
if (!match_raw[i]) {
|
if (!match_raw[i]) {
|
||||||
// === check if outside
|
// === check if outside
|
||||||
|
|
||||||
mjtNum sz1[3] = {size1[0]+margin, size1[1]+margin, size1[2]+margin};
|
mjtNum sz1[3] = {size1[0] + margin, size1[1] + margin,
|
||||||
mjtNum sz2[3] = {size2[0]+margin, size2[1]+margin, size2[2]+margin};
|
size1[2] + margin};
|
||||||
|
mjtNum sz2[3] = {size2[0] + margin, size2[1] + margin,
|
||||||
|
size2[2] + margin};
|
||||||
|
|
||||||
// relative distance (1%) outside of which contacts are removed
|
// relative distance (1%) outside of which contacts are removed
|
||||||
static mjtNum kRatio = 1.01;
|
static mjtNum kRatio = 1.01;
|
||||||
|
|
||||||
// is the contact outside: 1, inside: -1, within the removal width: 0
|
// is the contact outside: 1, inside: -1, within the removal width: 0
|
||||||
int out1 = mju_outsideBox(precon[i].pos, pos1, mat1, sz1,
|
int out1 = mju_outsideBox(precon[i].pos, pos1, mat1, sz1, kRatio);
|
||||||
kRatio);
|
int out2 = mju_outsideBox(precon[i].pos, pos2, mat2, sz2, kRatio);
|
||||||
int out2 = mju_outsideBox(precon[i].pos, pos2, mat2, sz2,
|
|
||||||
kRatio);
|
|
||||||
|
|
||||||
// mark as bad if outside one box and not inside the other box
|
// mark as bad if outside one box and not inside the other box
|
||||||
bool outside = (out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1);
|
bool outside = (out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1);
|
||||||
@@ -151,7 +150,6 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
|
|||||||
std::vector<int> match_raw(mjMAXCONPAIR);
|
std::vector<int> match_raw(mjMAXCONPAIR);
|
||||||
std::vector<int> match(data->ncon);
|
std::vector<int> match(data->ncon);
|
||||||
|
|
||||||
|
|
||||||
int g1 = -1;
|
int g1 = -1;
|
||||||
int g2 = -1;
|
int g2 = -1;
|
||||||
for (int c = 0; c < data->ncon; c++) {
|
for (int c = 0; c < data->ncon; c++) {
|
||||||
@@ -174,8 +172,8 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
|
|||||||
g2 = g2new;
|
g2 = g2new;
|
||||||
|
|
||||||
// call low-level box-box collider
|
// call low-level box-box collider
|
||||||
int num = mjc_BoxBox(model, data, precon.data(), g1, g2,
|
int num =
|
||||||
con->includemargin);
|
mjc_BoxBox(model, data, precon.data(), g1, g2, con->includemargin);
|
||||||
|
|
||||||
// allocate and clear arrays marking already matched contacts
|
// allocate and clear arrays marking already matched contacts
|
||||||
mju_zeroInt(match_raw.data(), num);
|
mju_zeroInt(match_raw.data(), num);
|
||||||
@@ -185,8 +183,7 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
|
|||||||
int nmatched = 0;
|
int nmatched = 0;
|
||||||
for (int i = 0; i < num; i++) {
|
for (int i = 0; i < num; i++) {
|
||||||
for (int j = 0; j < data->ncon; j++) {
|
for (int j = 0; j < data->ncon; j++) {
|
||||||
if (!match[j] &&
|
if (!match[j] && precon[i].pos[0] == data->contact[j].pos[0] &&
|
||||||
precon[i].pos[0] == data->contact[j].pos[0] &&
|
|
||||||
precon[i].pos[1] == data->contact[j].pos[1] &&
|
precon[i].pos[1] == data->contact[j].pos[1] &&
|
||||||
precon[i].pos[2] == data->contact[j].pos[2]) {
|
precon[i].pos[2] == data->contact[j].pos[2]) {
|
||||||
match_raw[i] = match[j] = 1;
|
match_raw[i] = match[j] = 1;
|
||||||
@@ -255,20 +252,17 @@ TEST_F(MjCollisionBoxTest, BoxSphere) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
for (mjtNum z : {-.015, -.00501, -.005, -.00499, 0.0, 0.004}) {
|
for (mjtNum z : {-.015, -.00501, -.005, -.00499, 0.0, 0.004}) {
|
||||||
data->qpos[2] = z;
|
data->qpos[2] = z;
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
EXPECT_EQ(data->ncon, 2);
|
EXPECT_EQ(data->ncon, 2);
|
||||||
EXPECT_THAT(data->contact[0].dist,
|
EXPECT_THAT(data->contact[0].dist,
|
||||||
MjNear(data->contact[1].dist, 1e-8, 1e-6));
|
MjNear(data->contact[1].dist, 1e-8, 1e-6));
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) {
|
TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) {
|
||||||
@@ -281,22 +275,19 @@ TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
mjPreContact precon[mjMAXCONPAIR];
|
mjPreContact precon[mjMAXCONPAIR];
|
||||||
|
|
||||||
for (mjfCollision collision : {mjc_BoxBox, mjc_Convex}) {
|
for (mjfCollision collision : {mjc_BoxBox, mjc_Convex}) {
|
||||||
int n = collision(model, data, precon, 0, 1, 0.0);
|
int n = collision(model.get(), data.get(), precon, 0, 1, 0.0);
|
||||||
for (int i = 0; i < n; i++) {
|
for (int i = 0; i < n; i++) {
|
||||||
EXPECT_NEAR(precon[i].dist, -0.5, MjTol(1e-8, 1e-6));
|
EXPECT_NEAR(precon[i].dist, -0.5, MjTol(1e-8, 1e-6));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
@@ -14,6 +14,8 @@
|
|||||||
|
|
||||||
// Tests for engine/engine_collision_driver.c.
|
// Tests for engine/engine_collision_driver.c.
|
||||||
|
|
||||||
|
#include "src/engine/engine_collision_driver.h"
|
||||||
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <cstddef>
|
#include <cstddef>
|
||||||
@@ -26,22 +28,20 @@
|
|||||||
#include <mujoco/mjmodel.h>
|
#include <mujoco/mjmodel.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
#include "test/fixture.h"
|
#include "test/fixture.h"
|
||||||
#include "src/engine/engine_collision_driver.h"
|
|
||||||
|
|
||||||
|
|
||||||
namespace mujoco {
|
namespace mujoco {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
using MjCollisionTest = MujocoTest;
|
using MjCollisionTest = MujocoTest;
|
||||||
using GeomPair = std::pair<std::string, std::string>;
|
using GeomPair = std::pair<std::string, std::string>;
|
||||||
using ::testing::IsEmpty;
|
|
||||||
using ::testing::ElementsAre;
|
using ::testing::ElementsAre;
|
||||||
|
using ::testing::IsEmpty;
|
||||||
using ::testing::NotNull;
|
using ::testing::NotNull;
|
||||||
|
|
||||||
// Returns a sorted list of pairs of colliding geom names, where each pair of
|
// Returns a sorted list of pairs of colliding geom names, where each pair of
|
||||||
// geom names is sorted.
|
// geom names is sorted.
|
||||||
static std::vector<GeomPair> colliding_pairs(
|
static std::vector<GeomPair> colliding_pairs(const mjModel* model,
|
||||||
const mjModel* model, const mjData* data) {
|
const mjData* data) {
|
||||||
std::vector<GeomPair> result;
|
std::vector<GeomPair> result;
|
||||||
for (int i = 0; i < data->ncon; i++) {
|
for (int i = 0; i < data->ncon; i++) {
|
||||||
std::string geom1 = mj_id2name(model, mjOBJ_GEOM, data->contact[i].geom[0]);
|
std::string geom1 = mj_id2name(model, mjOBJ_GEOM, data->contact[i].geom[0]);
|
||||||
@@ -53,18 +53,16 @@ static std::vector<GeomPair> colliding_pairs(
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCollisionTest, AllCollisions) {
|
TEST_F(MjCollisionTest, AllCollisions) {
|
||||||
static const char* const kModelFilePath =
|
static const char* const kModelFilePath = "engine/testdata/collisions.xml";
|
||||||
"engine/testdata/collisions.xml";
|
|
||||||
const std::string xml_path = GetTestDataFilePath(kModelFilePath);
|
const std::string xml_path = GetTestDataFilePath(kModelFilePath);
|
||||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
|
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
|
||||||
mjData* data = mj_makeData(model);
|
mjData* data = mj_makeData(model);
|
||||||
|
|
||||||
// mjCOL_ALL is the default
|
// mjCOL_ALL is the default
|
||||||
mj_fwdPosition(model, data);
|
mj_fwdPosition(model, data);
|
||||||
EXPECT_THAT(colliding_pairs(model, data), ElementsAre(
|
EXPECT_THAT(colliding_pairs(model, data),
|
||||||
GeomPair("box", "sphere_collides"),
|
ElementsAre(GeomPair("box", "sphere_collides"),
|
||||||
GeomPair("box", "sphere_predefined")
|
GeomPair("box", "sphere_predefined")));
|
||||||
));
|
|
||||||
|
|
||||||
mj_deleteData(data);
|
mj_deleteData(data);
|
||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
@@ -72,20 +70,16 @@ TEST_F(MjCollisionTest, AllCollisions) {
|
|||||||
|
|
||||||
TEST_F(MjCollisionTest, EmptyModel) {
|
TEST_F(MjCollisionTest, EmptyModel) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString("<mujoco/>", error, sizeof(error));
|
MjModelPtr model = LoadModelFromString("<mujoco/>", error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
mj_fwdPosition(model, data);
|
mj_fwdPosition(model.get(), data.get());
|
||||||
EXPECT_THAT(colliding_pairs(model, data), IsEmpty());
|
EXPECT_THAT(colliding_pairs(model.get(), data.get()), IsEmpty());
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCollisionTest, ZeroedHessian) {
|
TEST_F(MjCollisionTest, ZeroedHessian) {
|
||||||
static const char* const kModelFilePath =
|
static const char* const kModelFilePath = "engine/testdata/collisions.xml";
|
||||||
"engine/testdata/collisions.xml";
|
|
||||||
const std::string xml_path = GetTestDataFilePath(kModelFilePath);
|
const std::string xml_path = GetTestDataFilePath(kModelFilePath);
|
||||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
|
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
|
||||||
mjData* data = mj_makeData(model);
|
mjData* data = mj_makeData(model);
|
||||||
@@ -123,18 +117,15 @@ TEST_F(MjCollisionTest, ContactCount) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
ASSERT_THAT(d, NotNull());
|
ASSERT_THAT(d, NotNull());
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// there are 8 spheres, all touching the floor
|
// there are 8 spheres, all touching the floor
|
||||||
EXPECT_EQ(d->ncon, 8);
|
EXPECT_EQ(d->ncon, 8);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCollisionTest, FilterParent) {
|
TEST_F(MjCollisionTest, FilterParent) {
|
||||||
@@ -159,12 +150,12 @@ TEST_F(MjCollisionTest, FilterParent) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
ASSERT_THAT(d, NotNull());
|
ASSERT_THAT(d, NotNull());
|
||||||
|
|
||||||
mj_fwdPosition(m, d);
|
mj_fwdPosition(m.get(), d.get());
|
||||||
|
|
||||||
// there should be zero contacts, because colliding1 and colliding2 are in
|
// there should be zero contacts, because colliding1 and colliding2 are in
|
||||||
// bodies that have a parent-child relationship, through welds
|
// bodies that have a parent-child relationship, through welds
|
||||||
@@ -172,13 +163,10 @@ TEST_F(MjCollisionTest, FilterParent) {
|
|||||||
|
|
||||||
// when this filtering is disabled, the geoms should collide
|
// when this filtering is disabled, the geoms should collide
|
||||||
m->opt.disableflags |= mjDSBL_FILTERPARENT;
|
m->opt.disableflags |= mjDSBL_FILTERPARENT;
|
||||||
mj_fwdPosition(m, d);
|
mj_fwdPosition(m.get(), d.get());
|
||||||
|
|
||||||
EXPECT_THAT(colliding_pairs(m, d),
|
EXPECT_THAT(colliding_pairs(m.get(), d.get()),
|
||||||
ElementsAre(GeomPair("colliding1", "colliding2")));
|
ElementsAre(GeomPair("colliding1", "colliding2")));
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) {
|
TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) {
|
||||||
@@ -194,20 +182,17 @@ TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
ASSERT_THAT(d, NotNull());
|
ASSERT_THAT(d, NotNull());
|
||||||
|
|
||||||
mj_fwdPosition(m, d);
|
mj_fwdPosition(m.get(), d.get());
|
||||||
|
|
||||||
// even though colliding1 and colliding2 are have a parent-child relationship,
|
// even though colliding1 and colliding2 are have a parent-child relationship,
|
||||||
// they collide because colliding1 is in <worldbody>
|
// they collide because colliding1 is in <worldbody>
|
||||||
EXPECT_THAT(colliding_pairs(m, d),
|
EXPECT_THAT(colliding_pairs(m.get(), d.get()),
|
||||||
ElementsAre(GeomPair("colliding1", "colliding2")));
|
ElementsAre(GeomPair("colliding1", "colliding2")));
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCollisionTest, TestOBB) {
|
TEST_F(MjCollisionTest, TestOBB) {
|
||||||
@@ -219,14 +204,18 @@ TEST_F(MjCollisionTest, TestOBB) {
|
|||||||
mjtNum mat2[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
|
mjtNum mat2[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
|
||||||
|
|
||||||
EXPECT_THAT(
|
EXPECT_THAT(
|
||||||
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0), true);
|
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0),
|
||||||
|
true);
|
||||||
|
|
||||||
// rotate by 45 degrees
|
// rotate by 45 degrees
|
||||||
mat2[0] = 1./mju_sqrt(2.); mat2[1] = -1./mju_sqrt(2.);
|
mat2[0] = 1. / mju_sqrt(2.);
|
||||||
mat2[3] = 1./mju_sqrt(2.); mat2[4] = 1./mju_sqrt(2.);
|
mat2[1] = -1. / mju_sqrt(2.);
|
||||||
|
mat2[3] = 1. / mju_sqrt(2.);
|
||||||
|
mat2[4] = 1. / mju_sqrt(2.);
|
||||||
|
|
||||||
EXPECT_THAT(
|
EXPECT_THAT(
|
||||||
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0), false);
|
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0),
|
||||||
|
false);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCollisionTest, PlaneInBody) {
|
TEST_F(MjCollisionTest, PlaneInBody) {
|
||||||
@@ -244,13 +233,11 @@ TEST_F(MjCollisionTest, PlaneInBody) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
ASSERT_THAT(d, NotNull());
|
ASSERT_THAT(d, NotNull());
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCollisionTest, PinchingSucceeds) {
|
TEST_F(MjCollisionTest, PinchingSucceeds) {
|
||||||
@@ -299,13 +286,13 @@ TEST_F(MjCollisionTest, PinchingSucceeds) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
ASSERT_THAT(d, NotNull());
|
ASSERT_THAT(d, NotNull());
|
||||||
|
|
||||||
int lift_id = mj_name2id(m, mjOBJ_ACTUATOR, "lift");
|
int lift_id = mj_name2id(m.get(), mjOBJ_ACTUATOR, "lift");
|
||||||
int grasp_id = mj_name2id(m, mjOBJ_ACTUATOR, "grasp");
|
int grasp_id = mj_name2id(m.get(), mjOBJ_ACTUATOR, "grasp");
|
||||||
|
|
||||||
// Phase 1: Lower gripper.
|
// Phase 1: Lower gripper.
|
||||||
// The gripper base starts at z=0.5. The finger has length 0.2 (size 0.1),
|
// The gripper base starts at z=0.5. The finger has length 0.2 (size 0.1),
|
||||||
@@ -316,21 +303,21 @@ TEST_F(MjCollisionTest, PinchingSucceeds) {
|
|||||||
for (int i = 0; i < 500; ++i) {
|
for (int i = 0; i < 500; ++i) {
|
||||||
d->ctrl[lift_id] = -0.35; // Lower
|
d->ctrl[lift_id] = -0.35; // Lower
|
||||||
d->ctrl[grasp_id] = 0; // Open
|
d->ctrl[grasp_id] = 0; // Open
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
// Phase 2: Pinch
|
// Phase 2: Pinch
|
||||||
for (int i = 0; i < 100; ++i) {
|
for (int i = 0; i < 100; ++i) {
|
||||||
d->ctrl[lift_id] = -0.35; // Hold height
|
d->ctrl[lift_id] = -0.35; // Hold height
|
||||||
d->ctrl[grasp_id] = 0.8; // Close (max 1)
|
d->ctrl[grasp_id] = 0.8; // Close (max 1)
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
// Phase 3: Lift
|
// Phase 3: Lift
|
||||||
for (int i = 0; i < 1000; ++i) {
|
for (int i = 0; i < 1000; ++i) {
|
||||||
d->ctrl[lift_id] = 0.5; // Lift up
|
d->ctrl[lift_id] = 0.5; // Lift up
|
||||||
d->ctrl[grasp_id] = 0.8; // Keep closed
|
d->ctrl[grasp_id] = 0.8; // Keep closed
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
// Check if cloth is lifted
|
// Check if cloth is lifted
|
||||||
@@ -352,9 +339,6 @@ TEST_F(MjCollisionTest, PinchingSucceeds) {
|
|||||||
// Specialized primitives (mjraw_BoxTriangle, mjraw_CapsuleTriangle) should
|
// Specialized primitives (mjraw_BoxTriangle, mjraw_CapsuleTriangle) should
|
||||||
// enable stable pinching, so we expect the cloth to be lifted.
|
// enable stable pinching, so we expect the cloth to be lifted.
|
||||||
EXPECT_GT(avg_z, 0.2) << "Cloth slipped out of gripper!";
|
EXPECT_GT(avg_z, 0.2) << "Cloth slipped out of gripper!";
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCollisionTest, MarginSumming) {
|
TEST_F(MjCollisionTest, MarginSumming) {
|
||||||
@@ -376,18 +360,15 @@ TEST_F(MjCollisionTest, MarginSumming) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
ASSERT_THAT(d, NotNull());
|
ASSERT_THAT(d, NotNull());
|
||||||
|
|
||||||
mj_fwdPosition(m, d);
|
mj_fwdPosition(m.get(), d.get());
|
||||||
|
|
||||||
// With margin summing, we expect 1 contact
|
// With margin summing, we expect 1 contact
|
||||||
EXPECT_EQ(d->ncon, 1);
|
EXPECT_EQ(d->ncon, 1);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCollisionTest, MaxContact) {
|
TEST_F(MjCollisionTest, MaxContact) {
|
||||||
@@ -414,48 +395,45 @@ TEST_F(MjCollisionTest, MaxContact) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
ASSERT_THAT(d, NotNull());
|
ASSERT_THAT(d, NotNull());
|
||||||
|
|
||||||
int mesh = mj_name2id(m, mjOBJ_GEOM, "mesh");
|
int mesh = mj_name2id(m.get(), mjOBJ_GEOM, "mesh");
|
||||||
int box = mj_name2id(m, mjOBJ_GEOM, "box");
|
int box = mj_name2id(m.get(), mjOBJ_GEOM, "box");
|
||||||
int plane = mj_name2id(m, mjOBJ_GEOM, "plane");
|
int plane = mj_name2id(m.get(), mjOBJ_GEOM, "plane");
|
||||||
int sphere = mj_name2id(m, mjOBJ_GEOM, "sphere");
|
int sphere = mj_name2id(m.get(), mjOBJ_GEOM, "sphere");
|
||||||
int capsule = mj_name2id(m, mjOBJ_GEOM, "capsule");
|
int capsule = mj_name2id(m.get(), mjOBJ_GEOM, "capsule");
|
||||||
int ellipsoid = mj_name2id(m, mjOBJ_GEOM, "ellipsoid");
|
int ellipsoid = mj_name2id(m.get(), mjOBJ_GEOM, "ellipsoid");
|
||||||
int cylinder = mj_name2id(m, mjOBJ_GEOM, "cylinder");
|
int cylinder = mj_name2id(m.get(), mjOBJ_GEOM, "cylinder");
|
||||||
|
|
||||||
EXPECT_EQ(mj_maxContact(m, mesh, box, -1), 4);
|
EXPECT_EQ(mj_maxContact(m.get(), mesh, box, -1), 4);
|
||||||
EXPECT_EQ(mj_maxContact(m, mesh, plane, -1), 3);
|
EXPECT_EQ(mj_maxContact(m.get(), mesh, plane, -1), 3);
|
||||||
EXPECT_EQ(mj_maxContact(m, box, plane, -1), 4);
|
EXPECT_EQ(mj_maxContact(m.get(), box, plane, -1), 4);
|
||||||
EXPECT_EQ(mj_maxContact(m, mesh, mesh, -1), 4);
|
EXPECT_EQ(mj_maxContact(m.get(), mesh, mesh, -1), 4);
|
||||||
EXPECT_EQ(mj_maxContact(m, box, box, -1), 8);
|
EXPECT_EQ(mj_maxContact(m.get(), box, box, -1), 8);
|
||||||
EXPECT_EQ(mj_maxContact(m, capsule, capsule, -1), 2);
|
EXPECT_EQ(mj_maxContact(m.get(), capsule, capsule, -1), 2);
|
||||||
EXPECT_EQ(mj_maxContact(m, capsule, box, -1), 4);
|
EXPECT_EQ(mj_maxContact(m.get(), capsule, box, -1), 4);
|
||||||
EXPECT_EQ(mj_maxContact(m, capsule, plane, -1), 2);
|
EXPECT_EQ(mj_maxContact(m.get(), capsule, plane, -1), 2);
|
||||||
EXPECT_EQ(mj_maxContact(m, cylinder, plane, -1), 4);
|
EXPECT_EQ(mj_maxContact(m.get(), cylinder, plane, -1), 4);
|
||||||
EXPECT_EQ(mj_maxContact(m, sphere, sphere, -1), 1);
|
EXPECT_EQ(mj_maxContact(m.get(), sphere, sphere, -1), 1);
|
||||||
EXPECT_EQ(mj_maxContact(m, sphere, capsule, -1), 1);
|
EXPECT_EQ(mj_maxContact(m.get(), sphere, capsule, -1), 1);
|
||||||
EXPECT_EQ(mj_maxContact(m, sphere, box, -1), 1);
|
EXPECT_EQ(mj_maxContact(m.get(), sphere, box, -1), 1);
|
||||||
EXPECT_EQ(mj_maxContact(m, sphere, mesh, -1), 1);
|
EXPECT_EQ(mj_maxContact(m.get(), sphere, mesh, -1), 1);
|
||||||
EXPECT_EQ(mj_maxContact(m, sphere, plane, -1), 1);
|
EXPECT_EQ(mj_maxContact(m.get(), sphere, plane, -1), 1);
|
||||||
EXPECT_EQ(mj_maxContact(m, sphere, cylinder, -1), 1);
|
EXPECT_EQ(mj_maxContact(m.get(), sphere, cylinder, -1), 1);
|
||||||
EXPECT_EQ(mj_maxContact(m, ellipsoid, ellipsoid, -1), 1);
|
EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, ellipsoid, -1), 1);
|
||||||
EXPECT_EQ(mj_maxContact(m, ellipsoid, box, -1), 1);
|
EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, box, -1), 1);
|
||||||
EXPECT_EQ(mj_maxContact(m, ellipsoid, mesh, -1), 1);
|
EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, mesh, -1), 1);
|
||||||
EXPECT_EQ(mj_maxContact(m, ellipsoid, plane, -1), 1);
|
EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, plane, -1), 1);
|
||||||
EXPECT_EQ(mj_maxContact(m, ellipsoid, cylinder, -1), 1);
|
EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, cylinder, -1), 1);
|
||||||
EXPECT_EQ(mj_maxContact(m, ellipsoid, capsule, -1), 1);
|
EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, capsule, -1), 1);
|
||||||
EXPECT_EQ(mj_maxContact(m, capsule, cylinder, -1), 5);
|
EXPECT_EQ(mj_maxContact(m.get(), capsule, cylinder, -1), 5);
|
||||||
EXPECT_EQ(mj_maxContact(m, capsule, mesh, -1), 5);
|
EXPECT_EQ(mj_maxContact(m.get(), capsule, mesh, -1), 5);
|
||||||
EXPECT_EQ(mj_maxContact(m, cylinder, cylinder, -1), 5);
|
EXPECT_EQ(mj_maxContact(m.get(), cylinder, cylinder, -1), 5);
|
||||||
EXPECT_EQ(mj_maxContact(m, cylinder, box, -1), 5);
|
EXPECT_EQ(mj_maxContact(m.get(), cylinder, box, -1), 5);
|
||||||
EXPECT_EQ(mj_maxContact(m, cylinder, mesh, -1), 5);
|
EXPECT_EQ(mj_maxContact(m.get(), cylinder, mesh, -1), 5);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
@@ -42,9 +42,6 @@ using ::testing::ElementsAre;
|
|||||||
using ::testing::Pointwise;
|
using ::testing::Pointwise;
|
||||||
using ::testing::DoubleNear;
|
using ::testing::DoubleNear;
|
||||||
|
|
||||||
using TestModel = std::unique_ptr<mjModel, void (*)(mjModel*)>;
|
|
||||||
using TestData = std::unique_ptr<mjData, void (*)(mjData*)>;
|
|
||||||
|
|
||||||
constexpr mjtNum kTolerance = 1e-6;
|
constexpr mjtNum kTolerance = 1e-6;
|
||||||
constexpr int kMaxIterations = 1000;
|
constexpr int kMaxIterations = 1000;
|
||||||
constexpr char kEllipsoidXml[] = R"(
|
constexpr char kEllipsoidXml[] = R"(
|
||||||
@@ -66,18 +63,7 @@ constexpr char kEllipsoidXml[] = R"(
|
|||||||
</keyframe>
|
</keyframe>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel LoadModel(std::string_view xml) {
|
mjtNum GeomDist(const MjModelPtr& m, const MjDataPtr& d, int g1, int g2,
|
||||||
char error[1024];
|
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
|
||||||
EXPECT_NE(model, nullptr) << "Failed to load model: " << error;
|
|
||||||
return TestModel(model, mj_deleteModel);
|
|
||||||
}
|
|
||||||
|
|
||||||
TestData MakeData(const mjModel* model) {
|
|
||||||
return TestData(mj_makeData(model), mj_deleteData);
|
|
||||||
}
|
|
||||||
|
|
||||||
mjtNum GeomDist(const TestModel& m, const TestData& d, int g1, int g2,
|
|
||||||
mjtNum x1[3], mjtNum x2[3], mjtNum cutoff = mjMAX_LIMIT) {
|
mjtNum x1[3], mjtNum x2[3], mjtNum cutoff = mjMAX_LIMIT) {
|
||||||
mjCCDConfig config;
|
mjCCDConfig config;
|
||||||
mjCCDStatus status;
|
mjCCDStatus status;
|
||||||
@@ -102,8 +88,8 @@ mjtNum GeomDist(const TestModel& m, const TestData& d, int g1, int g2,
|
|||||||
}
|
}
|
||||||
|
|
||||||
int Penetration(mjCCDStatus& status, mjtNum& depth, std::vector<mjtNum>& dir,
|
int Penetration(mjCCDStatus& status, mjtNum& depth, std::vector<mjtNum>& dir,
|
||||||
std::vector<mjtNum>& pos, const TestModel& model,
|
std::vector<mjtNum>& pos, const MjModelPtr& model,
|
||||||
const TestData& data, int g1, int g2, mjtNum margin = 0,
|
const MjDataPtr& data, int g1, int g2, mjtNum margin = 0,
|
||||||
int max_contacts = 1) {
|
int max_contacts = 1) {
|
||||||
mjCCDObj obj1, obj2;
|
mjCCDObj obj1, obj2;
|
||||||
mjc_initCCDObj(&obj1, model.get(), data.get(), g1, margin);
|
mjc_initCCDObj(&obj1, model.get(), data.get(), g1, margin);
|
||||||
@@ -180,8 +166,8 @@ TEST_F(MjGjkTest, SphereSphereDist) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -203,8 +189,8 @@ TEST_F(MjGjkTest, SphereSphereDistCutoff) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -223,8 +209,8 @@ TEST_F(MjGjkTest, SphereSphereNoDist) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -247,8 +233,8 @@ TEST_F(MjGjkTest, SphereSphereIntersect) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -284,8 +270,8 @@ TEST_F(MjGjkTest, BoxBoxDepth) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -313,8 +299,8 @@ TEST_F(MjGjkTest, BoxBoxDepth2) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat + 9;
|
mjtNum* xmat = data->geom_xmat + 9;
|
||||||
@@ -358,8 +344,8 @@ TEST_F(MjGjkTest, BoxBoxDepth3) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat;
|
mjtNum* xmat = data->geom_xmat;
|
||||||
@@ -421,8 +407,8 @@ TEST_F(MjGjkTest, BoxBoxSize05) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat;
|
mjtNum* xmat = data->geom_xmat;
|
||||||
@@ -479,8 +465,8 @@ TEST_F(MjGjkTest, BoxBoxSize05b) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat;
|
mjtNum* xmat = data->geom_xmat;
|
||||||
@@ -540,8 +526,8 @@ TEST_F(MjGjkTest, BoxBoxSize05c) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat;
|
mjtNum* xmat = data->geom_xmat;
|
||||||
@@ -601,8 +587,8 @@ TEST_F(MjGjkTest, BoxBoxTouching) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -627,8 +613,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -661,8 +647,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD2) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -695,8 +681,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD3) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat + 9;
|
mjtNum* xmat = data->geom_xmat + 9;
|
||||||
@@ -737,8 +723,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD4) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat;
|
mjtNum* xmat = data->geom_xmat;
|
||||||
@@ -800,8 +786,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD5) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat;
|
mjtNum* xmat = data->geom_xmat;
|
||||||
@@ -864,8 +850,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD6) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat + 9;
|
mjtNum* xmat = data->geom_xmat + 9;
|
||||||
@@ -910,8 +896,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD7) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat;
|
mjtNum* xmat = data->geom_xmat;
|
||||||
@@ -969,8 +955,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD8) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat;
|
mjtNum* xmat = data->geom_xmat;
|
||||||
@@ -1027,8 +1013,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD9) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat;
|
mjtNum* xmat = data->geom_xmat;
|
||||||
@@ -1086,8 +1072,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD10) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xpos = data->geom_xpos;
|
mjtNum* xpos = data->geom_xpos;
|
||||||
@@ -1123,8 +1109,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD11) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xpos = data->geom_xpos;
|
mjtNum* xpos = data->geom_xpos;
|
||||||
@@ -1184,8 +1170,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD12) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xpos = data->geom_xpos;
|
mjtNum* xpos = data->geom_xpos;
|
||||||
@@ -1243,8 +1229,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD13) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xpos = data->geom_xpos;
|
mjtNum* xpos = data->geom_xpos;
|
||||||
@@ -1305,8 +1291,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD14) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xpos = data->geom_xpos;
|
mjtNum* xpos = data->geom_xpos;
|
||||||
@@ -1362,8 +1348,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD15) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat;
|
mjtNum* xmat = data->geom_xmat;
|
||||||
@@ -1441,8 +1427,8 @@ TEST_F(MjGjkTest, SmallBoxMesh) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -1485,8 +1471,8 @@ TEST_F(MjGjkTest, BoxMesh) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -1515,8 +1501,8 @@ TEST_F(MjGjkTest, BoxMesh2) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -1545,8 +1531,8 @@ TEST_F(MjGjkTest, BoxMeshPrune) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -1577,8 +1563,8 @@ TEST_F(MjGjkTest, MeshMesh) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -1609,8 +1595,8 @@ TEST_F(MjGjkTest, MeshMeshPrune) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -1633,8 +1619,8 @@ TEST_F(MjGjkTest, BoxEdge) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "box1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "box1");
|
||||||
@@ -1657,8 +1643,8 @@ TEST_F(MjGjkTest, BoxEdge2) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat;
|
mjtNum* xmat = data->geom_xmat;
|
||||||
@@ -1715,8 +1701,8 @@ TEST_F(MjGjkTest, BoxEdgeEdge) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat;
|
mjtNum* xmat = data->geom_xmat;
|
||||||
@@ -1779,8 +1765,8 @@ TEST_F(MjGjkTest, MeshEdge) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "box1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "box1");
|
||||||
@@ -1819,8 +1805,8 @@ TEST_F(MjGjkTest, MeshEdge2) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "floor");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "floor");
|
||||||
@@ -1835,8 +1821,8 @@ TEST_F(MjGjkTest, MeshEdge2) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) {
|
TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) {
|
||||||
TestModel model = LoadModel(kEllipsoidXml);
|
MjModelPtr model = LoadModelFromString(kEllipsoidXml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_resetDataKeyframe(model.get(), data.get(), 0);
|
mj_resetDataKeyframe(model.get(), data.get(), 0);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
@@ -1861,8 +1847,8 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -1884,8 +1870,8 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidSlowConvergence) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* xmat = data->geom_xmat;
|
mjtNum* xmat = data->geom_xmat;
|
||||||
@@ -1943,8 +1929,8 @@ TEST_F(MjGjkTest, BoxBox) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -1963,8 +1949,8 @@ TEST_F(MjGjkTest, BoxBoxLarge) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -2013,8 +1999,8 @@ static constexpr char xml[] = R"(
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -2046,8 +2032,8 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -2071,8 +2057,8 @@ TEST_F(MjGjkTest, CapsuleCapsule) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
@@ -2101,8 +2087,8 @@ TEST_F(MjGjkTest, CylinderBoxMargin) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
// margin=0.1 means forces generated when dist<0.1
|
// margin=0.1 means forces generated when dist<0.1
|
||||||
@@ -2121,8 +2107,8 @@ TEST_F(MjGjkTest, BoxEdgeFlipped) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
TestModel model = LoadModel(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
TestData data = MakeData(model.get());
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model.get(), data.get());
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
|
||||||
|
|||||||
@@ -14,14 +14,16 @@
|
|||||||
|
|
||||||
// Tests for sdf collisions.
|
// Tests for sdf collisions.
|
||||||
|
|
||||||
|
#include "src/engine/engine_collision_sdf.h"
|
||||||
|
|
||||||
#include <cstdio>
|
#include <cstdio>
|
||||||
|
|
||||||
#include <gmock/gmock.h>
|
#include <gmock/gmock.h>
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <mujoco/mjdata.h>
|
#include <mujoco/mjdata.h>
|
||||||
#include <mujoco/mjmodel.h>
|
#include <mujoco/mjmodel.h>
|
||||||
#include <mujoco/mjtype.h>
|
#include <mujoco/mjtype.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
#include "src/engine/engine_collision_sdf.h"
|
|
||||||
#include "test/fixture.h"
|
#include "test/fixture.h"
|
||||||
|
|
||||||
namespace mujoco {
|
namespace mujoco {
|
||||||
@@ -48,19 +50,21 @@ static constexpr char kSdfModel[] = R"(
|
|||||||
|
|
||||||
TEST_F(SdfTest, SdfPrimitive) {
|
TEST_F(SdfTest, SdfPrimitive) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(kSdfModel, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(kSdfModel, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
ASSERT_THAT(model->ngeom, kgeoms);
|
ASSERT_THAT(model->ngeom, kgeoms);
|
||||||
|
|
||||||
mjSDF sdf;
|
mjSDF sdf;
|
||||||
const mjpPlugin* null_plugin = NULL;
|
const mjpPlugin* null_plugin = NULL;
|
||||||
mjtNum gradient[3], dist[kgeoms][kpoints] = {
|
mjtNum gradient[3],
|
||||||
|
dist[kgeoms][kpoints] = {
|
||||||
{0, 0, 0, 0, 1, 1}, // plane
|
{0, 0, 0, 0, 1, 1}, // plane
|
||||||
{-1, 0, 0, mju_sqrt(2)-1, mju_sqrt(2)-1, mju_sqrt(3)-1}, // sphere
|
{-1, 0, 0, mju_sqrt(2) - 1, mju_sqrt(2) - 1,
|
||||||
{(mjtNum)-.1, (mjtNum).9, (mjtNum).9,
|
mju_sqrt(3) - 1}, // sphere
|
||||||
mju_sqrt(2)-(mjtNum).1, (mjtNum).9, mju_sqrt(2)-(mjtNum).1}, // capsule
|
{(mjtNum)-.1, (mjtNum).9, (mjtNum).9, mju_sqrt(2) - (mjtNum).1,
|
||||||
|
(mjtNum).9, mju_sqrt(2) - (mjtNum).1}, // capsule
|
||||||
{-1, 0, 0, mju_sqrt(2) - 1, 0, mju_sqrt(2) - 1}, // cylinder
|
{-1, 0, 0, mju_sqrt(2) - 1, 0, mju_sqrt(2) - 1}, // cylinder
|
||||||
{-mju_sqrt(3), 0, 0, 0, 0, 0}, // box
|
{-mju_sqrt(3), 0, 0, 0, 0, 0}, // box
|
||||||
};
|
};
|
||||||
@@ -73,14 +77,11 @@ TEST_F(SdfTest, SdfPrimitive) {
|
|||||||
sdf.type = mjSDFTYPE_SINGLE;
|
sdf.type = mjSDFTYPE_SINGLE;
|
||||||
sdf.geomtype = (mjtGeom*)(model->geom_type + i);
|
sdf.geomtype = (mjtGeom*)(model->geom_type + i);
|
||||||
for (int j = 0; j < kpoints; j++) {
|
for (int j = 0; j < kpoints; j++) {
|
||||||
EXPECT_NEAR(mjc_distance(model, data, &sdf, points[j]), dist[i][j],
|
EXPECT_NEAR(mjc_distance(model.get(), data.get(), &sdf, points[j]),
|
||||||
MjTol(1e-9, 5e-7));
|
dist[i][j], MjTol(1e-9, 5e-7));
|
||||||
mjc_gradient(model, data, &sdf, gradient, points[j]);
|
mjc_gradient(model.get(), data.get(), &sdf, gradient, points[j]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static constexpr char kFlexSdfModel[] = R"(
|
static constexpr char kFlexSdfModel[] = R"(
|
||||||
@@ -118,9 +119,9 @@ static constexpr char kFlexSdfModel[] = R"(
|
|||||||
|
|
||||||
TEST_F(SdfTest, FlexSdfCollision) {
|
TEST_F(SdfTest, FlexSdfCollision) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(kFlexSdfModel, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(kFlexSdfModel, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
|
|
||||||
// check there is at least one flex
|
// check there is at least one flex
|
||||||
@@ -128,7 +129,7 @@ TEST_F(SdfTest, FlexSdfCollision) {
|
|||||||
|
|
||||||
// simulate for a few steps to let flex fall onto torus
|
// simulate for a few steps to let flex fall onto torus
|
||||||
for (int i = 0; i < 100; i++) {
|
for (int i = 0; i < 100; i++) {
|
||||||
mj_step(model, data);
|
mj_step(model.get(), data.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
// should have contacts between flex and SDF
|
// should have contacts between flex and SDF
|
||||||
@@ -143,11 +144,7 @@ TEST_F(SdfTest, FlexSdfCollision) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
EXPECT_TRUE(has_flex_contact) << "Expected at least one flex contact";
|
EXPECT_TRUE(has_flex_contact) << "Expected at least one flex contact";
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace mujoco
|
} // namespace mujoco
|
||||||
|
|
||||||
|
|||||||
@@ -14,6 +14,8 @@
|
|||||||
|
|
||||||
// Tests for engine/engine_core_constraint.c.
|
// Tests for engine/engine_core_constraint.c.
|
||||||
|
|
||||||
|
#include "src/engine/engine_core_constraint.h"
|
||||||
|
|
||||||
#include <array>
|
#include <array>
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
#include <string>
|
#include <string>
|
||||||
@@ -23,7 +25,6 @@
|
|||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <mujoco/mjmodel.h>
|
#include <mujoco/mjmodel.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
#include "src/engine/engine_core_constraint.h"
|
|
||||||
#include "src/engine/engine_core_util.h"
|
#include "src/engine/engine_core_util.h"
|
||||||
#include "src/engine/engine_support.h"
|
#include "src/engine/engine_support.h"
|
||||||
#include "src/engine/engine_util_misc.h"
|
#include "src/engine/engine_util_misc.h"
|
||||||
@@ -51,11 +52,11 @@ TEST_F(CoreConstraintTest, RestPenetration) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, testing::NotNull()) << error;
|
ASSERT_THAT(model.get(), testing::NotNull()) << error;
|
||||||
mjtNum gravity = -model->opt.gravity[2];
|
mjtNum gravity = -model->opt.gravity[2];
|
||||||
mjtNum damping_ratio = 0.8;
|
mjtNum damping_ratio = 0.8;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
for (const mjtNum reference : {-100.0, -10.0, 0.1, 0.01}) {
|
for (const mjtNum reference : {-100.0, -10.0, 0.1, 0.01}) {
|
||||||
for (const mjtNum impedance : {0.3, 0.9, 0.99}) {
|
for (const mjtNum impedance : {0.3, 0.9, 0.99}) {
|
||||||
@@ -68,14 +69,15 @@ TEST_F(CoreConstraintTest, RestPenetration) {
|
|||||||
// set solref
|
// set solref
|
||||||
for (int i = 0; i < model->ngeom; i++) {
|
for (int i = 0; i < model->ngeom; i++) {
|
||||||
model->geom_solref[i * mjNREF + 0] = reference;
|
model->geom_solref[i * mjNREF + 0] = reference;
|
||||||
model->geom_solref[i*mjNREF + 1] = reference < 0 ? -10 : damping_ratio;
|
model->geom_solref[i * mjNREF + 1] =
|
||||||
|
reference < 0 ? -10 : damping_ratio;
|
||||||
}
|
}
|
||||||
|
|
||||||
// simulate for 50 seconds
|
// simulate for 50 seconds
|
||||||
mj_resetData(model, data);
|
mj_resetData(model.get(), data.get());
|
||||||
while (data->time < 50) {
|
while (data->time < 50) {
|
||||||
mjtNum time = data->time;
|
mjtNum time = data->time;
|
||||||
mj_step(model, data);
|
mj_step(model.get(), data.get());
|
||||||
ASSERT_GT(data->time, time) << "Divergence detected";
|
ASSERT_GT(data->time, time) << "Divergence detected";
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -91,9 +93,6 @@ TEST_F(CoreConstraintTest, RestPenetration) {
|
|||||||
EXPECT_THAT(depth, MjNear(expected_depth, 1e-10, 1e-3));
|
EXPECT_THAT(depth, MjNear(expected_depth, 1e-10, 1e-3));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static const char* const kDoflessContactPath =
|
static const char* const kDoflessContactPath =
|
||||||
@@ -242,17 +241,10 @@ TEST_F(CoreConstraintTest, ConstraintUpdateImpl) {
|
|||||||
int adr = d2->island_iefcadr[island];
|
int adr = d2->island_iefcadr[island];
|
||||||
int* state = d2->iefc_state + adr;
|
int* state = d2->iefc_state + adr;
|
||||||
mjtNum* force = d2->iefc_force + adr;
|
mjtNum* force = d2->iefc_force + adr;
|
||||||
mj_constraintUpdate_impl(ne, nf, efcnum,
|
mj_constraintUpdate_impl(ne, nf, efcnum, d2->iefc_D + adr,
|
||||||
d2->iefc_D + adr,
|
d2->iefc_R + adr, d2->iefc_frictionloss + adr,
|
||||||
d2->iefc_R + adr,
|
jari, d2->iefc_type + adr, d2->iefc_id + adr,
|
||||||
d2->iefc_frictionloss + adr,
|
d2->contact, state, force, &cost2i,
|
||||||
jari,
|
|
||||||
d2->iefc_type + adr,
|
|
||||||
d2->iefc_id + adr,
|
|
||||||
d2->contact,
|
|
||||||
state,
|
|
||||||
force,
|
|
||||||
&cost2i,
|
|
||||||
/*flg_coneHessian=*/1);
|
/*flg_coneHessian=*/1);
|
||||||
|
|
||||||
// compare nefc vectors
|
// compare nefc vectors
|
||||||
@@ -308,9 +300,9 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, testing::NotNull()) << error;
|
ASSERT_THAT(model.get(), testing::NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_EQ(model->neq, 1);
|
ASSERT_EQ(model->neq, 1);
|
||||||
ASSERT_EQ(model->eq_type[0], mjEQ_FLEXVERT);
|
ASSERT_EQ(model->eq_type[0], mjEQ_FLEXVERT);
|
||||||
ASSERT_EQ(model->nflex, 1);
|
ASSERT_EQ(model->nflex, 1);
|
||||||
@@ -318,7 +310,7 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
|
|||||||
ASSERT_EQ(model->flex_edgenum[0], 16);
|
ASSERT_EQ(model->flex_edgenum[0], 16);
|
||||||
|
|
||||||
// step1 to populate flexvert_length
|
// step1 to populate flexvert_length
|
||||||
mj_step1(model, data);
|
mj_step1(model.get(), data.get());
|
||||||
EXPECT_EQ(data->ne, 2 * model->flex_vertnum[0]);
|
EXPECT_EQ(data->ne, 2 * model->flex_vertnum[0]);
|
||||||
EXPECT_EQ(data->nefc, 18);
|
EXPECT_EQ(data->nefc, 18);
|
||||||
for (int i = 0; i < 18; ++i) {
|
for (int i = 0; i < 18; ++i) {
|
||||||
@@ -336,7 +328,7 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
|
|||||||
for (int j = 0; j < model->flex_vertnum[0]; ++j) {
|
for (int j = 0; j < model->flex_vertnum[0]; ++j) {
|
||||||
qvel[3 * j + i] = 1.0;
|
qvel[3 * j + i] = 1.0;
|
||||||
}
|
}
|
||||||
mj_mulJacVec(model, data, Jqvel.data(), qvel.data());
|
mj_mulJacVec(model.get(), data.get(), Jqvel.data(), qvel.data());
|
||||||
for (int j = 0; j < data->nefc; ++j) {
|
for (int j = 0; j < data->nefc; ++j) {
|
||||||
EXPECT_NEAR(Jqvel[j], 0, MjTol(1e-9, 1e-5));
|
EXPECT_NEAR(Jqvel[j], 0, MjTol(1e-9, 1e-5));
|
||||||
}
|
}
|
||||||
@@ -355,14 +347,11 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
|
|||||||
qvel[3 * j + 1] = linvel[1];
|
qvel[3 * j + 1] = linvel[1];
|
||||||
qvel[3 * j + 2] = linvel[2];
|
qvel[3 * j + 2] = linvel[2];
|
||||||
}
|
}
|
||||||
mj_mulJacVec(model, data, Jqvel.data(), qvel.data());
|
mj_mulJacVec(model.get(), data.get(), Jqvel.data(), qvel.data());
|
||||||
for (int j = 0; j < data->nefc; ++j) {
|
for (int j = 0; j < data->nefc; ++j) {
|
||||||
EXPECT_NEAR(Jqvel[j], 0, MjTol(1e-9, 1e-5));
|
EXPECT_NEAR(Jqvel[j], 0, MjTol(1e-9, 1e-5));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Test flex strain constraint with pinned nodes attached to freejoint parent
|
// Test flex strain constraint with pinned nodes attached to freejoint parent
|
||||||
@@ -392,12 +381,12 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
mj_resetData(m, d);
|
mj_resetData(m.get(), d.get());
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// Check that we have constraints
|
// Check that we have constraints
|
||||||
EXPECT_GT(d->nefc, 0) << "No constraints generated";
|
EXPECT_GT(d->nefc, 0) << "No constraints generated";
|
||||||
@@ -449,8 +438,8 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
|
|||||||
mju_copy(d->qpos, qpos0.data(), m->nq);
|
mju_copy(d->qpos, qpos0.data(), m->nq);
|
||||||
mjtNum dqpos[100] = {0};
|
mjtNum dqpos[100] = {0};
|
||||||
dqpos[j] = eps;
|
dqpos[j] = eps;
|
||||||
mj_integratePos(m, d->qpos, dqpos, 1);
|
mj_integratePos(m.get(), d->qpos, dqpos, 1);
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
for (int i = 0; i < num_constraints_to_check; i++) {
|
for (int i = 0; i < num_constraints_to_check; i++) {
|
||||||
mjtNum fd = (d->efc_pos[i] - efc_pos0[i]) / eps;
|
mjtNum fd = (d->efc_pos[i] - efc_pos0[i]) / eps;
|
||||||
@@ -472,7 +461,7 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
|
|||||||
d->qpos[4] = 0;
|
d->qpos[4] = 0;
|
||||||
d->qpos[5] = 0;
|
d->qpos[5] = 0;
|
||||||
d->qpos[6] = mju_sin(angle / 2); // z
|
d->qpos[6] = mju_sin(angle / 2); // z
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
mjtNum max_strain_rotated = 0;
|
mjtNum max_strain_rotated = 0;
|
||||||
for (int i = 0; i < d->ne; i++) {
|
for (int i = 0; i < d->ne; i++) {
|
||||||
@@ -494,8 +483,8 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
|
|||||||
mju_copy(d->qpos, qpos_rot.data(), m->nq);
|
mju_copy(d->qpos, qpos_rot.data(), m->nq);
|
||||||
mjtNum dqpos[100] = {0};
|
mjtNum dqpos[100] = {0};
|
||||||
dqpos[j] = eps;
|
dqpos[j] = eps;
|
||||||
mj_integratePos(m, d->qpos, dqpos, 1);
|
mj_integratePos(m.get(), d->qpos, dqpos, 1);
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
mjtNum fd = (d->efc_pos[0] - efc_pos_rot[0]) / eps;
|
mjtNum fd = (d->efc_pos[0] - efc_pos_rot[0]) / eps;
|
||||||
mjtNum analytic = d->efc_J[0 * nv + j];
|
mjtNum analytic = d->efc_J[0 * nv + j];
|
||||||
@@ -510,12 +499,12 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
|
|||||||
|
|
||||||
// Reset for simulation
|
// Reset for simulation
|
||||||
mju_copy(d->qpos, qpos0.data(), m->nq);
|
mju_copy(d->qpos, qpos0.data(), m->nq);
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// Run simulation only if checks pass
|
// Run simulation only if checks pass
|
||||||
if (!has_bad_constraint && !has_bad_jacobian) {
|
if (!has_bad_constraint && !has_bad_jacobian) {
|
||||||
for (int i = 0; i < 2000; i++) {
|
for (int i = 0; i < 2000; i++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
|
|
||||||
ASSERT_FALSE(mju_isBad(d->qpos[0]))
|
ASSERT_FALSE(mju_isBad(d->qpos[0]))
|
||||||
<< "Simulation became unstable at step " << i;
|
<< "Simulation became unstable at step " << i;
|
||||||
@@ -529,9 +518,6 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Test flex strain constraint WITHOUT pinned nodes (simpler case)
|
// Test flex strain constraint WITHOUT pinned nodes (simpler case)
|
||||||
@@ -554,12 +540,12 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
mj_resetData(m, d);
|
mj_resetData(m.get(), d.get());
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// Check constraints
|
// Check constraints
|
||||||
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
|
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
|
||||||
@@ -597,7 +583,7 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) {
|
|||||||
d->qpos[4] = 0; // x
|
d->qpos[4] = 0; // x
|
||||||
d->qpos[5] = 0; // y
|
d->qpos[5] = 0; // y
|
||||||
d->qpos[6] = mju_sin(angle / 2); // z
|
d->qpos[6] = mju_sin(angle / 2); // z
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
mjtNum max_strain_rotated = 0;
|
mjtNum max_strain_rotated = 0;
|
||||||
for (int i = 0; i < d->ne; i++) {
|
for (int i = 0; i < d->ne; i++) {
|
||||||
@@ -610,15 +596,12 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) {
|
|||||||
|
|
||||||
// Run simulation for a few steps to check stability
|
// Run simulation for a few steps to check stability
|
||||||
mju_copy(d->qpos, qpos0.data(), m->nq);
|
mju_copy(d->qpos, qpos0.data(), m->nq);
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
for (int i = 0; i < 100; i++) {
|
for (int i = 0; i < 100; i++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation unstable at step " << i;
|
ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation unstable at step " << i;
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Test flex strain constraint with quadratic interpolation
|
// Test flex strain constraint with quadratic interpolation
|
||||||
@@ -641,12 +624,12 @@ TEST_F(CoreConstraintTest, StrainConstraintQuadratic) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
mj_resetData(m, d);
|
mj_resetData(m.get(), d.get());
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// Check constraints generated
|
// Check constraints generated
|
||||||
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
|
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
|
||||||
@@ -674,13 +657,9 @@ TEST_F(CoreConstraintTest, StrainConstraintQuadratic) {
|
|||||||
|
|
||||||
// Run simulation for a few steps
|
// Run simulation for a few steps
|
||||||
for (int i = 0; i < 100; i++) {
|
for (int i = 0; i < 100; i++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
ASSERT_FALSE(mju_isBad(d->qpos[0]))
|
ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation unstable at step " << i;
|
||||||
<< "Simulation unstable at step " << i;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) {
|
TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) {
|
||||||
@@ -701,11 +680,11 @@ TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// Check number of equalities
|
// Check number of equalities
|
||||||
EXPECT_EQ(m->neq, 6);
|
EXPECT_EQ(m->neq, 6);
|
||||||
@@ -719,9 +698,6 @@ TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) {
|
|||||||
EXPECT_NEAR(d->efc_pos[i], 0, 1e-10)
|
EXPECT_NEAR(d->efc_pos[i], 0, 1e-10)
|
||||||
<< "nonzero constraint residual at " << i;
|
<< "nonzero constraint residual at " << i;
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Test quadratic passive forces (no constraints) for stability
|
// Test quadratic passive forces (no constraints) for stability
|
||||||
@@ -741,13 +717,13 @@ TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
// Run for 500 steps — should stay stable
|
// Run for 500 steps — should stay stable
|
||||||
for (int i = 0; i < 500; i++) {
|
for (int i = 0; i < 500; i++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
ASSERT_FALSE(mju_isBad(d->qpos[0]))
|
ASSERT_FALSE(mju_isBad(d->qpos[0]))
|
||||||
<< "Passive quadratic unstable at step " << i;
|
<< "Passive quadratic unstable at step " << i;
|
||||||
for (int j = 0; j < m->nv; j++) {
|
for (int j = 0; j < m->nv; j++) {
|
||||||
@@ -755,9 +731,6 @@ TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) {
|
|||||||
<< "Velocity exploded at step " << i;
|
<< "Velocity exploded at step " << i;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Test quadratic with anisotropic cells (like what mesh bounding box creates)
|
// Test quadratic with anisotropic cells (like what mesh bounding box creates)
|
||||||
@@ -782,27 +755,24 @@ TEST_F(CoreConstraintTest, QuadraticAnisotropicStrain) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
|
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
|
||||||
|
|
||||||
// Run for 200 steps with gravity + contact
|
// Run for 200 steps with gravity + contact
|
||||||
for (int i = 0; i < 200; i++) {
|
for (int i = 0; i < 200; i++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
ASSERT_FALSE(mju_isBad(d->qpos[0]))
|
ASSERT_FALSE(mju_isBad(d->qpos[0]))
|
||||||
<< "Anisotropic quadratic unstable at step " << i;
|
<< "Anisotropic quadratic unstable at step " << i;
|
||||||
for (int j = 0; j < m->nv; j++) {
|
for (int j = 0; j < m->nv; j++) {
|
||||||
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
|
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
|
||||||
<< "Velocity exploded at step " << i
|
<< "Velocity exploded at step " << i << ", qvel[" << j
|
||||||
<< ", qvel[" << j << "]=" << d->qvel[j];
|
<< "]=" << d->qvel[j];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(CoreConstraintTest, ContactSharedDofJacobian) {
|
TEST_F(CoreConstraintTest, ContactSharedDofJacobian) {
|
||||||
@@ -826,21 +796,18 @@ TEST_F(CoreConstraintTest, ContactSharedDofJacobian) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
ASSERT_EQ(model->nv, 3);
|
ASSERT_EQ(model->nv, 3);
|
||||||
ASSERT_TRUE(mj_isSparse(model));
|
ASSERT_TRUE(mj_isSparse(model.get()));
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
ASSERT_EQ(data->ncon, 1);
|
ASSERT_EQ(data->ncon, 1);
|
||||||
ASSERT_GE(data->nefc, 1);
|
ASSERT_GE(data->nefc, 1);
|
||||||
|
|
||||||
EXPECT_EQ(data->efc_J_rownnz[0], 2);
|
EXPECT_EQ(data->efc_J_rownnz[0], 2);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static const char* const kJdotvConnect2dPath =
|
static const char* const kJdotvConnect2dPath =
|
||||||
@@ -852,10 +819,8 @@ static const char* const kJdotvWeld3dPath =
|
|||||||
|
|
||||||
// validate mj_Jdotv against finite-differenced constraint Jacobian
|
// validate mj_Jdotv against finite-differenced constraint Jacobian
|
||||||
TEST_F(CoreConstraintTest, JdotvFiniteDifference) {
|
TEST_F(CoreConstraintTest, JdotvFiniteDifference) {
|
||||||
|
for (const char* path :
|
||||||
for (const char* path : {kJdotvConnect2dPath,
|
{kJdotvConnect2dPath, kJdotvConnect3dPath, kJdotvWeld3dPath}) {
|
||||||
kJdotvConnect3dPath,
|
|
||||||
kJdotvWeld3dPath}) {
|
|
||||||
const std::string xml_path = GetTestDataFilePath(path);
|
const std::string xml_path = GetTestDataFilePath(path);
|
||||||
char err[1024];
|
char err[1024];
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err));
|
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err));
|
||||||
@@ -876,8 +841,8 @@ TEST_F(CoreConstraintTest, JdotvFiniteDifference) {
|
|||||||
// get dense J_0 (ne x nv)
|
// get dense J_0 (ne x nv)
|
||||||
std::vector<mjtNum> J0(ne * nv);
|
std::vector<mjtNum> J0(ne * nv);
|
||||||
if (mj_isSparse(m)) {
|
if (mj_isSparse(m)) {
|
||||||
mju_sparse2dense(J0.data(), d->efc_J, ne, nv,
|
mju_sparse2dense(J0.data(), d->efc_J, ne, nv, d->efc_J_rownnz,
|
||||||
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
|
d->efc_J_rowadr, d->efc_J_colind);
|
||||||
} else {
|
} else {
|
||||||
mju_copy(J0.data(), d->efc_J, ne * nv);
|
mju_copy(J0.data(), d->efc_J, ne * nv);
|
||||||
}
|
}
|
||||||
@@ -900,8 +865,8 @@ TEST_F(CoreConstraintTest, JdotvFiniteDifference) {
|
|||||||
ASSERT_EQ(d->ne, ne) << "constraint count changed after integration";
|
ASSERT_EQ(d->ne, ne) << "constraint count changed after integration";
|
||||||
std::vector<mjtNum> Jh(ne * nv);
|
std::vector<mjtNum> Jh(ne * nv);
|
||||||
if (mj_isSparse(m)) {
|
if (mj_isSparse(m)) {
|
||||||
mju_sparse2dense(Jh.data(), d->efc_J, ne, nv,
|
mju_sparse2dense(Jh.data(), d->efc_J, ne, nv, d->efc_J_rownnz,
|
||||||
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
|
d->efc_J_rowadr, d->efc_J_colind);
|
||||||
} else {
|
} else {
|
||||||
mju_copy(Jh.data(), d->efc_J, ne * nv);
|
mju_copy(Jh.data(), d->efc_J, ne * nv);
|
||||||
}
|
}
|
||||||
@@ -927,9 +892,8 @@ TEST_F(CoreConstraintTest, JdotvFiniteDifference) {
|
|||||||
|
|
||||||
// Test 2: forward-inverse identity preserved with Jdot*v correction
|
// Test 2: forward-inverse identity preserved with Jdot*v correction
|
||||||
TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) {
|
TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) {
|
||||||
for (const char* path : {kJdotvConnect2dPath,
|
for (const char* path :
|
||||||
kJdotvConnect3dPath,
|
{kJdotvConnect2dPath, kJdotvConnect3dPath, kJdotvWeld3dPath}) {
|
||||||
kJdotvWeld3dPath}) {
|
|
||||||
const std::string xml_path = GetTestDataFilePath(path);
|
const std::string xml_path = GetTestDataFilePath(path);
|
||||||
char err[1024];
|
char err[1024];
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err));
|
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err));
|
||||||
@@ -946,8 +910,7 @@ TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) {
|
|||||||
|
|
||||||
mjtNum epsilon = MjTol(1e-10, 1e-2);
|
mjtNum epsilon = MjTol(1e-10, 1e-2);
|
||||||
EXPECT_LT(fwdinv, epsilon)
|
EXPECT_LT(fwdinv, epsilon)
|
||||||
<< "fwdinv broken for " << path
|
<< "fwdinv broken for " << path << " (fwdinv=" << fwdinv << ")";
|
||||||
<< " (fwdinv=" << fwdinv << ")";
|
|
||||||
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
mj_deleteModel(m);
|
mj_deleteModel(m);
|
||||||
@@ -964,10 +927,9 @@ struct StrainConstraintTestCase {
|
|||||||
std::string flex_xyaxes;
|
std::string flex_xyaxes;
|
||||||
};
|
};
|
||||||
|
|
||||||
class StrainConstraintRotatedTest : public CoreConstraintTest,
|
class StrainConstraintRotatedTest
|
||||||
public ::testing::WithParamInterface<
|
: public CoreConstraintTest,
|
||||||
StrainConstraintTestCase> {
|
public ::testing::WithParamInterface<StrainConstraintTestCase> {};
|
||||||
};
|
|
||||||
|
|
||||||
TEST_P(StrainConstraintRotatedTest, ResidualIsZero) {
|
TEST_P(StrainConstraintRotatedTest, ResidualIsZero) {
|
||||||
auto param = GetParam();
|
auto param = GetParam();
|
||||||
@@ -1006,11 +968,11 @@ TEST_P(StrainConstraintRotatedTest, ResidualIsZero) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml.c_str(), error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml.c_str(), error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// Check we have strain constraints
|
// Check we have strain constraints
|
||||||
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
|
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
|
||||||
@@ -1021,24 +983,19 @@ TEST_P(StrainConstraintRotatedTest, ResidualIsZero) {
|
|||||||
for (int i = 0; i < d->ne; i++) {
|
for (int i = 0; i < d->ne; i++) {
|
||||||
max_pos = mju_max(max_pos, mju_abs(d->efc_pos[i]));
|
max_pos = mju_max(max_pos, mju_abs(d->efc_pos[i]));
|
||||||
}
|
}
|
||||||
EXPECT_LT(max_pos, 1e-6)
|
EXPECT_LT(max_pos, 1e-6) << "Strain constraint residual should be ~0"
|
||||||
<< "Strain constraint residual should be ~0"
|
|
||||||
<< " (max_pos=" << max_pos << ")";
|
<< " (max_pos=" << max_pos << ")";
|
||||||
|
|
||||||
// Verify stability
|
// Verify stability
|
||||||
for (int i = 0; i < 200; i++) {
|
for (int i = 0; i < 200; i++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
ASSERT_FALSE(mju_isBad(d->qpos[0]))
|
ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation unstable at step " << i;
|
||||||
<< "Simulation unstable at step " << i;
|
|
||||||
for (int j = 0; j < m->nv; j++) {
|
for (int j = 0; j < m->nv; j++) {
|
||||||
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
|
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
|
||||||
<< "Velocity exploded at step " << i
|
<< "Velocity exploded at step " << i << ", qvel[" << j
|
||||||
<< ", qvel[" << j << "]=" << d->qvel[j];
|
<< "]=" << d->qvel[j];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
INSTANTIATE_TEST_SUITE_P(
|
INSTANTIATE_TEST_SUITE_P(
|
||||||
@@ -1106,11 +1063,11 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, testing::NotNull()) << error;
|
ASSERT_THAT(model.get(), testing::NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
// find central vertex index (13 for 3x3x3 grid)
|
// find central vertex index (13 for 3x3x3 grid)
|
||||||
int central_idx = 13;
|
int central_idx = 13;
|
||||||
@@ -1121,7 +1078,8 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) {
|
|||||||
int rest = central_idx % (nx * ny);
|
int rest = central_idx % (nx * ny);
|
||||||
int j = rest / nx;
|
int j = rest / nx;
|
||||||
int i = rest % nx;
|
int i = rest % nx;
|
||||||
ASSERT_TRUE(i > 0 && i < nx-1 && j > 0 && j < ny-1 && k > 0 && k < nz-1);
|
ASSERT_TRUE(i > 0 && i < nx - 1 && j > 0 && j < ny - 1 && k > 0 &&
|
||||||
|
k < nz - 1);
|
||||||
|
|
||||||
// create manual contact with central vertex
|
// create manual contact with central vertex
|
||||||
mjContact con;
|
mjContact con;
|
||||||
@@ -1136,20 +1094,20 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) {
|
|||||||
con.vert[1] = central_idx;
|
con.vert[1] = central_idx;
|
||||||
con.dim = 1;
|
con.dim = 1;
|
||||||
mju_copy3(con.pos, data->flexvert_xpos + 3 * central_idx);
|
mju_copy3(con.pos, data->flexvert_xpos + 3 * central_idx);
|
||||||
con.frame[0] = 0; con.frame[1] = 0; con.frame[2] = 1; // normal
|
con.frame[0] = 0;
|
||||||
|
con.frame[1] = 0;
|
||||||
|
con.frame[2] = 1; // normal
|
||||||
|
|
||||||
// buffer for Jacobian
|
// buffer for Jacobian
|
||||||
std::vector<mjtNum> jacdif(3 * model->nv, 0.0);
|
std::vector<mjtNum> jacdif(3 * model->nv, 0.0);
|
||||||
|
|
||||||
// call mj_contactJacobian
|
// call mj_contactJacobian
|
||||||
mj_contactJacobian(model, data, &con, 1, nullptr, jacdif.data(),
|
mj_contactJacobian(model.get(), data.get(), &con, 1, nullptr, jacdif.data(),
|
||||||
nullptr, nullptr, nullptr,
|
nullptr, nullptr, nullptr, nullptr, nullptr, nullptr,
|
||||||
nullptr, nullptr, nullptr,
|
|
||||||
nullptr);
|
nullptr);
|
||||||
|
|
||||||
// check that boundary nodes have non-zero entries, and central node has zero
|
// check that boundary nodes have non-zero entries, and central node has zero
|
||||||
|
|
||||||
|
|
||||||
bool boundary_has_dof = false;
|
bool boundary_has_dof = false;
|
||||||
bool interior_has_dof = false;
|
bool interior_has_dof = false;
|
||||||
|
|
||||||
@@ -1185,9 +1143,6 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) {
|
|||||||
<< "Boundary nodes should receive contact force";
|
<< "Boundary nodes should receive contact force";
|
||||||
EXPECT_FALSE(interior_has_dof)
|
EXPECT_FALSE(interior_has_dof)
|
||||||
<< "Interior nodes should not receive contact force";
|
<< "Interior nodes should not receive contact force";
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
@@ -15,8 +15,6 @@
|
|||||||
// Tests for engine/engine_core_smooth.c.
|
// Tests for engine/engine_core_smooth.c.
|
||||||
|
|
||||||
#include "src/engine/engine_core_smooth.h"
|
#include "src/engine/engine_core_smooth.h"
|
||||||
#include "src/engine/engine_util_misc.h"
|
|
||||||
#include "src/engine/engine_util_sparse.h"
|
|
||||||
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <string>
|
#include <string>
|
||||||
@@ -30,6 +28,8 @@
|
|||||||
#include <mujoco/mjspec.h>
|
#include <mujoco/mjspec.h>
|
||||||
#include <mujoco/mjxmacro.h>
|
#include <mujoco/mjxmacro.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
|
#include "src/engine/engine_util_misc.h"
|
||||||
|
#include "src/engine/engine_util_sparse.h"
|
||||||
#include "test/fixture.h"
|
#include "test/fixture.h"
|
||||||
|
|
||||||
namespace mujoco {
|
namespace mujoco {
|
||||||
@@ -44,7 +44,6 @@ using ::testing::NotNull;
|
|||||||
using ::testing::Pointwise;
|
using ::testing::Pointwise;
|
||||||
using CoreSmoothTest = MujocoTest;
|
using CoreSmoothTest = MujocoTest;
|
||||||
|
|
||||||
|
|
||||||
constexpr bool EndsWith(std::string_view str, std::string_view suffix) {
|
constexpr bool EndsWith(std::string_view str, std::string_view suffix) {
|
||||||
return str.size() >= suffix.size() &&
|
return str.size() >= suffix.size() &&
|
||||||
str.substr(str.size() - suffix.size()) == suffix;
|
str.substr(str.size() - suffix.size()) == suffix;
|
||||||
@@ -64,12 +63,12 @@ TEST_F(CoreSmoothTest, MjDataWorldBodyValuesAreInitialized) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
mj_resetDataDebug(model, data, 'd');
|
mj_resetDataDebug(model.get(), data.get(), 'd');
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
mj_rnePostConstraint(model, data);
|
mj_rnePostConstraint(model.get(), data.get());
|
||||||
|
|
||||||
{
|
{
|
||||||
#define X(type, name, d0, d1) \
|
#define X(type, name, d0, d1) \
|
||||||
@@ -86,9 +85,6 @@ TEST_F(CoreSmoothTest, MjDataWorldBodyValuesAreInitialized) {
|
|||||||
MJDATA_POINTERS
|
MJDATA_POINTERS
|
||||||
#undef X
|
#undef X
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// --------------------------- mj_kinematics -----------------------------------
|
// --------------------------- mj_kinematics -----------------------------------
|
||||||
@@ -101,16 +97,13 @@ TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
mj_resetDataDebug(model, data, 'd');
|
mj_resetDataDebug(model.get(), data.get(), 'd');
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
EXPECT_THAT(AsVector(&data->xipos[0], 3), ElementsAre(0, 0, 0));
|
EXPECT_THAT(AsVector(&data->xipos[0], 3), ElementsAre(0, 0, 0));
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ----------------------------- mj_tendon -------------------------------------
|
// ----------------------------- mj_tendon -------------------------------------
|
||||||
@@ -145,19 +138,16 @@ TEST_F(CoreSmoothTest, FixedTendonSortedIndices) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
ASSERT_EQ(model->ntendon, 1);
|
ASSERT_EQ(model->ntendon, 1);
|
||||||
ASSERT_EQ(model->nwrap, 3);
|
ASSERT_EQ(model->nwrap, 3);
|
||||||
|
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
mj_fwdPosition(model, data);
|
mj_fwdPosition(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum* J = data->ten_J;
|
mjtNum* J = data->ten_J;
|
||||||
EXPECT_THAT(vector<mjtNum>(J, J + 3), ElementsAre(1, 2, 3));
|
EXPECT_THAT(vector<mjtNum>(J, J + 3), ElementsAre(1, 2, 3));
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static const char* const kTen_J0 = "engine/testdata/core_smooth/ten_J0.xml";
|
static const char* const kTen_J0 = "engine/testdata/core_smooth/ten_J0.xml";
|
||||||
@@ -250,8 +240,8 @@ TEST_F(CoreSmoothTest, TendonArmature) {
|
|||||||
int rowadr = m->ten_J_rowadr[j];
|
int rowadr = m->ten_J_rowadr[j];
|
||||||
int* rownnz = m->ten_J_rownnz + j;
|
int* rownnz = m->ten_J_rownnz + j;
|
||||||
int zero = 0;
|
int zero = 0;
|
||||||
mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv,
|
mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv, rownnz, &zero,
|
||||||
rownnz, &zero, m->ten_J_colind + rowadr);
|
m->ten_J_colind + rowadr);
|
||||||
|
|
||||||
// get tendon inertia only, using outer product
|
// get tendon inertia only, using outer product
|
||||||
mju_mulMatMat(ten_M.data(), ten_J.data(), ten_J.data(), nv, 1, nv);
|
mju_mulMatMat(ten_M.data(), ten_J.data(), ten_J.data(), nv, 1, nv);
|
||||||
@@ -393,14 +383,12 @@ TEST_F(CoreSmoothTest, TendonInertiaEquivalent) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// --------------------------- connect constraint ------------------------------
|
// --------------------------- connect constraint ------------------------------
|
||||||
|
|
||||||
// test that bodies hanging on connects lead to expected force sensor readings
|
// test that bodies hanging on connects lead to expected force sensor readings
|
||||||
void TestConnect(const char* const filepath) {
|
void TestConnect(const char* const filepath) {
|
||||||
const std::string xml_path = GetTestDataFilePath(filepath);
|
const std::string xml_path = GetTestDataFilePath(filepath);
|
||||||
mjModel* model =
|
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
|
||||||
mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
|
|
||||||
mjData* data = mj_makeData(model);
|
mjData* data = mj_makeData(model);
|
||||||
// settle physics:
|
// settle physics:
|
||||||
for (int i = 0; i < 1000; i++) {
|
for (int i = 0; i < 1000; i++) {
|
||||||
@@ -414,49 +402,42 @@ void TestConnect(const char* const filepath) {
|
|||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, RnePostConnectForceSlide) {
|
TEST_F(CoreSmoothTest, RnePostConnectForceSlide) {
|
||||||
constexpr char kModelFilePath[] =
|
constexpr char kModelFilePath[] =
|
||||||
"engine/testdata/core_smooth/rne_post/connect/force_slide.xml";
|
"engine/testdata/core_smooth/rne_post/connect/force_slide.xml";
|
||||||
TestConnect(kModelFilePath);
|
TestConnect(kModelFilePath);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, RnePostConnectForceSlideRotated) {
|
TEST_F(CoreSmoothTest, RnePostConnectForceSlideRotated) {
|
||||||
constexpr char kModelFilePath[] =
|
constexpr char kModelFilePath[] =
|
||||||
"engine/testdata/core_smooth/rne_post/connect/force_slide_rotated.xml";
|
"engine/testdata/core_smooth/rne_post/connect/force_slide_rotated.xml";
|
||||||
TestConnect(kModelFilePath);
|
TestConnect(kModelFilePath);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, RnePostConnectForceFree) {
|
TEST_F(CoreSmoothTest, RnePostConnectForceFree) {
|
||||||
constexpr char kModelFilePath[] =
|
constexpr char kModelFilePath[] =
|
||||||
"engine/testdata/core_smooth/rne_post/connect/force_free.xml";
|
"engine/testdata/core_smooth/rne_post/connect/force_free.xml";
|
||||||
TestConnect(kModelFilePath);
|
TestConnect(kModelFilePath);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, RnePostConnectTorque) {
|
TEST_F(CoreSmoothTest, RnePostConnectTorque) {
|
||||||
constexpr char kModelFilePath[] =
|
constexpr char kModelFilePath[] =
|
||||||
"engine/testdata/core_smooth/rne_post/connect/torque_free.xml";
|
"engine/testdata/core_smooth/rne_post/connect/torque_free.xml";
|
||||||
TestConnect(kModelFilePath);
|
TestConnect(kModelFilePath);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, RnePostConnectMultipleConstraints) {
|
TEST_F(CoreSmoothTest, RnePostConnectMultipleConstraints) {
|
||||||
constexpr char kModelFilePath[] =
|
constexpr char kModelFilePath[] =
|
||||||
"engine/testdata/core_smooth/rne_post/connect/multiple_constraints.xml";
|
"engine/testdata/core_smooth/rne_post/connect/multiple_constraints.xml";
|
||||||
TestConnect(kModelFilePath);
|
TestConnect(kModelFilePath);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// --------------------------- weld constraint ---------------------------------
|
// --------------------------- weld constraint ---------------------------------
|
||||||
|
|
||||||
// test that bodies attached with welds lead to expected force sensor readings
|
// test that bodies attached with welds lead to expected force sensor readings
|
||||||
void TestWeld(const char* const filepath) {
|
void TestWeld(const char* const filepath) {
|
||||||
const std::string xml_path = GetTestDataFilePath(filepath);
|
const std::string xml_path = GetTestDataFilePath(filepath);
|
||||||
mjModel* model =
|
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
|
||||||
mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
|
|
||||||
mjData* data = mj_makeData(model);
|
mjData* data = mj_makeData(model);
|
||||||
// settle physics:
|
// settle physics:
|
||||||
for (int i = 0; i < 1000; i++) {
|
for (int i = 0; i < 1000; i++) {
|
||||||
@@ -464,8 +445,7 @@ void TestWeld(const char* const filepath) {
|
|||||||
}
|
}
|
||||||
for (int sensor_index = 0; sensor_index < model->nsensor; sensor_index++) {
|
for (int sensor_index = 0; sensor_index < model->nsensor; sensor_index++) {
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
EXPECT_NEAR(
|
EXPECT_NEAR(data->sensordata[model->sensor_adr[sensor_index] + i],
|
||||||
data->sensordata[model->sensor_adr[sensor_index] + i],
|
|
||||||
model->sensor_user[model->nuser_sensor * sensor_index + i],
|
model->sensor_user[model->nuser_sensor * sensor_index + i],
|
||||||
MjTol(1e-6, 5e-5));
|
MjTol(1e-6, 5e-5));
|
||||||
}
|
}
|
||||||
@@ -474,56 +454,48 @@ void TestWeld(const char* const filepath) {
|
|||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, RnePostWeldForceFree) {
|
TEST_F(CoreSmoothTest, RnePostWeldForceFree) {
|
||||||
constexpr char kModelFilePath[] =
|
constexpr char kModelFilePath[] =
|
||||||
"engine/testdata/core_smooth/rne_post/weld/force_free.xml";
|
"engine/testdata/core_smooth/rne_post/weld/force_free.xml";
|
||||||
TestWeld(kModelFilePath);
|
TestWeld(kModelFilePath);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, RnePostWeldForceFreeRotated) {
|
TEST_F(CoreSmoothTest, RnePostWeldForceFreeRotated) {
|
||||||
constexpr char kModelFilePath[] =
|
constexpr char kModelFilePath[] =
|
||||||
"engine/testdata/core_smooth/rne_post/weld/force_free_rotated.xml";
|
"engine/testdata/core_smooth/rne_post/weld/force_free_rotated.xml";
|
||||||
TestWeld(kModelFilePath);
|
TestWeld(kModelFilePath);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFree) {
|
TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFree) {
|
||||||
constexpr char kModelFilePath[] =
|
constexpr char kModelFilePath[] =
|
||||||
"engine/testdata/core_smooth/rne_post/weld/force_torque_free.xml";
|
"engine/testdata/core_smooth/rne_post/weld/force_torque_free.xml";
|
||||||
TestWeld(kModelFilePath);
|
TestWeld(kModelFilePath);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFreeRotated) {
|
TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFreeRotated) {
|
||||||
constexpr char kModelFilePath[] =
|
constexpr char kModelFilePath[] =
|
||||||
"engine/testdata/core_smooth/rne_post/weld/force_torque_free_rotated.xml";
|
"engine/testdata/core_smooth/rne_post/weld/force_torque_free_rotated.xml";
|
||||||
TestWeld(kModelFilePath);
|
TestWeld(kModelFilePath);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, WeldRatioForceFree) {
|
TEST_F(CoreSmoothTest, WeldRatioForceFree) {
|
||||||
constexpr char kModelFilePath[] =
|
constexpr char kModelFilePath[] =
|
||||||
"engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml";
|
"engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml";
|
||||||
TestConnect(kModelFilePath);
|
TestConnect(kModelFilePath);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, WeldRatioForceSlide) {
|
TEST_F(CoreSmoothTest, WeldRatioForceSlide) {
|
||||||
constexpr char kModelFilePath[] =
|
constexpr char kModelFilePath[] =
|
||||||
"engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml";
|
"engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml";
|
||||||
TestConnect(kModelFilePath);
|
TestConnect(kModelFilePath);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, WeldRatioTorqueFree) {
|
TEST_F(CoreSmoothTest, WeldRatioTorqueFree) {
|
||||||
constexpr char kModelFilePath[] =
|
constexpr char kModelFilePath[] =
|
||||||
"engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml";
|
"engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml";
|
||||||
TestConnect(kModelFilePath);
|
TestConnect(kModelFilePath);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, WeldRatioForceSlideRotated) {
|
TEST_F(CoreSmoothTest, WeldRatioForceSlideRotated) {
|
||||||
constexpr char kModelFilePath[] =
|
constexpr char kModelFilePath[] =
|
||||||
"engine/testdata/core_smooth/rne_post/weld/"
|
"engine/testdata/core_smooth/rne_post/weld/"
|
||||||
@@ -601,7 +573,8 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
|
|||||||
|
|
||||||
// check that position matches target to within 1e-3 length units
|
// check that position matches target to within 1e-3 length units
|
||||||
mjtNum relpos[3];
|
mjtNum relpos[3];
|
||||||
mju_sub3(relpos, data->site_xpos+3*site_id, data->site_xpos+3*refsite_id);
|
mju_sub3(relpos, data->site_xpos + 3 * site_id,
|
||||||
|
data->site_xpos + 3 * refsite_id);
|
||||||
EXPECT_THAT(relpos, Pointwise(MjNear(1e-3, 5e-3), targetpos));
|
EXPECT_THAT(relpos, Pointwise(MjNear(1e-3, 5e-3), targetpos));
|
||||||
|
|
||||||
// check that orientation matches target to within 0.06 radians
|
// check that orientation matches target to within 0.06 radians
|
||||||
@@ -655,8 +628,8 @@ TEST_F(CoreSmoothTest, FactorI) {
|
|||||||
// dense L matrix
|
// dense L matrix
|
||||||
int nv = model->nv;
|
int nv = model->nv;
|
||||||
vector<mjtNum> Ldense(nv * nv, 0);
|
vector<mjtNum> Ldense(nv * nv, 0);
|
||||||
mju_sparse2dense(Ldense.data(), data->qLD, nv, nv,
|
mju_sparse2dense(Ldense.data(), data->qLD, nv, nv, model->M_rownnz,
|
||||||
model->M_rownnz, model->M_rowadr, model->M_colind);
|
model->M_rowadr, model->M_colind);
|
||||||
for (int i = 0; i < nv; i++) {
|
for (int i = 0; i < nv; i++) {
|
||||||
// set diagonal to 1
|
// set diagonal to 1
|
||||||
Ldense[i * nv + i] = 1;
|
Ldense[i * nv + i] = 1;
|
||||||
@@ -664,8 +637,8 @@ TEST_F(CoreSmoothTest, FactorI) {
|
|||||||
|
|
||||||
// dense D matrix
|
// dense D matrix
|
||||||
vector<mjtNum> Ddense(nv * nv);
|
vector<mjtNum> Ddense(nv * nv);
|
||||||
mju_sparse2dense(Ddense.data(), data->qLD, nv, nv,
|
mju_sparse2dense(Ddense.data(), data->qLD, nv, nv, model->M_rownnz,
|
||||||
model->M_rownnz, model->M_rowadr, model->M_colind);
|
model->M_rowadr, model->M_colind);
|
||||||
for (int i = 0; i < nv; i++) {
|
for (int i = 0; i < nv; i++) {
|
||||||
for (int j = 0; j < nv; j++) {
|
for (int j = 0; j < nv; j++) {
|
||||||
// zero everything except the diagonal
|
// zero everything except the diagonal
|
||||||
@@ -724,15 +697,16 @@ TEST_F(CoreSmoothTest, SolveLDs) {
|
|||||||
|
|
||||||
// compare LD and LDs densified matrices
|
// compare LD and LDs densified matrices
|
||||||
vector<mjtNum> LDdense(nv * nv);
|
vector<mjtNum> LDdense(nv * nv);
|
||||||
mju_sparse2dense(LDdense.data(), d->qLD, nv, nv,
|
mju_sparse2dense(LDdense.data(), d->qLD, nv, nv, m->M_rownnz, m->M_rowadr,
|
||||||
m->M_rownnz, m->M_rowadr, m->M_colind);
|
m->M_colind);
|
||||||
vector<mjtNum> LDdense2(nv * nv);
|
vector<mjtNum> LDdense2(nv * nv);
|
||||||
legacyToDense(m, LDdense2.data(), LDlegacy.data());
|
legacyToDense(m, LDdense2.data(), LDlegacy.data());
|
||||||
|
|
||||||
// expect lower triangles to match exactly
|
// expect lower triangles to match exactly
|
||||||
for (int i = 0; i < nv; i++) {
|
for (int i = 0; i < nv; i++) {
|
||||||
for (int j = 0; j < i; j++) {
|
for (int j = 0; j < i; j++) {
|
||||||
EXPECT_NEAR(LDdense[i*nv+j], LDdense2[i*nv+j], MjTol(1e-14, 1e-6));
|
EXPECT_NEAR(LDdense[i * nv + j], LDdense2[i * nv + j],
|
||||||
|
MjTol(1e-14, 1e-6));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -743,8 +717,8 @@ TEST_F(CoreSmoothTest, SolveLDs) {
|
|||||||
for (int i = 0; i < nv; i += 2) vec[i] = vec2[i] = 0;
|
for (int i = 0; i < nv; i += 2) vec[i] = vec2[i] = 0;
|
||||||
|
|
||||||
mj_solveLD_legacy(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv);
|
mj_solveLD_legacy(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv);
|
||||||
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1,
|
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1, m->M_rownnz, m->M_rowadr,
|
||||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
m->M_colind, nullptr);
|
||||||
|
|
||||||
// expect vectors to match up to floating point precision
|
// expect vectors to match up to floating point precision
|
||||||
for (int i = 0; i < nv; i++) {
|
for (int i = 0; i < nv; i++) {
|
||||||
@@ -778,8 +752,8 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) {
|
|||||||
for (int i = 0; i < nv * n; i += 3) vec[i] = vec2[i] = 0;
|
for (int i = 0; i < nv * n; i += 3) vec[i] = vec2[i] = 0;
|
||||||
|
|
||||||
mj_solveLD_legacy(m, vec.data(), n, LDlegacy.data(), d->qLDiagInv);
|
mj_solveLD_legacy(m, vec.data(), n, LDlegacy.data(), d->qLDiagInv);
|
||||||
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n,
|
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, m->M_rownnz, m->M_rowadr,
|
||||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
m->M_colind, nullptr);
|
||||||
|
|
||||||
// expect vectors to match up to floating point precision
|
// expect vectors to match up to floating point precision
|
||||||
for (int i = 0; i < nv * n; i++) {
|
for (int i = 0; i < nv * n; i++) {
|
||||||
@@ -816,8 +790,8 @@ TEST_F(CoreSmoothTest, SolveM2) {
|
|||||||
vector<mjtNum> res(nv * n);
|
vector<mjtNum> res(nv * n);
|
||||||
|
|
||||||
mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n);
|
mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n);
|
||||||
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n,
|
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, m->M_rownnz, m->M_rowadr,
|
||||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
m->M_colind, nullptr);
|
||||||
|
|
||||||
// expect equality of dot(v, M^-1 * v) and dot(M^-1/2 * v, M^-1/2 * v)
|
// expect equality of dot(v, M^-1 * v) and dot(M^-1/2 * v, M^-1/2 * v)
|
||||||
for (int i = 0; i < n; i++) {
|
for (int i = 0; i < n; i++) {
|
||||||
@@ -856,8 +830,8 @@ TEST_F(CoreSmoothTest, FactorIs) {
|
|||||||
vector<mjtNum> qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv);
|
vector<mjtNum> qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv);
|
||||||
vector<mjtNum> qLDiagInv(nv, 0);
|
vector<mjtNum> qLDiagInv(nv, 0);
|
||||||
|
|
||||||
mj_factorI(qLD.data(), qLDiagInv.data(), nv,
|
mj_factorI(qLD.data(), qLDiagInv.data(), nv, m->M_rownnz, m->M_rowadr,
|
||||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
m->M_colind, nullptr);
|
||||||
|
|
||||||
// expect outputs to match to floating point precision
|
// expect outputs to match to floating point precision
|
||||||
EXPECT_THAT(qLD, Pointwise(MjNear(1e-12, 1e-4), qLDexpected));
|
EXPECT_THAT(qLD, Pointwise(MjNear(1e-12, 1e-4), qLDexpected));
|
||||||
@@ -891,9 +865,9 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
// check that nJfv is correct:
|
// check that nJfv is correct:
|
||||||
// corner vertices: 2 * (1+3) * 3 + 2 * (1+2) * 3 = 42
|
// corner vertices: 2 * (1+3) * 3 + 2 * (1+2) * 3 = 42
|
||||||
@@ -903,7 +877,7 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
|
|||||||
EXPECT_EQ(m->nJfv, 123);
|
EXPECT_EQ(m->nJfv, 123);
|
||||||
|
|
||||||
// Run kinematics to populate xpos/xmat initially
|
// Run kinematics to populate xpos/xmat initially
|
||||||
mj_fwdKinematics(m, d);
|
mj_fwdKinematics(m.get(), d.get());
|
||||||
|
|
||||||
// Check invariants for scale=1
|
// Check invariants for scale=1
|
||||||
// The constraints should be satisfied
|
// The constraints should be satisfied
|
||||||
@@ -979,8 +953,8 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
|
|||||||
auto fd_check = [&](double tolerance) {
|
auto fd_check = [&](double tolerance) {
|
||||||
std::vector<mjtNum> qpos0(m->nq);
|
std::vector<mjtNum> qpos0(m->nq);
|
||||||
mju_copy(qpos0.data(), d->qpos, m->nq);
|
mju_copy(qpos0.data(), d->qpos, m->nq);
|
||||||
mj_kinematics(m, d);
|
mj_kinematics(m.get(), d.get());
|
||||||
mj_flex(m, d);
|
mj_flex(m.get(), d.get());
|
||||||
|
|
||||||
mjtNum eps = MjTol(1e-6, 1e-4);
|
mjtNum eps = MjTol(1e-6, 1e-4);
|
||||||
int nflexvert = m->flex_vertnum[0];
|
int nflexvert = m->flex_vertnum[0];
|
||||||
@@ -994,9 +968,9 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
|
|||||||
|
|
||||||
// plus
|
// plus
|
||||||
mju_copy(d->qpos, qpos_backup.data(), m->nq);
|
mju_copy(d->qpos, qpos_backup.data(), m->nq);
|
||||||
mj_integratePos(m, d->qpos, qvel.data(), eps);
|
mj_integratePos(m.get(), d->qpos, qvel.data(), eps);
|
||||||
mj_kinematics(m, d);
|
mj_kinematics(m.get(), d.get());
|
||||||
mj_flex(m, d);
|
mj_flex(m.get(), d.get());
|
||||||
std::vector<mjtNum> L_plus(2 * nflexvert);
|
std::vector<mjtNum> L_plus(2 * nflexvert);
|
||||||
for (int e = 0; e < 2 * nflexvert; ++e) {
|
for (int e = 0; e < 2 * nflexvert; ++e) {
|
||||||
L_plus[e] = d->flexvert_length[e];
|
L_plus[e] = d->flexvert_length[e];
|
||||||
@@ -1004,9 +978,9 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
|
|||||||
|
|
||||||
// minus
|
// minus
|
||||||
mju_copy(d->qpos, qpos_backup.data(), m->nq);
|
mju_copy(d->qpos, qpos_backup.data(), m->nq);
|
||||||
mj_integratePos(m, d->qpos, qvel.data(), -eps);
|
mj_integratePos(m.get(), d->qpos, qvel.data(), -eps);
|
||||||
mj_kinematics(m, d);
|
mj_kinematics(m.get(), d.get());
|
||||||
mj_flex(m, d);
|
mj_flex(m.get(), d.get());
|
||||||
std::vector<mjtNum> L_minus(2 * nflexvert);
|
std::vector<mjtNum> L_minus(2 * nflexvert);
|
||||||
for (int e = 0; e < 2 * nflexvert; ++e) {
|
for (int e = 0; e < 2 * nflexvert; ++e) {
|
||||||
L_minus[e] = d->flexvert_length[e];
|
L_minus[e] = d->flexvert_length[e];
|
||||||
@@ -1017,8 +991,8 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
mju_copy(d->qpos, qpos_backup.data(), m->nq);
|
mju_copy(d->qpos, qpos_backup.data(), m->nq);
|
||||||
mj_kinematics(m, d);
|
mj_kinematics(m.get(), d.get());
|
||||||
mj_flex(m, d);
|
mj_flex(m.get(), d.get());
|
||||||
|
|
||||||
// Compare with analytic
|
// Compare with analytic
|
||||||
std::vector<mjtNum> jac_analytic(2 * nflexvert * m->nv);
|
std::vector<mjtNum> jac_analytic(2 * nflexvert * m->nv);
|
||||||
@@ -1035,8 +1009,8 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
|
|||||||
EXPECT_THAT(jac_analytic, Pointwise(MjNear(tolerance, 1e-1), jac_fd));
|
EXPECT_THAT(jac_analytic, Pointwise(MjNear(tolerance, 1e-1), jac_fd));
|
||||||
|
|
||||||
mju_copy(d->qpos, qpos0.data(), m->nq);
|
mju_copy(d->qpos, qpos0.data(), m->nq);
|
||||||
mj_kinematics(m, d);
|
mj_kinematics(m.get(), d.get());
|
||||||
mj_flex(m, d);
|
mj_flex(m.get(), d.get());
|
||||||
};
|
};
|
||||||
|
|
||||||
fd_check(MjTol(5e-5, 5e-2));
|
fd_check(MjTol(5e-5, 5e-2));
|
||||||
@@ -1047,7 +1021,7 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
|
|||||||
d->qpos[3 * i + 1] = d->xpos[3 * (i + 1) + 1];
|
d->qpos[3 * i + 1] = d->xpos[3 * (i + 1) + 1];
|
||||||
d->qpos[3 * i + 2] = d->xpos[3 * (i + 1) + 2];
|
d->qpos[3 * i + 2] = d->xpos[3 * (i + 1) + 2];
|
||||||
}
|
}
|
||||||
mj_fwdKinematics(m, d);
|
mj_fwdKinematics(m.get(), d.get());
|
||||||
|
|
||||||
// Get mass scaling factor
|
// Get mass scaling factor
|
||||||
mjtNum scale = 1.0;
|
mjtNum scale = 1.0;
|
||||||
@@ -1072,9 +1046,6 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
|
|||||||
d->qpos[3 * i + 2] += 0.01 * (i % 2 ? 1 : -1);
|
d->qpos[3 * i + 2] += 0.01 * (i % 2 ? 1 : -1);
|
||||||
}
|
}
|
||||||
fd_check(5e-5);
|
fd_check(5e-5);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Test failure case for flexvert_J sparsity with skipped flexes
|
// Test failure case for flexvert_J sparsity with skipped flexes
|
||||||
@@ -1107,12 +1078,12 @@ TEST_F(CoreSmoothTest, FlexvertJSparsitySkippedFlex) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// Forward dynamics to compute Jacobians
|
// Forward dynamics to compute Jacobians
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
// Check sparsity overlap
|
// Check sparsity overlap
|
||||||
// Flex 0 starts at row 0
|
// Flex 0 starts at row 0
|
||||||
@@ -1136,9 +1107,6 @@ TEST_F(CoreSmoothTest, FlexvertJSparsitySkippedFlex) {
|
|||||||
// Verify that Flex 2 starts AFTER Flex 0 ends
|
// Verify that Flex 2 starts AFTER Flex 0 ends
|
||||||
EXPECT_GE(f2_start_adr, f0_end_adr)
|
EXPECT_GE(f2_start_adr, f0_end_adr)
|
||||||
<< "Flex 2 Jacobian overwrites Flex 0 Jacobian due to skipped Flex 1";
|
<< "Flex 2 Jacobian overwrites Flex 0 Jacobian due to skipped Flex 1";
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Test stability of flexvert constraint under different integrator/solver
|
// Test stability of flexvert constraint under different integrator/solver
|
||||||
|
|||||||
@@ -49,10 +49,10 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
ASSERT_EQ(model->nflex, 1);
|
ASSERT_EQ(model->nflex, 1);
|
||||||
int f = 0;
|
int f = 0;
|
||||||
@@ -74,7 +74,7 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
std::vector<mjtNum> xpos(3 * nodenum);
|
std::vector<mjtNum> xpos(3 * nodenum);
|
||||||
mju_flexGatherState(model, data, f, xpos.data(), NULL);
|
mju_flexGatherState(model.get(), data.get(), f, xpos.data(), NULL);
|
||||||
|
|
||||||
// Verify that gathered xpos matches the rotated data->xpos
|
// Verify that gathered xpos matches the rotated data->xpos
|
||||||
for (int i = 0; i < nodenum; i++) {
|
for (int i = 0; i < nodenum; i++) {
|
||||||
@@ -83,9 +83,6 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) {
|
|||||||
EXPECT_NEAR(xpos[3 * i + 1], data->xpos[3 * b + 1], 1e-5);
|
EXPECT_NEAR(xpos[3 * i + 1], data->xpos[3 * b + 1], 1e-5);
|
||||||
EXPECT_NEAR(xpos[3 * i + 2], data->xpos[3 * b + 2], 1e-5);
|
EXPECT_NEAR(xpos[3 * i + 2], data->xpos[3 * b + 2], 1e-5);
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) {
|
TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) {
|
||||||
@@ -102,15 +99,15 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
ASSERT_EQ(model->nflex, 1);
|
ASSERT_EQ(model->nflex, 1);
|
||||||
int f = 0;
|
int f = 0;
|
||||||
model->flex_interp[f] = -1;
|
model->flex_interp[f] = -1;
|
||||||
|
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
int nodenum = model->flex_nodenum[f];
|
int nodenum = model->flex_nodenum[f];
|
||||||
int nstart = model->flex_nodeadr[f];
|
int nstart = model->flex_nodeadr[f];
|
||||||
@@ -126,18 +123,15 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
std::vector<mjtNum> xpos(3 * nodenum);
|
std::vector<mjtNum> xpos(3 * nodenum);
|
||||||
mju_flexGatherState(model, data, f, xpos.data(), NULL);
|
mju_flexGatherState(model.get(), data.get(), f, xpos.data(), NULL);
|
||||||
|
|
||||||
// Verify that gathered xpos for center node (13) is the TFI reconstructed position
|
// Verify that gathered xpos for center node (13) is the TFI reconstructed
|
||||||
|
// position
|
||||||
EXPECT_NEAR(xpos[3 * 13 + 0], shift[0], 1e-5);
|
EXPECT_NEAR(xpos[3 * 13 + 0], shift[0], 1e-5);
|
||||||
EXPECT_NEAR(xpos[3 * 13 + 1], shift[1], 1e-5);
|
EXPECT_NEAR(xpos[3 * 13 + 1], shift[1], 1e-5);
|
||||||
EXPECT_NEAR(xpos[3 * 13 + 2], shift[2], 1e-5);
|
EXPECT_NEAR(xpos[3 * 13 + 2], shift[2], 1e-5);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
using AngMomMatTest = MujocoTest;
|
using AngMomMatTest = MujocoTest;
|
||||||
|
|
||||||
static constexpr char AngMomTestingModel[] = R"(
|
static constexpr char AngMomTestingModel[] = R"(
|
||||||
@@ -172,26 +166,26 @@ static constexpr char AngMomTestingModel[] = R"(
|
|||||||
// compare subtree angular momentum computed in two ways
|
// compare subtree angular momentum computed in two ways
|
||||||
TEST_F(AngMomMatTest, CompareAngMom) {
|
TEST_F(AngMomMatTest, CompareAngMom) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model =
|
MjModelPtr model =
|
||||||
LoadModelFromString(AngMomTestingModel, error, sizeof(error));
|
LoadModelFromString(AngMomTestingModel, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
int nv = model->nv;
|
int nv = model->nv;
|
||||||
int bodyid = mj_name2id(model, mjOBJ_BODY, "link1");
|
int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "link1");
|
||||||
|
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// reset to the keyframe with some angular velocities
|
// reset to the keyframe with some angular velocities
|
||||||
mj_resetDataKeyframe(model, data, 0);
|
mj_resetDataKeyframe(model.get(), data.get(), 0);
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
// get the reference value of angular momentum
|
// get the reference value of angular momentum
|
||||||
mj_subtreeVel(model, data);
|
mj_subtreeVel(model.get(), data.get());
|
||||||
mjtNum angmom_ref[3];
|
mjtNum angmom_ref[3];
|
||||||
mju_copy3(angmom_ref, data->subtree_angmom + 3 * bodyid);
|
mju_copy3(angmom_ref, data->subtree_angmom + 3 * bodyid);
|
||||||
|
|
||||||
// compute angular momentum using the angular momentum matrix
|
// compute angular momentum using the angular momentum matrix
|
||||||
mjtNum* angmom_mat = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
|
mjtNum* angmom_mat = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
|
||||||
mj_angmomMat(model, data, angmom_mat, bodyid);
|
mj_angmomMat(model.get(), data.get(), angmom_mat, bodyid);
|
||||||
mjtNum angmom_test[3];
|
mjtNum angmom_test[3];
|
||||||
mju_mulMatVec(angmom_test, angmom_mat, data->qvel, 3, nv);
|
mju_mulMatVec(angmom_test, angmom_mat, data->qvel, 3, nv);
|
||||||
|
|
||||||
@@ -201,28 +195,26 @@ TEST_F(AngMomMatTest, CompareAngMom) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
mju_free(angmom_mat);
|
mju_free(angmom_mat);
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// compare subtree angular momentum matrix: analytical and findiff
|
// compare subtree angular momentum matrix: analytical and findiff
|
||||||
TEST_F(AngMomMatTest, CompareAngMomMats) {
|
TEST_F(AngMomMatTest, CompareAngMomMats) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model =
|
MjModelPtr model =
|
||||||
LoadModelFromString(AngMomTestingModel, error, sizeof(error));
|
LoadModelFromString(AngMomTestingModel, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
int nv = model->nv;
|
int nv = model->nv;
|
||||||
int bodyid = mj_name2id(model, mjOBJ_BODY, "link1");
|
int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "link1");
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
mjtNum* angmom_mat = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
|
mjtNum* angmom_mat = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
|
||||||
mjtNum* angmom_mat_fd = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
|
mjtNum* angmom_mat_fd = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
|
||||||
|
|
||||||
// reset to the keyframe with some angular velocities
|
// reset to the keyframe with some angular velocities
|
||||||
mj_resetDataKeyframe(model, data, 0);
|
mj_resetDataKeyframe(model.get(), data.get(), 0);
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
// compute the angular momentum matrix using the analytical method
|
// compute the angular momentum matrix using the analytical method
|
||||||
mj_angmomMat(model, data, angmom_mat, bodyid);
|
mj_angmomMat(model.get(), data.get(), angmom_mat, bodyid);
|
||||||
|
|
||||||
// compute the angular momentum matrix using finite differences
|
// compute the angular momentum matrix using finite differences
|
||||||
static const mjtNum eps = MjTol(1e-6, 1e-3);
|
static const mjtNum eps = MjTol(1e-6, 1e-3);
|
||||||
@@ -230,16 +222,16 @@ TEST_F(AngMomMatTest, CompareAngMomMats) {
|
|||||||
// reset vel, forward nudge i-th dof, get angmom
|
// reset vel, forward nudge i-th dof, get angmom
|
||||||
mju_copy(data->qvel, model->key_qvel, model->nv);
|
mju_copy(data->qvel, model->key_qvel, model->nv);
|
||||||
data->qvel[i] += eps;
|
data->qvel[i] += eps;
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
mj_subtreeVel(model, data);
|
mj_subtreeVel(model.get(), data.get());
|
||||||
mjtNum agmf[3];
|
mjtNum agmf[3];
|
||||||
mju_copy3(agmf, data->subtree_angmom + 3 * bodyid);
|
mju_copy3(agmf, data->subtree_angmom + 3 * bodyid);
|
||||||
|
|
||||||
// reset vel, backward nudge i-th dof, get angmom
|
// reset vel, backward nudge i-th dof, get angmom
|
||||||
mju_copy(data->qvel, model->key_qvel, model->nv);
|
mju_copy(data->qvel, model->key_qvel, model->nv);
|
||||||
data->qvel[i] -= eps;
|
data->qvel[i] -= eps;
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
mj_subtreeVel(model, data);
|
mj_subtreeVel(model.get(), data.get());
|
||||||
mjtNum agmb[3];
|
mjtNum agmb[3];
|
||||||
mju_copy3(agmb, data->subtree_angmom + 3 * bodyid);
|
mju_copy3(agmb, data->subtree_angmom + 3 * bodyid);
|
||||||
|
|
||||||
@@ -256,8 +248,6 @@ TEST_F(AngMomMatTest, CompareAngMomMats) {
|
|||||||
|
|
||||||
mju_free(angmom_mat_fd);
|
mju_free(angmom_mat_fd);
|
||||||
mju_free(angmom_mat);
|
mju_free(angmom_mat);
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
using JacobianTest = MujocoTest;
|
using JacobianTest = MujocoTest;
|
||||||
@@ -297,22 +287,22 @@ static constexpr char kJacobianTestingModel[] = R"(
|
|||||||
// compare analytic and finite-differenced subtree-com Jacobian
|
// compare analytic and finite-differenced subtree-com Jacobian
|
||||||
TEST_F(JacobianTest, SubtreeJac) {
|
TEST_F(JacobianTest, SubtreeJac) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model =
|
MjModelPtr model =
|
||||||
LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
|
LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
int nv = model->nv;
|
int nv = model->nv;
|
||||||
int bodyid = mj_name2id(model, mjOBJ_BODY, "main");
|
int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "main");
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
mjtNum* jac_subtree = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
|
mjtNum* jac_subtree = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
|
||||||
mjtNum* qpos = (mjtNum*)mju_malloc(sizeof(mjtNum) * model->nq);
|
mjtNum* qpos = (mjtNum*)mju_malloc(sizeof(mjtNum) * model->nq);
|
||||||
mjtNum* nudge = (mjtNum*)mju_malloc(sizeof(mjtNum) * nv);
|
mjtNum* nudge = (mjtNum*)mju_malloc(sizeof(mjtNum) * nv);
|
||||||
|
|
||||||
// all we need for Jacobians are kinematics and CoM-related quantities
|
// all we need for Jacobians are kinematics and CoM-related quantities
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
mj_comPos(model, data);
|
mj_comPos(model.get(), data.get());
|
||||||
|
|
||||||
// get subtree CoM Jacobian of free body
|
// get subtree CoM Jacobian of free body
|
||||||
mj_jacSubtreeCom(model, data, jac_subtree, bodyid);
|
mj_jacSubtreeCom(model.get(), data.get(), jac_subtree, bodyid);
|
||||||
|
|
||||||
// save current subtree-com and qpos, clear nudge
|
// save current subtree-com and qpos, clear nudge
|
||||||
mjtNum subtree_com[3];
|
mjtNum subtree_com[3];
|
||||||
@@ -326,16 +316,17 @@ TEST_F(JacobianTest, SubtreeJac) {
|
|||||||
// reset qpos, nudge i-th dof, update data->qpos, reset nudge
|
// reset qpos, nudge i-th dof, update data->qpos, reset nudge
|
||||||
mju_copy(data->qpos, qpos, model->nq);
|
mju_copy(data->qpos, qpos, model->nq);
|
||||||
nudge[i] = 1;
|
nudge[i] = 1;
|
||||||
mj_integratePos(model, data->qpos, nudge, eps);
|
mj_integratePos(model.get(), data->qpos, nudge, eps);
|
||||||
nudge[i] = 0;
|
nudge[i] = 0;
|
||||||
|
|
||||||
// kinematics and comPos to get nudged com
|
// kinematics and comPos to get nudged com
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
mj_comPos(model, data);
|
mj_comPos(model.get(), data.get());
|
||||||
|
|
||||||
// compare finite-differenced and analytic Jacobian
|
// compare finite-differenced and analytic Jacobian
|
||||||
for (int j = 0; j < 3; j++) {
|
for (int j = 0; j < 3; j++) {
|
||||||
mjtNum findiff = (data->subtree_com[3*bodyid+j] - subtree_com[j]) / eps;
|
mjtNum findiff =
|
||||||
|
(data->subtree_com[3 * bodyid + j] - subtree_com[j]) / eps;
|
||||||
EXPECT_THAT(jac_subtree[nv * j + i], MjNear(findiff, eps, 1e-2));
|
EXPECT_THAT(jac_subtree[nv * j + i], MjNear(findiff, eps, 1e-2));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -343,38 +334,36 @@ TEST_F(JacobianTest, SubtreeJac) {
|
|||||||
mju_free(nudge);
|
mju_free(nudge);
|
||||||
mju_free(qpos);
|
mju_free(qpos);
|
||||||
mju_free(jac_subtree);
|
mju_free(jac_subtree);
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// confirm that applying linear forces via the subtree-com Jacobian only creates
|
// confirm that applying linear forces via the subtree-com Jacobian only creates
|
||||||
// the expected linear accelerations (no accelerations of internal joints)
|
// the expected linear accelerations (no accelerations of internal joints)
|
||||||
TEST_F(JacobianTest, SubtreeJacNoInternalAcc) {
|
TEST_F(JacobianTest, SubtreeJacNoInternalAcc) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model =
|
MjModelPtr model =
|
||||||
LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
|
LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
int nv = model->nv;
|
int nv = model->nv;
|
||||||
int bodyid = mj_name2id(model, mjOBJ_BODY, "main");
|
int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "main");
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
mjtNum* jac_subtree = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
|
mjtNum* jac_subtree = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
|
||||||
|
|
||||||
// all we need for Jacobians are kinematics and CoM-related quantities
|
// all we need for Jacobians are kinematics and CoM-related quantities
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
mj_comPos(model, data);
|
mj_comPos(model.get(), data.get());
|
||||||
|
|
||||||
// get subtree CoM Jacobian of free body
|
// get subtree CoM Jacobian of free body
|
||||||
mj_jacSubtreeCom(model, data, jac_subtree, bodyid);
|
mj_jacSubtreeCom(model.get(), data.get(), jac_subtree, bodyid);
|
||||||
|
|
||||||
// uncomment for debugging
|
// uncomment for debugging
|
||||||
// mju_printMat(jac_subtree, 3, nv);
|
// mju_printMat(jac_subtree, 3, nv);
|
||||||
|
|
||||||
// call fwdPosition since we'll need the factorised mass matrix in the test
|
// call fwdPosition since we'll need the factorised mass matrix in the test
|
||||||
mj_fwdPosition(model, data);
|
mj_fwdPosition(model.get(), data.get());
|
||||||
|
|
||||||
// treating the subtree Jacobian as the projection of 3 axis-aligned unit
|
// treating the subtree Jacobian as the projection of 3 axis-aligned unit
|
||||||
// forces into joint space, solve for the resulting accelerations in-place
|
// forces into joint space, solve for the resulting accelerations in-place
|
||||||
mj_solveM(model, data, jac_subtree, jac_subtree, 3);
|
mj_solveM(model.get(), data.get(), jac_subtree, jac_subtree, 3);
|
||||||
|
|
||||||
// expect to find accelerations of magnitude 1/subtreemass in the first 3
|
// expect to find accelerations of magnitude 1/subtreemass in the first 3
|
||||||
// coordinates of the free joint and 0s elsewhere, since applying forces to
|
// coordinates of the free joint and 0s elsewhere, since applying forces to
|
||||||
@@ -389,8 +378,6 @@ TEST_F(JacobianTest, SubtreeJacNoInternalAcc) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
mju_free(jac_subtree);
|
mju_free(jac_subtree);
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static constexpr char kQuat[] = R"(
|
static constexpr char kQuat[] = R"(
|
||||||
@@ -519,28 +506,28 @@ static constexpr char kHinge[] = R"(
|
|||||||
TEST_F(JacobianTest, JacDot) {
|
TEST_F(JacobianTest, JacDot) {
|
||||||
for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) {
|
for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
int nv = model->nv;
|
int nv = model->nv;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// load keyframe if present, step for a bit
|
// load keyframe if present, step for a bit
|
||||||
if (model->nkey) mj_resetDataKeyframe(model, data, 0);
|
if (model->nkey) mj_resetDataKeyframe(model.get(), data.get(), 0);
|
||||||
while (data->time < 0.1) {
|
while (data->time < 0.1) {
|
||||||
mj_step(model, data);
|
mj_step(model.get(), data.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
// minimal call required for mj_jacDot outputs to be valid
|
// minimal call required for mj_jacDot outputs to be valid
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
mj_comPos(model, data);
|
mj_comPos(model.get(), data.get());
|
||||||
mj_comVel(model, data);
|
mj_comVel(model.get(), data.get());
|
||||||
|
|
||||||
// get bodyid
|
// get bodyid
|
||||||
int bodyid = mj_name2id(model, mjOBJ_BODY, "query");
|
int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "query");
|
||||||
EXPECT_GT(bodyid, 0);
|
EXPECT_GT(bodyid, 0);
|
||||||
|
|
||||||
// get site position
|
// get site position
|
||||||
int siteid = mj_name2id(model, mjOBJ_SITE, "query");
|
int siteid = mj_name2id(model.get(), mjOBJ_SITE, "query");
|
||||||
EXPECT_GT(siteid, -1);
|
EXPECT_GT(siteid, -1);
|
||||||
mjtNum point[3];
|
mjtNum point[3];
|
||||||
mju_copy3(point, data->site_xpos + 3 * siteid);
|
mju_copy3(point, data->site_xpos + 3 * siteid);
|
||||||
@@ -548,20 +535,23 @@ TEST_F(JacobianTest, JacDot) {
|
|||||||
// jac, jac_dot
|
// jac, jac_dot
|
||||||
std::vector<mjtNum> jacp(3 * nv);
|
std::vector<mjtNum> jacp(3 * nv);
|
||||||
std::vector<mjtNum> jacr(3 * nv);
|
std::vector<mjtNum> jacr(3 * nv);
|
||||||
mj_jac(model, data, jacp.data(), jacr.data(), point, bodyid);
|
mj_jac(model.get(), data.get(), jacp.data(), jacr.data(), point, bodyid);
|
||||||
std::vector<mjtNum> jacp_dot(3 * nv);
|
std::vector<mjtNum> jacp_dot(3 * nv);
|
||||||
std::vector<mjtNum> jacr_dot(3 * nv);
|
std::vector<mjtNum> jacr_dot(3 * nv);
|
||||||
mj_jacDot(model, data, jacp_dot.data(), jacr_dot.data(), point, bodyid);
|
mj_jacDot(model.get(), data.get(), jacp_dot.data(), jacr_dot.data(), point,
|
||||||
|
bodyid);
|
||||||
|
|
||||||
// jac_h: jacobian after integrating qpos with a timestep of h
|
// jac_h: jacobian after integrating qpos with a timestep of h
|
||||||
const mjtNum h = MjTol(1e-7, 5e-4);
|
const mjtNum h = MjTol(1e-7, 5e-4);
|
||||||
mj_integratePos(model, data->qpos, data->qvel, h);
|
mj_integratePos(model.get(), data->qpos, data->qvel, h);
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
mj_comPos(model, data);
|
mj_comPos(model.get(), data.get());
|
||||||
std::vector<mjtNum> jacp_h(3 * nv);
|
std::vector<mjtNum> jacp_h(3 * nv);
|
||||||
std::vector<mjtNum> jacr_h(3 * nv);
|
std::vector<mjtNum> jacr_h(3 * nv);
|
||||||
mju_copy3(point, data->site_xpos+3*siteid); // get updated site position
|
mju_copy3(point,
|
||||||
mj_jac(model, data, jacp_h.data(), jacr_h.data(), point, bodyid);
|
data->site_xpos + 3 * siteid); // get updated site position
|
||||||
|
mj_jac(model.get(), data.get(), jacp_h.data(), jacr_h.data(), point,
|
||||||
|
bodyid);
|
||||||
|
|
||||||
// jac_dot_h finite-difference approximation
|
// jac_dot_h finite-difference approximation
|
||||||
std::vector<mjtNum> jacp_dot_h(3 * nv);
|
std::vector<mjtNum> jacp_dot_h(3 * nv);
|
||||||
@@ -575,9 +565,6 @@ TEST_F(JacobianTest, JacDot) {
|
|||||||
mjtNum tol = 1e-5;
|
mjtNum tol = 1e-5;
|
||||||
EXPECT_THAT(jacp_dot, Pointwise(MjNear(tol, 5e-2), jacp_dot_h));
|
EXPECT_THAT(jacp_dot, Pointwise(MjNear(tol, 5e-2), jacp_dot_h));
|
||||||
EXPECT_THAT(jacr_dot, Pointwise(MjNear(tol, 5e-2), jacr_dot_h));
|
EXPECT_THAT(jacr_dot, Pointwise(MjNear(tol, 5e-2), jacr_dot_h));
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -585,26 +572,26 @@ TEST_F(JacobianTest, JacDot) {
|
|||||||
TEST_F(JacobianTest, JacDotSparse) {
|
TEST_F(JacobianTest, JacDotSparse) {
|
||||||
for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) {
|
for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
int nv = model->nv;
|
int nv = model->nv;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// load keyframe if present, step for a bit
|
// load keyframe if present, step for a bit
|
||||||
if (model->nkey) mj_resetDataKeyframe(model, data, 0);
|
if (model->nkey) mj_resetDataKeyframe(model.get(), data.get(), 0);
|
||||||
while (data->time < 0.1) {
|
while (data->time < 0.1) {
|
||||||
mj_step(model, data);
|
mj_step(model.get(), data.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
// minimal call required for mj_jacDot outputs to be valid
|
// minimal call required for mj_jacDot outputs to be valid
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
mj_comPos(model, data);
|
mj_comPos(model.get(), data.get());
|
||||||
mj_comVel(model, data);
|
mj_comVel(model.get(), data.get());
|
||||||
|
|
||||||
// get bodyid and site position
|
// get bodyid and site position
|
||||||
int bodyid = mj_name2id(model, mjOBJ_BODY, "query");
|
int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "query");
|
||||||
EXPECT_GT(bodyid, 0);
|
EXPECT_GT(bodyid, 0);
|
||||||
int siteid = mj_name2id(model, mjOBJ_SITE, "query");
|
int siteid = mj_name2id(model.get(), mjOBJ_SITE, "query");
|
||||||
EXPECT_GT(siteid, -1);
|
EXPECT_GT(siteid, -1);
|
||||||
mjtNum point[3];
|
mjtNum point[3];
|
||||||
mju_copy3(point, data->site_xpos + 3 * siteid);
|
mju_copy3(point, data->site_xpos + 3 * siteid);
|
||||||
@@ -612,7 +599,8 @@ TEST_F(JacobianTest, JacDotSparse) {
|
|||||||
// dense jacDot
|
// dense jacDot
|
||||||
std::vector<mjtNum> jacp_dense(3 * nv);
|
std::vector<mjtNum> jacp_dense(3 * nv);
|
||||||
std::vector<mjtNum> jacr_dense(3 * nv);
|
std::vector<mjtNum> jacr_dense(3 * nv);
|
||||||
mj_jacDot(model, data, jacp_dense.data(), jacr_dense.data(), point, bodyid);
|
mj_jacDot(model.get(), data.get(), jacp_dense.data(), jacr_dense.data(),
|
||||||
|
point, bodyid);
|
||||||
|
|
||||||
// compute body chain using public mjModel fields
|
// compute body chain using public mjModel fields
|
||||||
std::vector<int> chain(nv);
|
std::vector<int> chain(nv);
|
||||||
@@ -631,8 +619,8 @@ TEST_F(JacobianTest, JacDotSparse) {
|
|||||||
// sparse jacDot
|
// sparse jacDot
|
||||||
std::vector<mjtNum> jacp_sparse(3 * NV);
|
std::vector<mjtNum> jacp_sparse(3 * NV);
|
||||||
std::vector<mjtNum> jacr_sparse(3 * NV);
|
std::vector<mjtNum> jacr_sparse(3 * NV);
|
||||||
mj_jacDotSparse(model, data, jacp_sparse.data(), jacr_sparse.data(),
|
mj_jacDotSparse(model.get(), data.get(), jacp_sparse.data(),
|
||||||
point, bodyid, NV, chain.data());
|
jacr_sparse.data(), point, bodyid, NV, chain.data());
|
||||||
|
|
||||||
// expand sparse to dense and compare
|
// expand sparse to dense and compare
|
||||||
std::vector<mjtNum> jacp_expanded(3 * nv, 0);
|
std::vector<mjtNum> jacp_expanded(3 * nv, 0);
|
||||||
@@ -648,13 +636,9 @@ TEST_F(JacobianTest, JacDotSparse) {
|
|||||||
// expect bitwise equality
|
// expect bitwise equality
|
||||||
EXPECT_EQ(jacp_expanded, jacp_dense);
|
EXPECT_EQ(jacp_expanded, jacp_dense);
|
||||||
EXPECT_EQ(jacr_expanded, jacr_dense);
|
EXPECT_EQ(jacr_expanded, jacr_dense);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// validate rotational Jacobian used in welds
|
// validate rotational Jacobian used in welds
|
||||||
TEST_F(JacobianTest, WeldRotJacobian) {
|
TEST_F(JacobianTest, WeldRotJacobian) {
|
||||||
#ifdef mjUSESINGLE
|
#ifdef mjUSESINGLE
|
||||||
@@ -678,18 +662,18 @@ TEST_F(JacobianTest, WeldRotJacobian) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, testing::NotNull()) << error;
|
ASSERT_THAT(model.get(), testing::NotNull()) << error;
|
||||||
ASSERT_EQ(model->nq, 7);
|
ASSERT_EQ(model->nq, 7);
|
||||||
ASSERT_EQ(model->nv, 6);
|
ASSERT_EQ(model->nv, 6);
|
||||||
static const int nv = 6; // for increased readability
|
static const int nv = 6; // for increased readability
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// arbitrary initial values for the ball and hinge joints
|
// arbitrary initial values for the ball and hinge joints
|
||||||
mjtNum qpos0[7] = {.5, .5, .5, .5, .7, .8, .9};
|
mjtNum qpos0[7] = {.5, .5, .5, .5, .7, .8, .9};
|
||||||
|
|
||||||
// compute required quantities using mj_step1
|
// compute required quantities using mj_step1
|
||||||
mj_step1(model, data);
|
mj_step1(model.get(), data.get());
|
||||||
|
|
||||||
// get orientation error
|
// get orientation error
|
||||||
mjtNum res[3];
|
mjtNum res[3];
|
||||||
@@ -716,7 +700,7 @@ TEST_F(JacobianTest, WeldRotJacobian) {
|
|||||||
mju_copy3(res, quat3 + 1);
|
mju_copy3(res, quat3 + 1);
|
||||||
};
|
};
|
||||||
|
|
||||||
RotationResidual(model, data, qpos0, NULL, res);
|
RotationResidual(model.get(), data.get(), qpos0, NULL, res);
|
||||||
|
|
||||||
// compute Jacobian with finite-differencing
|
// compute Jacobian with finite-differencing
|
||||||
mjtNum jacFD[3 * nv];
|
mjtNum jacFD[3 * nv];
|
||||||
@@ -728,7 +712,7 @@ TEST_F(JacobianTest, WeldRotJacobian) {
|
|||||||
dqpos[i] = eps;
|
dqpos[i] = eps;
|
||||||
|
|
||||||
// get nudged residual
|
// get nudged residual
|
||||||
RotationResidual(model, data, qpos0, dqpos, dres);
|
RotationResidual(model.get(), data.get(), qpos0, dqpos, dres);
|
||||||
|
|
||||||
// remove nudge
|
// remove nudge
|
||||||
dqpos[i] = 0.0;
|
dqpos[i] = 0.0;
|
||||||
@@ -741,7 +725,7 @@ TEST_F(JacobianTest, WeldRotJacobian) {
|
|||||||
|
|
||||||
// reset mjData to qpos0
|
// reset mjData to qpos0
|
||||||
mju_copy(data->qpos, qpos0, 7);
|
mju_copy(data->qpos, qpos0, 7);
|
||||||
mj_step1(model, data);
|
mj_step1(model.get(), data.get());
|
||||||
|
|
||||||
// intermediate quaternions quat1 and quat2
|
// intermediate quaternions quat1 and quat2
|
||||||
mjtNum quat1[4], negQuat2[4];
|
mjtNum quat1[4], negQuat2[4];
|
||||||
@@ -753,8 +737,8 @@ TEST_F(JacobianTest, WeldRotJacobian) {
|
|||||||
mjtNum point[3] = {0};
|
mjtNum point[3] = {0};
|
||||||
|
|
||||||
// rotational Jacobian difference
|
// rotational Jacobian difference
|
||||||
mj_jacDifPair(model, data, NULL, 2, 1, point, point,
|
mj_jacDifPair(model.get(), data.get(), NULL, 2, 1, point, point, NULL, NULL,
|
||||||
NULL, NULL, NULL, jac0, jac1, jacdif, mj_isSparse(model),
|
NULL, jac0, jac1, jacdif, mj_isSparse(model.get()),
|
||||||
/*flg_skipcommon=*/0);
|
/*flg_skipcommon=*/0);
|
||||||
|
|
||||||
// formula: 0.5 * neg(quat2) * (jac1-jac2) * quat1
|
// formula: 0.5 * neg(quat2) * (jac1-jac2) * quat1
|
||||||
@@ -778,12 +762,7 @@ TEST_F(JacobianTest, WeldRotJacobian) {
|
|||||||
// test that analytical and finite-differenced Jacobians match
|
// test that analytical and finite-differenced Jacobians match
|
||||||
EXPECT_THAT(AsVector(jacFD, 3 * nv),
|
EXPECT_THAT(AsVector(jacFD, 3 * nv),
|
||||||
Pointwise(MjNear(eps, 1e-3), AsVector(jacdif, 3 * nv)));
|
Pointwise(MjNear(eps, 1e-3), AsVector(jacdif, 3 * nv)));
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace mujoco
|
} // namespace mujoco
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -36,11 +36,11 @@ namespace mujoco {
|
|||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
using ::std::vector;
|
using ::std::vector;
|
||||||
using ::testing::Pointwise;
|
|
||||||
using ::testing::DoubleNear;
|
using ::testing::DoubleNear;
|
||||||
using ::testing::Eq;
|
|
||||||
using ::testing::Each;
|
using ::testing::Each;
|
||||||
|
using ::testing::Eq;
|
||||||
using ::testing::NotNull;
|
using ::testing::NotNull;
|
||||||
|
using ::testing::Pointwise;
|
||||||
using DerivativeTest = MujocoTest;
|
using DerivativeTest = MujocoTest;
|
||||||
|
|
||||||
// errors smaller than this are ignored
|
// errors smaller than this are ignored
|
||||||
@@ -59,8 +59,8 @@ static mjtNum RelativeError(mjtNum a, mjtNum b) {
|
|||||||
|
|
||||||
// expect two 2D arrays to have elementwise relative error smaller than eps
|
// expect two 2D arrays to have elementwise relative error smaller than eps
|
||||||
// return maximum absolute error
|
// return maximum absolute error
|
||||||
static mjtNum CompareMatrices(mjtNum* Actual, mjtNum* Expected,
|
static mjtNum CompareMatrices(mjtNum* Actual, mjtNum* Expected, int nrow,
|
||||||
int nrow, int ncol, mjtNum eps) {
|
int ncol, mjtNum eps) {
|
||||||
mjtNum max_error = 0;
|
mjtNum max_error = 0;
|
||||||
for (int i = 0; i < nrow; i++) {
|
for (int i = 0; i < nrow; i++) {
|
||||||
for (int j = 0; j < ncol; j++) {
|
for (int j = 0; j < ncol; j++) {
|
||||||
@@ -68,8 +68,7 @@ static mjtNum CompareMatrices(mjtNum* Actual, mjtNum* Expected,
|
|||||||
mjtNum expected = Expected[i * ncol + j];
|
mjtNum expected = Expected[i * ncol + j];
|
||||||
EXPECT_LT(RelativeError(actual, expected), eps)
|
EXPECT_LT(RelativeError(actual, expected), eps)
|
||||||
<< "error at position (" << i << ", " << j << ")"
|
<< "error at position (" << i << ", " << j << ")"
|
||||||
<< "\nexpected = " << expected
|
<< "\nexpected = " << expected << "\nactual = " << actual
|
||||||
<< "\nactual = " << actual
|
|
||||||
<< "\ndiff = " << expected - actual;
|
<< "\ndiff = " << expected - actual;
|
||||||
max_error = mjMAX(mju_abs(actual - expected), max_error);
|
max_error = mjMAX(mju_abs(actual - expected), max_error);
|
||||||
}
|
}
|
||||||
@@ -89,8 +88,7 @@ static const char* const kDamperActuatorsPath =
|
|||||||
"engine/testdata/actuation/damper.xml";
|
"engine/testdata/actuation/damper.xml";
|
||||||
static const char* const kDampedPendulumPath =
|
static const char* const kDampedPendulumPath =
|
||||||
"engine/testdata/derivative/damped_pendulum.xml";
|
"engine/testdata/derivative/damped_pendulum.xml";
|
||||||
static const char* const kLinearPath =
|
static const char* const kLinearPath = "engine/testdata/derivative/linear.xml";
|
||||||
"engine/testdata/derivative/linear.xml";
|
|
||||||
static const char* const kDCMotorPath =
|
static const char* const kDCMotorPath =
|
||||||
"engine/testdata/derivative/dcmotor.xml";
|
"engine/testdata/derivative/dcmotor.xml";
|
||||||
static const char* const kModelPath = "testdata/model.xml";
|
static const char* const kModelPath = "testdata/model.xml";
|
||||||
@@ -98,14 +96,13 @@ static const char* const kModelPath = "testdata/model.xml";
|
|||||||
// compare analytic and finite-difference d_smooth/d_qvel
|
// compare analytic and finite-difference d_smooth/d_qvel
|
||||||
TEST_F(DerivativeTest, SmoothDvel) {
|
TEST_F(DerivativeTest, SmoothDvel) {
|
||||||
// run test on all models
|
// run test on all models
|
||||||
for (const char* local_path : {kEnergyConservingPendulumPath,
|
for (const char* local_path :
|
||||||
kTumblingThinObjectPath,
|
{kEnergyConservingPendulumPath, kTumblingThinObjectPath,
|
||||||
kDampedActuatorsPath,
|
kDampedActuatorsPath, kDamperActuatorsPath, kDCMotorPath}) {
|
||||||
kDamperActuatorsPath,
|
|
||||||
kDCMotorPath}) {
|
|
||||||
const std::string xml_path = GetTestDataFilePath(local_path);
|
const std::string xml_path = GetTestDataFilePath(local_path);
|
||||||
char error[1024] = "";
|
char error[1024] = "";
|
||||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
|
mjModel* model =
|
||||||
|
mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
|
||||||
ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error;
|
ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error;
|
||||||
int nD = model->nD;
|
int nD = model->nD;
|
||||||
mjData* data = mj_makeData(model);
|
mjData* data = mj_makeData(model);
|
||||||
@@ -171,13 +168,12 @@ TEST_F(DerivativeTest, DisabledActuators) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m1 = LoadModelFromString(xml1, error, sizeof(error));
|
MjModelPtr m1 = LoadModelFromString(xml1, error, sizeof(error));
|
||||||
ASSERT_THAT(m1, NotNull()) << error;
|
ASSERT_THAT(m1.get(), NotNull()) << error;
|
||||||
mjData* d1 = mj_makeData(m1);
|
MjDataPtr d1 = MakeData(m1);
|
||||||
|
|
||||||
d1->ctrl[0] = 6;
|
d1->ctrl[0] = 6;
|
||||||
while (d1->time < 1)
|
while (d1->time < 1) mj_step(m1.get(), d1.get());
|
||||||
mj_step(m1, d1);
|
|
||||||
|
|
||||||
// model with a position actuator and an intvelocity actuator
|
// model with a position actuator and an intvelocity actuator
|
||||||
static constexpr char xml2[] = R"(
|
static constexpr char xml2[] = R"(
|
||||||
@@ -198,22 +194,16 @@ TEST_F(DerivativeTest, DisabledActuators) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
|
|
||||||
mjModel* m2 = LoadModelFromString(xml2);
|
MjModelPtr m2 = LoadModelFromString(xml2);
|
||||||
mjData* d2 = mj_makeData(m2);
|
MjDataPtr d2 = MakeData(m2);
|
||||||
|
|
||||||
d2->ctrl[0] = 6;
|
d2->ctrl[0] = 6;
|
||||||
d2->ctrl[1] = 6;
|
d2->ctrl[1] = 6;
|
||||||
|
|
||||||
while (d2->time < 1)
|
while (d2->time < 1) mj_step(m2.get(), d2.get());
|
||||||
mj_step(m2, d2);
|
|
||||||
|
|
||||||
// expect same qvel in both models
|
// expect same qvel in both models
|
||||||
EXPECT_EQ(d1->qvel[0], d2->qvel[0]);
|
EXPECT_EQ(d1->qvel[0], d2->qvel[0]);
|
||||||
|
|
||||||
mj_deleteData(d2);
|
|
||||||
mj_deleteModel(m2);
|
|
||||||
mj_deleteData(d1);
|
|
||||||
mj_deleteModel(m1);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// actuator order has no effect
|
// actuator order has no effect
|
||||||
@@ -242,15 +232,14 @@ TEST_F(DerivativeTest, ActuatorOrder) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m1 = LoadModelFromString(xml1, error, sizeof(error));
|
MjModelPtr m1 = LoadModelFromString(xml1, error, sizeof(error));
|
||||||
ASSERT_THAT(m1, NotNull()) << "Failed to load model: " << error;
|
ASSERT_THAT(m1.get(), NotNull()) << "Failed to load model: " << error;
|
||||||
mjData* d1 = mj_makeData(m1);
|
MjDataPtr d1 = MakeData(m1);
|
||||||
|
|
||||||
d1->ctrl[0] = 6;
|
d1->ctrl[0] = 6;
|
||||||
d1->ctrl[1] = 6;
|
d1->ctrl[1] = 6;
|
||||||
|
|
||||||
while (d1->time < 1)
|
while (d1->time < 1) mj_step(m1.get(), d1.get());
|
||||||
mj_step(m1, d1);
|
|
||||||
|
|
||||||
// model with stateful actuator second
|
// model with stateful actuator second
|
||||||
static constexpr char xml2[] = R"(
|
static constexpr char xml2[] = R"(
|
||||||
@@ -275,30 +264,24 @@ TEST_F(DerivativeTest, ActuatorOrder) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
|
|
||||||
mjModel* m2 = LoadModelFromString(xml2, error, sizeof(error));
|
MjModelPtr m2 = LoadModelFromString(xml2, error, sizeof(error));
|
||||||
ASSERT_THAT(m2, NotNull()) << "Failed to load model: " << error;
|
ASSERT_THAT(m2.get(), NotNull()) << "Failed to load model: " << error;
|
||||||
mjData* d2 = mj_makeData(m2);
|
MjDataPtr d2 = MakeData(m2);
|
||||||
|
|
||||||
d2->ctrl[0] = 6;
|
d2->ctrl[0] = 6;
|
||||||
d2->ctrl[1] = 6;
|
d2->ctrl[1] = 6;
|
||||||
|
|
||||||
while (d2->time < 1)
|
while (d2->time < 1) mj_step(m2.get(), d2.get());
|
||||||
mj_step(m2, d2);
|
|
||||||
|
|
||||||
// expect same qvel in both models
|
// expect same qvel in both models
|
||||||
EXPECT_EQ(d1->qvel[0], d2->qvel[0]);
|
EXPECT_EQ(d1->qvel[0], d2->qvel[0]);
|
||||||
EXPECT_EQ(d1->qvel[1], d2->qvel[1]);
|
EXPECT_EQ(d1->qvel[1], d2->qvel[1]);
|
||||||
|
|
||||||
mj_deleteData(d2);
|
|
||||||
mj_deleteModel(m2);
|
|
||||||
mj_deleteData(d1);
|
|
||||||
mj_deleteModel(m1);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// compare analytic and fin-diff d_qfrc_passive/d_qvel
|
// compare analytic and fin-diff d_qfrc_passive/d_qvel
|
||||||
TEST_F(DerivativeTest, PassiveDvel) {
|
TEST_F(DerivativeTest, PassiveDvel) {
|
||||||
for (const char* local_path : {kTumblingThinObjectPath,
|
for (const char* local_path :
|
||||||
kTumblingThinObjectEllipsoidPath}) {
|
{kTumblingThinObjectPath, kTumblingThinObjectEllipsoidPath}) {
|
||||||
// load model
|
// load model
|
||||||
const std::string xml_path = GetTestDataFilePath(local_path);
|
const std::string xml_path = GetTestDataFilePath(local_path);
|
||||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
||||||
@@ -355,9 +338,8 @@ TEST_F(DerivativeTest, StepSkip) {
|
|||||||
// disable warm-starts so we don't need to save qacc_warmstart
|
// disable warm-starts so we don't need to save qacc_warmstart
|
||||||
model->opt.disableflags |= mjDSBL_WARMSTART;
|
model->opt.disableflags |= mjDSBL_WARMSTART;
|
||||||
|
|
||||||
for (const mjtIntegrator integrator : {mjINT_EULER,
|
for (const mjtIntegrator integrator :
|
||||||
mjINT_IMPLICIT,
|
{mjINT_EULER, mjINT_IMPLICIT, mjINT_IMPLICITFAST}) {
|
||||||
mjINT_IMPLICITFAST}) {
|
|
||||||
model->opt.integrator = integrator;
|
model->opt.integrator = integrator;
|
||||||
|
|
||||||
// reset, take 20 steps
|
// reset, take 20 steps
|
||||||
@@ -449,8 +431,8 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) {
|
|||||||
Ac[i * nv + i] = -m->jnt_stiffness[i];
|
Ac[i * nv + i] = -m->jnt_stiffness[i];
|
||||||
Ac[nv * nv + i * nv + i] = -m->dof_damping[i];
|
Ac[nv * nv + i * nv + i] = -m->dof_damping[i];
|
||||||
}
|
}
|
||||||
mj_solveLD(Ac, d->qH, d->qHDiagInv, nv, 2*nv,
|
mj_solveLD(Ac, d->qH, d->qHDiagInv, nv, 2 * nv, m->M_rownnz, m->M_rowadr,
|
||||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
m->M_colind, nullptr);
|
||||||
|
|
||||||
// A = [dt*Ac; Ac]
|
// A = [dt*Ac; Ac]
|
||||||
mju_transpose(A, Ac, 2 * nv, nv);
|
mju_transpose(A, Ac, 2 * nv, nv);
|
||||||
@@ -477,8 +459,8 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) {
|
|||||||
mjtNum* BcT = mj_stackAllocNum(d, nv * nu);
|
mjtNum* BcT = mj_stackAllocNum(d, nv * nu);
|
||||||
mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz,
|
mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz,
|
||||||
d->moment_rowadr, d->moment_colind);
|
d->moment_rowadr, d->moment_colind);
|
||||||
mj_solveLD(Bc, d->qH, d->qHDiagInv, nv, nu,
|
mj_solveLD(Bc, d->qH, d->qHDiagInv, nv, nu, m->M_rownnz, m->M_rowadr,
|
||||||
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
|
m->M_colind, nullptr);
|
||||||
mju_transpose(BcT, Bc, nu, nv);
|
mju_transpose(BcT, Bc, nu, nv);
|
||||||
mju_scl(B, BcT, dt * dt, nu * nv);
|
mju_scl(B, BcT, dt * dt, nu * nv);
|
||||||
mju_scl(B + nu * nv, BcT, dt, nu * nv);
|
mju_scl(B + nu * nv, BcT, dt, nu * nv);
|
||||||
@@ -516,8 +498,8 @@ TEST_F(DerivativeTest, LinearSystem) {
|
|||||||
mjtNum* AFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv);
|
mjtNum* AFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv);
|
||||||
mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
|
mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
|
||||||
|
|
||||||
mjd_transitionFD(model, data, eps, /*centered=*/0,
|
mjd_transitionFD(model, data, eps, /*centered=*/0, AFD, BFD, nullptr,
|
||||||
AFD, BFD, nullptr, nullptr);
|
nullptr);
|
||||||
|
|
||||||
// uncomment for debugging:
|
// uncomment for debugging:
|
||||||
// PrintMatrix(AFD, 2*nv, 2*nv);
|
// PrintMatrix(AFD, 2*nv, 2*nv);
|
||||||
@@ -530,8 +512,8 @@ TEST_F(DerivativeTest, LinearSystem) {
|
|||||||
// central differenced A and B
|
// central differenced A and B
|
||||||
mjtNum* AFDc = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv);
|
mjtNum* AFDc = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv);
|
||||||
mjtNum* BFDc = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
|
mjtNum* BFDc = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
|
||||||
mjd_transitionFD(model, data, eps, /*centered=*/1,
|
mjd_transitionFD(model, data, eps, /*centered=*/1, AFDc, BFDc, nullptr,
|
||||||
AFDc, BFDc, nullptr, nullptr);
|
nullptr);
|
||||||
|
|
||||||
// expect central derivatives to be equal to forward differences
|
// expect central derivatives to be equal to forward differences
|
||||||
CompareMatrices(AFD, AFDc, 2 * nv, 2 * nv, eps);
|
CompareMatrices(AFD, AFDc, 2 * nv, 2 * nv, eps);
|
||||||
@@ -573,22 +555,22 @@ TEST_F(DerivativeTest, ClampedCtrlDerivatives) {
|
|||||||
// set ctrl to the limits, request forward differences
|
// set ctrl to the limits, request forward differences
|
||||||
data->ctrl[0] = 1;
|
data->ctrl[0] = 1;
|
||||||
data->ctrl[1] = -1;
|
data->ctrl[1] = -1;
|
||||||
mjd_transitionFD(model, data, eps, /*centered=*/0,
|
mjd_transitionFD(model, data, eps, /*centered=*/0, nullptr, BFD, nullptr,
|
||||||
nullptr, BFD, nullptr, nullptr);
|
nullptr);
|
||||||
// expect FD and analytic derivatives to be similar to eps precision
|
// expect FD and analytic derivatives to be similar to eps precision
|
||||||
CompareMatrices(B, BFD, 2 * nv, nu, eps);
|
CompareMatrices(B, BFD, 2 * nv, nu, eps);
|
||||||
|
|
||||||
// ctrl remains at limits, request central differences
|
// ctrl remains at limits, request central differences
|
||||||
mjd_transitionFD(model, data, eps, /*centered=*/1,
|
mjd_transitionFD(model, data, eps, /*centered=*/1, nullptr, BFD, nullptr,
|
||||||
nullptr, BFD, nullptr, nullptr);
|
nullptr);
|
||||||
// expect FD and analytic derivatives to be similar to eps precision
|
// expect FD and analytic derivatives to be similar to eps precision
|
||||||
CompareMatrices(B, BFD, 2 * nv, nu, eps);
|
CompareMatrices(B, BFD, 2 * nv, nu, eps);
|
||||||
|
|
||||||
// set ctrl beyond limits, request forward differences
|
// set ctrl beyond limits, request forward differences
|
||||||
data->ctrl[0] = 2;
|
data->ctrl[0] = 2;
|
||||||
data->ctrl[1] = -2;
|
data->ctrl[1] = -2;
|
||||||
mjd_transitionFD(model, data, eps, /*centered=*/0,
|
mjd_transitionFD(model, data, eps, /*centered=*/0, nullptr, BFD, nullptr,
|
||||||
nullptr, BFD, nullptr, nullptr);
|
nullptr);
|
||||||
// expect derivatives to be 0
|
// expect derivatives to be 0
|
||||||
EXPECT_THAT(AsVector(BFD, 2 * nv * nu), Each(Eq(0.0)));
|
EXPECT_THAT(AsVector(BFD, 2 * nv * nu), Each(Eq(0.0)));
|
||||||
|
|
||||||
@@ -597,8 +579,8 @@ TEST_F(DerivativeTest, ClampedCtrlDerivatives) {
|
|||||||
EXPECT_EQ(data->ctrl[1], -2.0);
|
EXPECT_EQ(data->ctrl[1], -2.0);
|
||||||
|
|
||||||
// ctrl remains beyond limits, request centered differences
|
// ctrl remains beyond limits, request centered differences
|
||||||
mjd_transitionFD(model, data, eps, /*centered=*/1,
|
mjd_transitionFD(model, data, eps, /*centered=*/1, nullptr, BFD, nullptr,
|
||||||
nullptr, BFD, nullptr, nullptr);
|
nullptr);
|
||||||
// expect derivatives to be 0
|
// expect derivatives to be 0
|
||||||
EXPECT_THAT(AsVector(BFD, 2 * nv * nu), Each(Eq(0.0)));
|
EXPECT_THAT(AsVector(BFD, 2 * nv * nu), Each(Eq(0.0)));
|
||||||
|
|
||||||
@@ -631,23 +613,19 @@ TEST_F(DerivativeTest, SensorDerivatives) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
|
|
||||||
mjModel* model = LoadModelFromString(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
int nv = model->nv, nu = model->nu, ns = model->nsensordata;
|
int nv = model->nv, nu = model->nu, ns = model->nsensordata;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// finite differenced C and D
|
// finite differenced C and D
|
||||||
mjtNum eps = 1e-6;
|
mjtNum eps = 1e-6;
|
||||||
mjtNum* CFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * 2 * nv);
|
mjtNum* CFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * 2 * nv);
|
||||||
mjtNum* DFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * nu);
|
mjtNum* DFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * nu);
|
||||||
mjd_transitionFD(model, data, eps, /*centered=*/0,
|
mjd_transitionFD(model.get(), data.get(), eps, /*centered=*/0, nullptr,
|
||||||
nullptr, nullptr, CFD, DFD);
|
nullptr, CFD, DFD);
|
||||||
|
|
||||||
// expected analytic C and D
|
// expected analytic C and D
|
||||||
mjtNum C[6] = {
|
mjtNum C[6] = {1, 0, 0, 1, 0, 0};
|
||||||
1, 0,
|
|
||||||
0, 1,
|
|
||||||
0, 0
|
|
||||||
};
|
|
||||||
|
|
||||||
mjtNum D[3] = {
|
mjtNum D[3] = {
|
||||||
0,
|
0,
|
||||||
@@ -661,8 +639,6 @@ TEST_F(DerivativeTest, SensorDerivatives) {
|
|||||||
|
|
||||||
mju_free(DFD);
|
mju_free(DFD);
|
||||||
mju_free(CFD);
|
mju_free(CFD);
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// if sensor derivatives aren't requested, don't compute sensors
|
// if sensor derivatives aren't requested, don't compute sensors
|
||||||
@@ -686,9 +662,9 @@ TEST_F(DerivativeTest, SensorSkip) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
|
|
||||||
mjModel* model = LoadModelFromString(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
int nv = model->nv, nu = model->nu;
|
int nv = model->nv, nu = model->nu;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// set a sentinel value in the sensor
|
// set a sentinel value in the sensor
|
||||||
data->sensordata[0] = 1337;
|
data->sensordata[0] = 1337;
|
||||||
@@ -696,14 +672,12 @@ TEST_F(DerivativeTest, SensorSkip) {
|
|||||||
// finite differenced B
|
// finite differenced B
|
||||||
mjtNum eps = 1e-6;
|
mjtNum eps = 1e-6;
|
||||||
mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
|
mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
|
||||||
mjd_transitionFD(model, data, eps, /*centered=*/0,
|
mjd_transitionFD(model.get(), data.get(), eps, /*centered=*/0, nullptr, BFD,
|
||||||
nullptr, BFD, nullptr, nullptr);
|
nullptr, nullptr);
|
||||||
|
|
||||||
EXPECT_EQ(data->sensordata[0], 1337) << "sensors should not be recomputed";
|
EXPECT_EQ(data->sensordata[0], 1337) << "sensors should not be recomputed";
|
||||||
|
|
||||||
mju_free(BFD);
|
mju_free(BFD);
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// derivatives don't mutate the state
|
// derivatives don't mutate the state
|
||||||
@@ -733,7 +707,6 @@ TEST_F(DerivativeTest, NoStateMutation) {
|
|||||||
data->act[i] = data0->act[i] = (mjtNum)i + 1;
|
data->act[i] = data0->act[i] = (mjtNum)i + 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// allocate Jacobians, call derivatives
|
// allocate Jacobians, call derivatives
|
||||||
int ndx = nv + nv + na;
|
int ndx = nv + nv + na;
|
||||||
mjtNum* A = (mjtNum*)mju_malloc(sizeof(mjtNum) * ndx * ndx);
|
mjtNum* A = (mjtNum*)mju_malloc(sizeof(mjtNum) * ndx * ndx);
|
||||||
@@ -754,22 +727,21 @@ TEST_F(DerivativeTest, NoStateMutation) {
|
|||||||
mju_free(C);
|
mju_free(C);
|
||||||
mju_free(B);
|
mju_free(B);
|
||||||
mju_free(A);
|
mju_free(A);
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteData(data0);
|
mj_deleteData(data0);
|
||||||
|
mj_deleteData(data);
|
||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
}
|
}
|
||||||
|
|
||||||
// compare dense and sparse derivatives of qfrc_bias (RNE)
|
// compare dense and sparse derivatives of qfrc_bias (RNE)
|
||||||
TEST_F(DerivativeTest, DenseSparseRneEquivalent) {
|
TEST_F(DerivativeTest, DenseSparseRneEquivalent) {
|
||||||
// run test on all models
|
// run test on all models
|
||||||
for (const char* local_path : {kEnergyConservingPendulumPath,
|
for (const char* local_path :
|
||||||
kTumblingThinObjectPath,
|
{kEnergyConservingPendulumPath, kTumblingThinObjectPath,
|
||||||
kDampedActuatorsPath,
|
kDampedActuatorsPath, kDamperActuatorsPath, kDCMotorPath}) {
|
||||||
kDamperActuatorsPath,
|
|
||||||
kDCMotorPath}) {
|
|
||||||
const std::string xml_path = GetTestDataFilePath(local_path);
|
const std::string xml_path = GetTestDataFilePath(local_path);
|
||||||
char error[1024] = "";
|
char error[1024] = "";
|
||||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
|
mjModel* model =
|
||||||
|
mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
|
||||||
ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error;
|
ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error;
|
||||||
int nD = model->nD;
|
int nD = model->nD;
|
||||||
mjtNum* qDeriv = (mjtNum*)mju_malloc(sizeof(mjtNum) * nD);
|
mjtNum* qDeriv = (mjtNum*)mju_malloc(sizeof(mjtNum) * nD);
|
||||||
@@ -799,8 +771,8 @@ TEST_F(DerivativeTest, DenseSparseRneEquivalent) {
|
|||||||
EXPECT_THAT(AsVector(data->qDeriv, nD),
|
EXPECT_THAT(AsVector(data->qDeriv, nD),
|
||||||
Pointwise(MjNear(1e-12, 5e-5), AsVector(qDeriv, nD)));
|
Pointwise(MjNear(1e-12, 5e-5), AsVector(qDeriv, nD)));
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mju_free(qDeriv);
|
mju_free(qDeriv);
|
||||||
|
mj_deleteData(data);
|
||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -829,21 +801,20 @@ TEST_F(DerivativeTest, LinearSystemInverse) {
|
|||||||
// get derivatives
|
// get derivatives
|
||||||
mjtNum eps = 1e-6;
|
mjtNum eps = 1e-6;
|
||||||
mjtByte flg_actuation = 0;
|
mjtByte flg_actuation = 0;
|
||||||
mjd_inverseFD(model, data, eps, flg_actuation,
|
mjd_inverseFD(model, data, eps, flg_actuation, DfDq.data(), DfDv.data(),
|
||||||
DfDq.data(), DfDv.data(), DfDa.data(),
|
DfDa.data(), DsDq.data(), DsDv.data(), DsDa.data(),
|
||||||
DsDq.data(), DsDv.data(), DsDa.data(),
|
|
||||||
DmDq.data());
|
DmDq.data());
|
||||||
|
|
||||||
// expect that position derivatives are the stiffnesses
|
// expect that position derivatives are the stiffnesses
|
||||||
vector<mjtNum> DfDq_expect = {model->jnt_stiffness[0], 0, 0,
|
vector<mjtNum> DfDq_expect = {model->jnt_stiffness[0], 0, 0, 0,
|
||||||
0, model->jnt_stiffness[1], 0,
|
model->jnt_stiffness[1], 0, 0, 0,
|
||||||
0, 0, model->jnt_stiffness[2]};
|
model->jnt_stiffness[2]};
|
||||||
EXPECT_THAT(DfDq, Pointwise(DoubleNear(eps), DfDq_expect));
|
EXPECT_THAT(DfDq, Pointwise(DoubleNear(eps), DfDq_expect));
|
||||||
|
|
||||||
// expect that velocity derivatives are the dampings
|
// expect that velocity derivatives are the dampings
|
||||||
vector<mjtNum> DfDv_expect = {model->dof_damping[0], 0, 0,
|
vector<mjtNum> DfDv_expect = {model->dof_damping[0], 0, 0, 0,
|
||||||
0, model->dof_damping[1], 0,
|
model->dof_damping[1], 0, 0, 0,
|
||||||
0, 0, model->dof_damping[2]};
|
model->dof_damping[2]};
|
||||||
EXPECT_THAT(DfDv, Pointwise(DoubleNear(eps), DfDv_expect));
|
EXPECT_THAT(DfDv, Pointwise(DoubleNear(eps), DfDv_expect));
|
||||||
|
|
||||||
// expect that acceleration derivatives are the mass matrix
|
// expect that acceleration derivatives are the mass matrix
|
||||||
@@ -907,8 +878,8 @@ void randomQuatPair(mjtNum qa[4], mjtNum qb[4], mjtNum angle, int seed) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// utility: finite-difference Jacobians of mju_subQuat
|
// utility: finite-difference Jacobians of mju_subQuat
|
||||||
static void subQuatFD(mjtNum Da[9], mjtNum Db[9],
|
static void subQuatFD(mjtNum Da[9], mjtNum Db[9], const mjtNum qa[4],
|
||||||
const mjtNum qa[4], const mjtNum qb[4], mjtNum eps) {
|
const mjtNum qb[4], mjtNum eps) {
|
||||||
// subQuat
|
// subQuat
|
||||||
mjtNum y[3];
|
mjtNum y[3];
|
||||||
mju_subQuat(y, qa, qb);
|
mju_subQuat(y, qa, qb);
|
||||||
@@ -949,7 +920,8 @@ static void subQuatFD(mjtNum Da[9], mjtNum Db[9],
|
|||||||
|
|
||||||
TEST_F(DerivativeTest, SubQuat) {
|
TEST_F(DerivativeTest, SubQuat) {
|
||||||
const int nrepeats = 10; // number of repeats
|
const int nrepeats = 10; // number of repeats
|
||||||
const mjtNum eps = MjTol(1e-7, 1e-3); // epsilon for finite-differencing and comparison
|
const mjtNum eps =
|
||||||
|
MjTol(1e-7, 1e-3); // epsilon for finite-differencing and comparison
|
||||||
|
|
||||||
int seed = 1;
|
int seed = 1;
|
||||||
for (int i = 0; i < nrepeats; i++) {
|
for (int i = 0; i < nrepeats; i++) {
|
||||||
@@ -1000,10 +972,9 @@ static void randomQuatVel(mjtNum quat[4], mjtNum vel[3], int seed) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// utility: finite-difference Jacobians of mju_quatIntegrate
|
// utility: finite-difference Jacobians of mju_quatIntegrate
|
||||||
void mjd_quatIntegrateFD(mjtNum Dquat[9], mjtNum Ds[9],
|
void mjd_quatIntegrateFD(mjtNum Dquat[9], mjtNum Ds[9], mjtNum Dvel[9],
|
||||||
mjtNum Dvel[9], mjtNum Dh[3],
|
mjtNum Dh[3], const mjtNum quat[4],
|
||||||
const mjtNum quat[4], const mjtNum vel[3],
|
const mjtNum vel[3], mjtNum h, mjtNum eps) {
|
||||||
mjtNum h, mjtNum eps) {
|
|
||||||
// compute y, output of mju_quatIntegrate(quat, vel, h)
|
// compute y, output of mju_quatIntegrate(quat, vel, h)
|
||||||
mjtNum y[4] = {quat[0], quat[1], quat[2], quat[3]};
|
mjtNum y[4] = {quat[0], quat[1], quat[2], quat[3]};
|
||||||
mju_quatIntegrate(y, vel, h);
|
mju_quatIntegrate(y, vel, h);
|
||||||
@@ -1058,7 +1029,8 @@ void mjd_quatIntegrateFD(mjtNum Dquat[9], mjtNum Ds[9],
|
|||||||
|
|
||||||
TEST_F(DerivativeTest, quatIntegrate) {
|
TEST_F(DerivativeTest, quatIntegrate) {
|
||||||
const int nrepeats = 10; // number of repeats
|
const int nrepeats = 10; // number of repeats
|
||||||
const mjtNum eps = MjTol(1e-7, 1e-3); // epsilon for finite-differencing and comparison
|
const mjtNum eps =
|
||||||
|
MjTol(1e-7, 1e-3); // epsilon for finite-differencing and comparison
|
||||||
|
|
||||||
int seed = 1;
|
int seed = 1;
|
||||||
for (int i = 0; i < nrepeats; i++) {
|
for (int i = 0; i < nrepeats; i++) {
|
||||||
@@ -1114,22 +1086,22 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
|
|
||||||
mjtNum dt_small = 1e-4;
|
mjtNum dt_small = 1e-4;
|
||||||
mjtNum dt_large = 1e-2;
|
mjtNum dt_large = 1e-2;
|
||||||
mjtNum duration = 1.0;
|
mjtNum duration = 1.0;
|
||||||
|
|
||||||
mjData* d_gt = mj_makeData(m);
|
MjDataPtr d_gt = MakeData(m);
|
||||||
mjData* d_implicit = mj_makeData(m);
|
MjDataPtr d_implicit = MakeData(m);
|
||||||
mjData* d_euler = mj_makeData(m);
|
MjDataPtr d_euler = MakeData(m);
|
||||||
|
|
||||||
mj_resetData(m, d_gt);
|
mj_resetData(m.get(), d_gt.get());
|
||||||
mj_resetData(m, d_implicit);
|
mj_resetData(m.get(), d_implicit.get());
|
||||||
mj_resetData(m, d_euler);
|
mj_resetData(m.get(), d_euler.get());
|
||||||
|
|
||||||
d_gt->ctrl[0] = 0.5;
|
d_gt.get()->ctrl[0] = 0.5;
|
||||||
d_implicit->ctrl[0] = 0.5;
|
d_implicit->ctrl[0] = 0.5;
|
||||||
d_euler->ctrl[0] = 0.5;
|
d_euler->ctrl[0] = 0.5;
|
||||||
|
|
||||||
@@ -1141,8 +1113,8 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
|
|||||||
m->opt.timestep = dt_large;
|
m->opt.timestep = dt_large;
|
||||||
|
|
||||||
m->opt.integrator = mjINT_IMPLICITFAST;
|
m->opt.integrator = mjINT_IMPLICITFAST;
|
||||||
mj_resetData(m, d_gt);
|
mj_resetData(m.get(), d_gt.get());
|
||||||
d_gt->ctrl[0] = 0.5;
|
d_gt.get()->ctrl[0] = 0.5;
|
||||||
m->opt.timestep = dt_small;
|
m->opt.timestep = dt_small;
|
||||||
m->opt.integrator = mjINT_EULER;
|
m->opt.integrator = mjINT_EULER;
|
||||||
|
|
||||||
@@ -1151,20 +1123,20 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
|
|||||||
m->opt.integrator = mjINT_EULER;
|
m->opt.integrator = mjINT_EULER;
|
||||||
m->opt.timestep = dt_small;
|
m->opt.timestep = dt_small;
|
||||||
for (int j = 0; j < substeps; j++) {
|
for (int j = 0; j < substeps; j++) {
|
||||||
mj_step(m, d_gt);
|
mj_step(m.get(), d_gt.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
// euler at large timestep
|
// euler at large timestep
|
||||||
m->opt.timestep = dt_large;
|
m->opt.timestep = dt_large;
|
||||||
mj_step(m, d_euler);
|
mj_step(m.get(), d_euler.get());
|
||||||
|
|
||||||
// implicitfast at large timestep
|
// implicitfast at large timestep
|
||||||
m->opt.integrator = mjINT_IMPLICITFAST;
|
m->opt.integrator = mjINT_IMPLICITFAST;
|
||||||
mj_step(m, d_implicit);
|
mj_step(m.get(), d_implicit.get());
|
||||||
|
|
||||||
// accumulate errors
|
// accumulate errors
|
||||||
mjtNum diff_implicit = d_gt->qpos[0] - d_implicit->qpos[0];
|
mjtNum diff_implicit = d_gt.get()->qpos[0] - d_implicit->qpos[0];
|
||||||
mjtNum diff_euler = d_gt->qpos[0] - d_euler->qpos[0];
|
mjtNum diff_euler = d_gt.get()->qpos[0] - d_euler->qpos[0];
|
||||||
error_implicit += diff_implicit * diff_implicit;
|
error_implicit += diff_implicit * diff_implicit;
|
||||||
error_euler += diff_euler * diff_euler;
|
error_euler += diff_euler * diff_euler;
|
||||||
}
|
}
|
||||||
@@ -1173,11 +1145,6 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
|
|||||||
EXPECT_LT(error_implicit, error_euler)
|
EXPECT_LT(error_implicit, error_euler)
|
||||||
<< "implicitfast should be more accurate than Euler at large timestep "
|
<< "implicitfast should be more accurate than Euler at large timestep "
|
||||||
<< "when forcerange derivatives are correctly handled";
|
<< "when forcerange derivatives are correctly handled";
|
||||||
|
|
||||||
mj_deleteData(d_euler);
|
|
||||||
mj_deleteData(d_implicit);
|
|
||||||
mj_deleteData(d_gt);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(DerivativeTest, NonlinearDampingDerivative) {
|
TEST_F(DerivativeTest, NonlinearDampingDerivative) {
|
||||||
@@ -1197,20 +1164,20 @@ TEST_F(DerivativeTest, NonlinearDampingDerivative) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
|
|
||||||
mjtNum dt_small = 1e-4;
|
mjtNum dt_small = 1e-4;
|
||||||
mjtNum dt_large = 1e-2;
|
mjtNum dt_large = 1e-2;
|
||||||
mjtNum duration = 1.0;
|
mjtNum duration = 1.0;
|
||||||
|
|
||||||
mjData* d_gt = mj_makeData(m);
|
MjDataPtr d_gt = MakeData(m);
|
||||||
mjData* d_enabled = mj_makeData(m);
|
MjDataPtr d_enabled = MakeData(m);
|
||||||
mjData* d_disabled = mj_makeData(m);
|
MjDataPtr d_disabled = MakeData(m);
|
||||||
|
|
||||||
mj_resetDataKeyframe(m, d_gt, 0);
|
mj_resetDataKeyframe(m.get(), d_gt.get(), 0);
|
||||||
mj_resetDataKeyframe(m, d_enabled, 0);
|
mj_resetDataKeyframe(m.get(), d_enabled.get(), 0);
|
||||||
mj_resetDataKeyframe(m, d_disabled, 0);
|
mj_resetDataKeyframe(m.get(), d_disabled.get(), 0);
|
||||||
|
|
||||||
m->opt.integrator = mjINT_EULER;
|
m->opt.integrator = mjINT_EULER;
|
||||||
mjtNum error_enabled = 0;
|
mjtNum error_enabled = 0;
|
||||||
@@ -1222,17 +1189,17 @@ TEST_F(DerivativeTest, NonlinearDampingDerivative) {
|
|||||||
m->opt.timestep = dt_small;
|
m->opt.timestep = dt_small;
|
||||||
m->opt.disableflags |= mjDSBL_EULERDAMP; // disable implicit damping
|
m->opt.disableflags |= mjDSBL_EULERDAMP; // disable implicit damping
|
||||||
for (int j = 0; j < substeps; j++) {
|
for (int j = 0; j < substeps; j++) {
|
||||||
mj_step(m, d_gt);
|
mj_step(m.get(), d_gt.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
m->opt.timestep = dt_large;
|
m->opt.timestep = dt_large;
|
||||||
mj_step(m, d_disabled);
|
mj_step(m.get(), d_disabled.get());
|
||||||
|
|
||||||
m->opt.disableflags &= ~mjDSBL_EULERDAMP; // enable implicit damping
|
m->opt.disableflags &= ~mjDSBL_EULERDAMP; // enable implicit damping
|
||||||
mj_step(m, d_enabled);
|
mj_step(m.get(), d_enabled.get());
|
||||||
|
|
||||||
mjtNum diff_enabled = d_gt->qvel[0] - d_enabled->qvel[0];
|
mjtNum diff_enabled = d_gt.get()->qvel[0] - d_enabled.get()->qvel[0];
|
||||||
mjtNum diff_disabled = d_gt->qvel[0] - d_disabled->qvel[0];
|
mjtNum diff_disabled = d_gt.get()->qvel[0] - d_disabled.get()->qvel[0];
|
||||||
error_enabled += diff_enabled * diff_enabled;
|
error_enabled += diff_enabled * diff_enabled;
|
||||||
error_disabled += diff_disabled * diff_disabled;
|
error_disabled += diff_disabled * diff_disabled;
|
||||||
}
|
}
|
||||||
@@ -1240,11 +1207,6 @@ TEST_F(DerivativeTest, NonlinearDampingDerivative) {
|
|||||||
EXPECT_LT(error_enabled, error_disabled)
|
EXPECT_LT(error_enabled, error_disabled)
|
||||||
<< "Euler with implicit damping should be more accurate than without "
|
<< "Euler with implicit damping should be more accurate than without "
|
||||||
<< "when nonlinear damping derivatives are correctly handled";
|
<< "when nonlinear damping derivatives are correctly handled";
|
||||||
|
|
||||||
mj_deleteData(d_disabled);
|
|
||||||
mj_deleteData(d_enabled);
|
|
||||||
mj_deleteData(d_gt);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// implicit derivatives should use next activation when actearly is set
|
// implicit derivatives should use next activation when actearly is set
|
||||||
@@ -1274,9 +1236,9 @@ TEST_F(DerivativeTest, ActearlyDerivative) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
// set identical ctrl with zero initial activation
|
// set identical ctrl with zero initial activation
|
||||||
d->ctrl[0] = 1.0;
|
d->ctrl[0] = 1.0;
|
||||||
@@ -1285,7 +1247,7 @@ TEST_F(DerivativeTest, ActearlyDerivative) {
|
|||||||
d->act[1] = 0.0;
|
d->act[1] = 0.0;
|
||||||
|
|
||||||
// step computes derivatives during implicit integration
|
// step computes derivatives during implicit integration
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
|
|
||||||
// both should have same act_dot
|
// both should have same act_dot
|
||||||
EXPECT_EQ(d->act_dot[0], d->act_dot[1]);
|
EXPECT_EQ(d->act_dot[0], d->act_dot[1]);
|
||||||
@@ -1305,12 +1267,8 @@ TEST_F(DerivativeTest, ActearlyDerivative) {
|
|||||||
<< "actearly=true should use next_act=1";
|
<< "actearly=true should use next_act=1";
|
||||||
EXPECT_NEAR(d->qDeriv[diag1], 0.0, 1e-10)
|
EXPECT_NEAR(d->qDeriv[diag1], 0.0, 1e-10)
|
||||||
<< "actearly=false should use current_act=0";
|
<< "actearly=false should use current_act=0";
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// verify stateful DC motor derivative matches analytical formula
|
// verify stateful DC motor derivative matches analytical formula
|
||||||
TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
|
TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
|
||||||
static constexpr char xml[] = R"(
|
static constexpr char xml[] = R"(
|
||||||
@@ -1330,19 +1288,19 @@ TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
// set nonzero velocity and ctrl
|
// set nonzero velocity and ctrl
|
||||||
d->qvel[0] = 1.0;
|
d->qvel[0] = 1.0;
|
||||||
d->ctrl[0] = 0.5;
|
d->ctrl[0] = 0.5;
|
||||||
|
|
||||||
// forward to compute act_dot, etc.
|
// forward to compute act_dot, etc.
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// compute analytical derivatives
|
// compute analytical derivatives
|
||||||
mjd_smooth_vel(m, d, /* flg_bias = */ 1);
|
mjd_smooth_vel(m.get(), d.get(), /* flg_bias = */ 1);
|
||||||
|
|
||||||
// extract diagonal of qDeriv
|
// extract diagonal of qDeriv
|
||||||
mjtNum qDeriv_diag = d->qDeriv[m->D_rowadr[0] + m->D_rownnz[0] - 1];
|
mjtNum qDeriv_diag = d->qDeriv[m->D_rowadr[0] + m->D_rownnz[0] - 1];
|
||||||
@@ -1353,12 +1311,8 @@ TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
|
|||||||
mjtNum expected = K * (-kd - K) * (1 - mju_exp(-h / te)) / R;
|
mjtNum expected = K * (-kd - K) * (1 - mju_exp(-h / te)) / R;
|
||||||
EXPECT_NEAR(qDeriv_diag, expected, 1e-10)
|
EXPECT_NEAR(qDeriv_diag, expected, 1e-10)
|
||||||
<< "stateful DC motor derivative should match analytical formula";
|
<< "stateful DC motor derivative should match analytical formula";
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// verify that stateful DC motor derivative converges to stateless as te -> 0
|
// verify that stateful DC motor derivative converges to stateless as te -> 0
|
||||||
TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) {
|
TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) {
|
||||||
// stateless DC motor with position controller
|
// stateless DC motor with position controller
|
||||||
@@ -1396,35 +1350,32 @@ TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m_sl = LoadModelFromString(xml_stateless, error, sizeof(error));
|
MjModelPtr m_sl = LoadModelFromString(xml_stateless, error, sizeof(error));
|
||||||
ASSERT_THAT(m_sl, NotNull()) << error;
|
ASSERT_THAT(m_sl.get(), NotNull()) << error;
|
||||||
mjData* d_sl = mj_makeData(m_sl);
|
MjDataPtr d_sl = MakeData(m_sl);
|
||||||
|
|
||||||
mjModel* m_sf = LoadModelFromString(xml_stateful, error, sizeof(error));
|
MjModelPtr m_sf = LoadModelFromString(xml_stateful, error, sizeof(error));
|
||||||
ASSERT_THAT(m_sf, NotNull()) << error;
|
ASSERT_THAT(m_sf.get(), NotNull()) << error;
|
||||||
mjData* d_sf = mj_makeData(m_sf);
|
MjDataPtr d_sf = MakeData(m_sf);
|
||||||
|
|
||||||
// set identical state
|
// set identical state
|
||||||
d_sl->qvel[0] = d_sf->qvel[0] = 1.0;
|
d_sl.get()->qvel[0] = d_sf.get()->qvel[0] = 1.0;
|
||||||
d_sl->ctrl[0] = d_sf->ctrl[0] = 0.5;
|
d_sl.get()->ctrl[0] = d_sf.get()->ctrl[0] = 0.5;
|
||||||
|
|
||||||
// forward and compute derivatives
|
// forward and compute derivatives
|
||||||
mj_forward(m_sl, d_sl);
|
mj_forward(m_sl.get(), d_sl.get());
|
||||||
mj_forward(m_sf, d_sf);
|
mj_forward(m_sf.get(), d_sf.get());
|
||||||
mjd_smooth_vel(m_sl, d_sl, 1);
|
mjd_smooth_vel(m_sl.get(), d_sl.get(), 1);
|
||||||
mjd_smooth_vel(m_sf, d_sf, 1);
|
mjd_smooth_vel(m_sf.get(), d_sf.get(), 1);
|
||||||
|
|
||||||
// extract diagonals
|
// extract diagonals
|
||||||
mjtNum diag_sl = d_sl->qDeriv[m_sl->D_rowadr[0] + m_sl->D_rownnz[0] - 1];
|
mjtNum diag_sl =
|
||||||
mjtNum diag_sf = d_sf->qDeriv[m_sf->D_rowadr[0] + m_sf->D_rownnz[0] - 1];
|
d_sl.get()->qDeriv[m_sl->D_rowadr[0] + m_sl->D_rownnz[0] - 1];
|
||||||
|
mjtNum diag_sf =
|
||||||
|
d_sf.get()->qDeriv[m_sf->D_rowadr[0] + m_sf->D_rownnz[0] - 1];
|
||||||
|
|
||||||
EXPECT_NEAR(diag_sf, diag_sl, 1e-6)
|
EXPECT_NEAR(diag_sf, diag_sl, 1e-6)
|
||||||
<< "stateful derivative should converge to stateless as te -> 0";
|
<< "stateful derivative should converge to stateless as te -> 0";
|
||||||
|
|
||||||
mj_deleteData(d_sf);
|
|
||||||
mj_deleteModel(m_sf);
|
|
||||||
mj_deleteData(d_sl);
|
|
||||||
mj_deleteModel(m_sl);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Utility: Rotate flex grid
|
// Utility: Rotate flex grid
|
||||||
@@ -1477,8 +1428,8 @@ void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name,
|
|||||||
// Builds K column-by-column using mjd_flexInterp_mul.
|
// Builds K column-by-column using mjd_flexInterp_mul.
|
||||||
// Result is -(h^2 + h*damping) * J'KJ (negative sign matches the old addH
|
// Result is -(h^2 + h*damping) * J'KJ (negative sign matches the old addH
|
||||||
// convention where stiffness is subtracted from the system matrix).
|
// convention where stiffness is subtracted from the system matrix).
|
||||||
static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense,
|
static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense, int nv,
|
||||||
int nv, mjtNum h) {
|
mjtNum h) {
|
||||||
std::vector<mjtNum> e_i(nv, 0);
|
std::vector<mjtNum> e_i(nv, 0);
|
||||||
std::vector<mjtNum> col(nv, 0);
|
std::vector<mjtNum> col(nv, 0);
|
||||||
for (int i = 0; i < nv; i++) {
|
for (int i = 0; i < nv; i++) {
|
||||||
@@ -1510,18 +1461,18 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(kXml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
int nD = model->nD;
|
int nD = model->nD;
|
||||||
int nv = model->nv;
|
int nv = model->nv;
|
||||||
ASSERT_EQ(model->nq, 24); // 8 corners * 3 dofs
|
ASSERT_EQ(model->nq, 24); // 8 corners * 3 dofs
|
||||||
|
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// iterate over rotations
|
// iterate over rotations
|
||||||
for (mjtNum angle : {0.0, 0.5, 1.0, mjPI / 2, mjPI, 2.0 * mjPI}) {
|
for (mjtNum angle : {0.0, 0.5, 1.0, mjPI / 2, mjPI, 2.0 * mjPI}) {
|
||||||
RotateFlexGrid(model, data, "flex", angle);
|
RotateFlexGrid(model.get(), data.get(), "flex", angle);
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
// part 1: stiffness verification
|
// part 1: stiffness verification
|
||||||
{
|
{
|
||||||
@@ -1543,7 +1494,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
|
|||||||
std::vector<mjtNum> H(nv * nv, 0);
|
std::vector<mjtNum> H(nv * nv, 0);
|
||||||
|
|
||||||
// assemble K into H column-by-column
|
// assemble K into H column-by-column
|
||||||
mulKD_dense(model, data, H.data(), nv, 1.0);
|
mulKD_dense(model.get(), data.get(), H.data(), nv, 1.0);
|
||||||
|
|
||||||
// restore damping
|
// restore damping
|
||||||
model->flex_damping[0] = save_damping;
|
model->flex_damping[0] = save_damping;
|
||||||
@@ -1553,13 +1504,13 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
|
|||||||
|
|
||||||
// finite difference of mj_passive for stiffness
|
// finite difference of mj_passive for stiffness
|
||||||
mjtNum eps = MjTol(1e-6, 1e-3);
|
mjtNum eps = MjTol(1e-6, 1e-3);
|
||||||
mjData* data_perturbed = mj_copyData(NULL, model, data);
|
mjData* data_perturbed = mj_copyData(NULL, model.get(), data.get());
|
||||||
|
|
||||||
// apply perturbation
|
// apply perturbation
|
||||||
mju_addToScl(data_perturbed->qpos, vec.data(), eps, nv);
|
mju_addToScl(data_perturbed->qpos, vec.data(), eps, nv);
|
||||||
|
|
||||||
// recompute geometry/passive
|
// recompute geometry/passive
|
||||||
mj_forward(model, data_perturbed);
|
mj_forward(model.get(), data_perturbed);
|
||||||
|
|
||||||
// compute FD estimate of K * vec
|
// compute FD estimate of K * vec
|
||||||
// qfrc_passive = -dV/dq => d(qfrc)/dq = -K
|
// qfrc_passive = -dV/dq => d(qfrc)/dq = -K
|
||||||
@@ -1602,7 +1553,8 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
|
|||||||
vKv += v[i] * K_full[i * nv + j] * v[j];
|
vKv += v[i] * K_full[i * nv + j] * v[j];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
EXPECT_GE(vKv, MjTol(-1e-8, -1e-5)) << "K matrix is not PSD at angle " << angle;
|
EXPECT_GE(vKv, MjTol(-1e-8, -1e-5))
|
||||||
|
<< "K matrix is not PSD at angle " << angle;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1611,12 +1563,12 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
|
|||||||
// set velocity non-zero to test damping
|
// set velocity non-zero to test damping
|
||||||
data->qvel[0] = 1.0;
|
data->qvel[0] = 1.0;
|
||||||
|
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
// get analytic derivatives (without Flex Damping currently)
|
// get analytic derivatives (without Flex Damping currently)
|
||||||
std::vector<mjtNum> qDerivAnalytic(nD);
|
std::vector<mjtNum> qDerivAnalytic(nD);
|
||||||
mju_zero(data->qDeriv, nD);
|
mju_zero(data->qDeriv, nD);
|
||||||
mjd_passive_vel(model, data);
|
mjd_passive_vel(model.get(), data.get());
|
||||||
mju_copy(qDerivAnalytic.data(), data->qDeriv, nD);
|
mju_copy(qDerivAnalytic.data(), data->qDeriv, nD);
|
||||||
|
|
||||||
// finite-difference derivatives
|
// finite-difference derivatives
|
||||||
@@ -1624,7 +1576,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
|
|||||||
mju_zero(data->qDeriv, nD);
|
mju_zero(data->qDeriv, nD);
|
||||||
mjtNum eps = MjTol(1e-6, 1e-3);
|
mjtNum eps = MjTol(1e-6, 1e-3);
|
||||||
|
|
||||||
mjd_passive_velFD(model, data, eps);
|
mjd_passive_velFD(model.get(), data.get(), eps);
|
||||||
mju_copy(qDerivFD.data(), data->qDeriv, nD);
|
mju_copy(qDerivFD.data(), data->qDeriv, nD);
|
||||||
|
|
||||||
// check that we have non-zero damping (FD should find it)
|
// check that we have non-zero damping (FD should find it)
|
||||||
@@ -1633,10 +1585,10 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
|
|||||||
// compute expected flex damping using mulKD_dense
|
// compute expected flex damping using mulKD_dense
|
||||||
// D = 4*H(0.5) - H(1)
|
// D = 4*H(0.5) - H(1)
|
||||||
vector<mjtNum> H1(nv * nv, 0);
|
vector<mjtNum> H1(nv * nv, 0);
|
||||||
mulKD_dense(model, data, H1.data(), nv, 1.0);
|
mulKD_dense(model.get(), data.get(), H1.data(), nv, 1.0);
|
||||||
|
|
||||||
vector<mjtNum> H2(nv * nv, 0);
|
vector<mjtNum> H2(nv * nv, 0);
|
||||||
mulKD_dense(model, data, H2.data(), nv, 0.5);
|
mulKD_dense(model.get(), data.get(), H2.data(), nv, 0.5);
|
||||||
|
|
||||||
vector<mjtNum> D(nv * nv);
|
vector<mjtNum> D(nv * nv);
|
||||||
for (int i = 0; i < nv * nv; i++) {
|
for (int i = 0; i < nv * nv; i++) {
|
||||||
@@ -1660,9 +1612,6 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
|
|||||||
<< "Damping Mismatch at angle: " << angle;
|
<< "Damping Mismatch at angle: " << angle;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Test Jacobian under deformation to highlight approximation error
|
// Test Jacobian under deformation to highlight approximation error
|
||||||
@@ -1681,14 +1630,14 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(kXml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
int nv = model->nv;
|
int nv = model->nv;
|
||||||
|
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// Apply rotation
|
// Apply rotation
|
||||||
RotateFlexGrid(model, data, "flex", 1.0); // 1 radian rotation
|
RotateFlexGrid(model.get(), data.get(), "flex", 1.0); // 1 radian rotation
|
||||||
|
|
||||||
// Apply deformation (stretch along X)
|
// Apply deformation (stretch along X)
|
||||||
// qpos is initialized by RotateFlexGrid.
|
// qpos is initialized by RotateFlexGrid.
|
||||||
@@ -1701,14 +1650,14 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) {
|
|||||||
}
|
}
|
||||||
mju_addTo(data->qpos, deformation.data(), nv);
|
mju_addTo(data->qpos, deformation.data(), nv);
|
||||||
|
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
// 1. Compute Analytic Jacobian (Approximate)
|
// 1. Compute Analytic Jacobian (Approximate)
|
||||||
// We use mulKD_dense to get K_approx
|
// We use mulKD_dense to get K_approx
|
||||||
std::vector<mjtNum> H_approx(nv * nv, 0);
|
std::vector<mjtNum> H_approx(nv * nv, 0);
|
||||||
|
|
||||||
// h=1, damping=0 => gives K
|
// h=1, damping=0 => gives K
|
||||||
mulKD_dense(model, data, H_approx.data(), nv, 1.0);
|
mulKD_dense(model.get(), data.get(), H_approx.data(), nv, 1.0);
|
||||||
|
|
||||||
// 2. Compute Finite Difference Jacobian (Ground Truth)
|
// 2. Compute Finite Difference Jacobian (Ground Truth)
|
||||||
// qfrc_passive = -dV/dq
|
// qfrc_passive = -dV/dq
|
||||||
@@ -1717,9 +1666,9 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) {
|
|||||||
mjtNum eps = 1e-6;
|
mjtNum eps = 1e-6;
|
||||||
|
|
||||||
for (int i = 0; i < nv; i++) {
|
for (int i = 0; i < nv; i++) {
|
||||||
mjData* data_p = mj_copyData(NULL, model, data);
|
mjData* data_p = mj_copyData(NULL, model.get(), data.get());
|
||||||
data_p->qpos[i] += eps;
|
data_p->qpos[i] += eps;
|
||||||
mj_forward(model, data_p);
|
mj_forward(model.get(), data_p);
|
||||||
|
|
||||||
for (int j = 0; j < nv; j++) {
|
for (int j = 0; j < nv; j++) {
|
||||||
// d(force_j)/d(q_i)
|
// d(force_j)/d(q_i)
|
||||||
@@ -1742,9 +1691,6 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) {
|
|||||||
// exists.
|
// exists.
|
||||||
EXPECT_GT(max_error, 1e-3)
|
EXPECT_GT(max_error, 1e-3)
|
||||||
<< "Jacobian approximation should differ from FD when deformed";
|
<< "Jacobian approximation should differ from FD when deformed";
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
+434
-641
File diff suppressed because it is too large
Load Diff
+183
-215
@@ -15,7 +15,6 @@
|
|||||||
// Tests for engine/{engine_io.c and engine_memory.c}.
|
// Tests for engine/{engine_io.c and engine_memory.c}.
|
||||||
|
|
||||||
#include "src/engine/engine_io.h"
|
#include "src/engine/engine_io.h"
|
||||||
#include "src/engine/engine_memory.h"
|
|
||||||
|
|
||||||
#include <array>
|
#include <array>
|
||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
@@ -26,11 +25,12 @@
|
|||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
#include <gmock/gmock.h>
|
#include <gmock/gmock.h>
|
||||||
#include <gtest/gtest.h>
|
|
||||||
#include <gtest/gtest-spi.h> // IWYU pragma: keep
|
#include <gtest/gtest-spi.h> // IWYU pragma: keep
|
||||||
|
#include <gtest/gtest.h>
|
||||||
#include <absl/strings/str_format.h>
|
#include <absl/strings/str_format.h>
|
||||||
#include <mujoco/mjxmacro.h>
|
#include <mujoco/mjxmacro.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
|
#include "src/engine/engine_memory.h"
|
||||||
#include "src/engine/engine_thread.h"
|
#include "src/engine/engine_thread.h"
|
||||||
#include "test/fixture.h"
|
#include "test/fixture.h"
|
||||||
|
|
||||||
@@ -57,19 +57,19 @@ TEST_F(EngineIoTest, VerifySizeModel) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
std::filesystem::path temp_file = (
|
std::filesystem::path temp_file =
|
||||||
std::filesystem::temp_directory_path() / "model.mjb");
|
(std::filesystem::temp_directory_path() / "model.mjb");
|
||||||
|
|
||||||
mj_saveModel(model, temp_file.string().c_str(), NULL, 0);
|
mj_saveModel(model.get(), temp_file.string().c_str(), NULL, 0);
|
||||||
|
|
||||||
std::uintmax_t file_size = std::filesystem::file_size(temp_file);
|
std::uintmax_t file_size = std::filesystem::file_size(temp_file);
|
||||||
int model_size = mj_sizeModel(model);
|
int model_size = mj_sizeModel(model.get());
|
||||||
|
|
||||||
std::filesystem::remove(temp_file);
|
std::filesystem::remove(temp_file);
|
||||||
mj_deleteModel(model);
|
|
||||||
|
|
||||||
EXPECT_EQ(file_size, model_size);
|
EXPECT_EQ(file_size, model_size);
|
||||||
}
|
}
|
||||||
@@ -87,15 +87,13 @@ TEST_F(EngineIoTest, MakeDataLoadsQpos0) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
model->qpos0[0] = 1;
|
model->qpos0[0] = 1;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
EXPECT_EQ(data->qpos[0], 1);
|
EXPECT_EQ(data->qpos[0], 1);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EngineIoTest, MakeDataLoadsMocapBodies) {
|
TEST_F(EngineIoTest, MakeDataLoadsMocapBodies) {
|
||||||
@@ -110,43 +108,40 @@ TEST_F(EngineIoTest, MakeDataLoadsMocapBodies) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
mjData* data = mj_makeData(model);
|
<< "Failed to load model: " << error.data();
|
||||||
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
EXPECT_EQ(data->mocap_pos[0], 42);
|
EXPECT_EQ(data->mocap_pos[0], 42);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EngineIoTest, MakeDataReturnsNullOnFailure) {
|
TEST_F(EngineIoTest, MakeDataReturnsNullOnFailure) {
|
||||||
constexpr char xml[] = "<mujoco/>";
|
constexpr char xml[] = "<mujoco/>";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
// fail mj_makeData intentionally with a bad size
|
// fail mj_makeData intentionally with a bad size
|
||||||
model->nbody = -1;
|
model->nbody = -1;
|
||||||
MockWarningHandler warning_handler;
|
MockWarningHandler warning_handler;
|
||||||
warning_handler.ExpectWarnings();
|
warning_handler.ExpectWarnings();
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
EXPECT_THAT(data, IsNull());
|
EXPECT_THAT(data, IsNull());
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EngineIoTest, ResetVariableSizes) {
|
TEST_F(EngineIoTest, ResetVariableSizes) {
|
||||||
constexpr char xml[] = "<mujoco/>";
|
constexpr char xml[] = "<mujoco/>";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to create mjData";
|
ASSERT_THAT(model.get(), NotNull()) << "Failed to create mjData";
|
||||||
|
|
||||||
// don't call mj_forward, vars should be reset
|
// don't call mj_forward, vars should be reset
|
||||||
EXPECT_EQ(data->ne, 0);
|
EXPECT_EQ(data->ne, 0);
|
||||||
@@ -155,18 +150,16 @@ TEST_F(EngineIoTest, ResetVariableSizes) {
|
|||||||
EXPECT_EQ(data->nJ, 0);
|
EXPECT_EQ(data->nJ, 0);
|
||||||
EXPECT_EQ(data->nA, 0);
|
EXPECT_EQ(data->nA, 0);
|
||||||
EXPECT_EQ(data->ncon, 0);
|
EXPECT_EQ(data->ncon, 0);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EngineIoTest, MakeDataResetsAllArenaPointerSizes) {
|
TEST_F(EngineIoTest, MakeDataResetsAllArenaPointerSizes) {
|
||||||
constexpr char xml[] = "<mujoco />";
|
constexpr char xml[] = "<mujoco />";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
mjData* data = mj_makeData(model);
|
<< "Failed to load model: " << error.data();
|
||||||
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
|
|
||||||
// without calling mj_forward, all array sizes should be zero.
|
// without calling mj_forward, all array sizes should be zero.
|
||||||
@@ -185,9 +178,6 @@ TEST_F(EngineIoTest, MakeDataResetsAllArenaPointerSizes) {
|
|||||||
#define MJ_D(n) n
|
#define MJ_D(n) n
|
||||||
#define MJ_M(n) n
|
#define MJ_M(n) n
|
||||||
#undef X
|
#undef X
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EngineIoTest, MjvCopyModel) {
|
TEST_F(EngineIoTest, MjvCopyModel) {
|
||||||
@@ -202,28 +192,27 @@ TEST_F(EngineIoTest, MjvCopyModel) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model1 = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model1 = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model1, NotNull()) << error;
|
ASSERT_THAT(model1.get(), NotNull()) << error;
|
||||||
|
|
||||||
mjModel* model2 = mj_copyModel(nullptr, model1);
|
mjModel* model2 = mj_copyModel(nullptr, model1.get());
|
||||||
ASSERT_THAT(model2, NotNull()) << error;
|
ASSERT_THAT(model2, NotNull()) << error;
|
||||||
|
|
||||||
model1->mesh_vert[0] = 0.1;
|
model1->mesh_vert[0] = 0.1;
|
||||||
model1->geom_rgba[0] = 0.2;
|
model1->geom_rgba[0] = 0.2;
|
||||||
mj_copyModel(model2, model1);
|
mj_copyModel(model2, model1.get());
|
||||||
|
|
||||||
EXPECT_FLOAT_EQ(model2->mesh_vert[0], 0.1);
|
EXPECT_FLOAT_EQ(model2->mesh_vert[0], 0.1);
|
||||||
EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.2);
|
EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.2);
|
||||||
|
|
||||||
model1->mesh_vert[0] = 0.3;
|
model1->mesh_vert[0] = 0.3;
|
||||||
model1->geom_rgba[0] = 0.4;
|
model1->geom_rgba[0] = 0.4;
|
||||||
mjv_copyModel(model2, model1);
|
mjv_copyModel(model2, model1.get());
|
||||||
|
|
||||||
EXPECT_FLOAT_EQ(model2->mesh_vert[0], 0.1); // unchanged
|
EXPECT_FLOAT_EQ(model2->mesh_vert[0], 0.1); // unchanged
|
||||||
EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.4);
|
EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.4);
|
||||||
|
|
||||||
mj_deleteModel(model2);
|
mj_deleteModel(model2);
|
||||||
mj_deleteModel(model1);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EngineIoTest, MjvCopyData) {
|
TEST_F(EngineIoTest, MjvCopyData) {
|
||||||
@@ -239,23 +228,21 @@ TEST_F(EngineIoTest, MjvCopyData) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
mjData* data1 = mj_makeData(model);
|
MjDataPtr data1 = MakeData(model);
|
||||||
mj_forward(model, data1);
|
mj_forward(model.get(), data1.get());
|
||||||
EXPECT_THAT(data1->efc_J, NotNull());
|
EXPECT_THAT(data1->efc_J, NotNull());
|
||||||
|
|
||||||
mjData* data2 = mj_copyData(nullptr, model, data1);
|
mjData* data2 = mj_copyData(nullptr, model.get(), data1.get());
|
||||||
EXPECT_THAT(data2->efc_J, NotNull());
|
EXPECT_THAT(data2->efc_J, NotNull());
|
||||||
|
|
||||||
mj_deleteData(data2);
|
mj_deleteData(data2);
|
||||||
data2 = mjv_copyData(nullptr, model, data1);
|
data2 = mjv_copyData(nullptr, model.get(), data1.get());
|
||||||
EXPECT_THAT(data2->efc_J, IsNull());
|
EXPECT_THAT(data2->efc_J, IsNull());
|
||||||
|
|
||||||
mj_deleteData(data2);
|
mj_deleteData(data2);
|
||||||
mj_deleteData(data1);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
using ValidateReferencesTest = MujocoTest;
|
using ValidateReferencesTest = MujocoTest;
|
||||||
@@ -273,15 +260,14 @@ TEST_F(ValidateReferencesTest, BodyReferences) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
EXPECT_THAT(mj_validateReferences(model), IsNull());
|
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
|
||||||
|
|
||||||
model->jnt_bodyid[0] = 2;
|
model->jnt_bodyid[0] = 2;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("jnt_bodyid"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("jnt_bodyid"));
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ValidateReferencesTest, AddressRange) {
|
TEST_F(ValidateReferencesTest, AddressRange) {
|
||||||
@@ -298,20 +284,19 @@ TEST_F(ValidateReferencesTest, AddressRange) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
EXPECT_THAT(mj_validateReferences(model), IsNull());
|
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
|
||||||
|
|
||||||
model->body_jntnum[1] = 3;
|
model->body_jntnum[1] = 3;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("body_jntadr"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("body_jntadr"));
|
||||||
model->body_jntnum[1] = 2;
|
model->body_jntnum[1] = 2;
|
||||||
// Could be more strict and test for -1, but at the moment the code is a bit
|
// Could be more strict and test for -1, but at the moment the code is a bit
|
||||||
// lenient.
|
// lenient.
|
||||||
model->body_jntadr[1] = -2;
|
model->body_jntadr[1] = -2;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("body_jntadr"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("body_jntadr"));
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ValidateReferencesTest, AddressRangeNegativeNum) {
|
TEST_F(ValidateReferencesTest, AddressRangeNegativeNum) {
|
||||||
@@ -328,16 +313,16 @@ TEST_F(ValidateReferencesTest, AddressRangeNegativeNum) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
EXPECT_THAT(mj_validateReferences(model), IsNull());
|
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
|
||||||
|
|
||||||
// jntadr + jntnum is within safe range, but jntnum is negative.
|
// jntadr + jntnum is within safe range, but jntnum is negative.
|
||||||
model->body_jntadr[1] += 5;
|
model->body_jntadr[1] += 5;
|
||||||
model->body_jntnum[1] -= 5;
|
model->body_jntnum[1] -= 5;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("body_jntnum"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("body_jntnum"));
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ValidateReferencesTest, GeomCondim) {
|
TEST_F(ValidateReferencesTest, GeomCondim) {
|
||||||
@@ -353,16 +338,15 @@ TEST_F(ValidateReferencesTest, GeomCondim) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
EXPECT_THAT(mj_validateReferences(model), IsNull());
|
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
|
||||||
model->geom_condim[0] = 7;
|
model->geom_condim[0] = 7;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("geom_condim"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("geom_condim"));
|
||||||
model->geom_condim[0] = -1;
|
model->geom_condim[0] = -1;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("geom_condim"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("geom_condim"));
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ValidateReferencesTest, HField) {
|
TEST_F(ValidateReferencesTest, HField) {
|
||||||
@@ -375,18 +359,17 @@ TEST_F(ValidateReferencesTest, HField) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
EXPECT_THAT(mj_validateReferences(model), IsNull());
|
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
|
||||||
|
|
||||||
model->hfield_adr[0] = -2;
|
model->hfield_adr[0] = -2;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("hfield_adr"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("hfield_adr"));
|
||||||
model->hfield_adr[0] = 0;
|
model->hfield_adr[0] = 0;
|
||||||
model->hfield_ncol[0] = 4;
|
model->hfield_ncol[0] = 4;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("hfield_adr"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("hfield_adr"));
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ValidateReferencesTest, Texture) {
|
TEST_F(ValidateReferencesTest, Texture) {
|
||||||
@@ -399,18 +382,17 @@ TEST_F(ValidateReferencesTest, Texture) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
EXPECT_THAT(mj_validateReferences(model), IsNull());
|
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
|
||||||
|
|
||||||
model->tex_adr[0] = -2;
|
model->tex_adr[0] = -2;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("tex_adr"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("tex_adr"));
|
||||||
model->tex_adr[0] = 0;
|
model->tex_adr[0] = 0;
|
||||||
model->tex_height[0] = 4;
|
model->tex_height[0] = 4;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("tex_adr"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("tex_adr"));
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ValidateReferencesTest, GeomPairs) {
|
TEST_F(ValidateReferencesTest, GeomPairs) {
|
||||||
@@ -433,19 +415,18 @@ TEST_F(ValidateReferencesTest, GeomPairs) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
EXPECT_THAT(mj_validateReferences(model), IsNull());
|
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
|
||||||
|
|
||||||
// Invalid geomid=4
|
// Invalid geomid=4
|
||||||
model->pair_signature[0] = (1 << 16) | 5;
|
model->pair_signature[0] = (1 << 16) | 5;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("pair_body1"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("pair_body1"));
|
||||||
|
|
||||||
model->pair_signature[0] = (5 << 16) | 1;
|
model->pair_signature[0] = (5 << 16) | 1;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("pair_body2"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("pair_body2"));
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ValidateReferencesTest, SensorsAddress) {
|
TEST_F(ValidateReferencesTest, SensorsAddress) {
|
||||||
@@ -482,12 +463,12 @@ TEST_F(ValidateReferencesTest, SensorsAddress) {
|
|||||||
for (const std::string& sensor_string : sensor_strings) {
|
for (const std::string& sensor_string : sensor_strings) {
|
||||||
std::string xml = absl::StrFormat(xml_template, sensor_string);
|
std::string xml = absl::StrFormat(xml_template, sensor_string);
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model =
|
MjModelPtr model =
|
||||||
LoadModelFromString(xml.c_str(), error.data(), error.size());
|
LoadModelFromString(xml.c_str(), error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
EXPECT_THAT(mj_validateReferences(model), IsNull());
|
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -503,11 +484,11 @@ TEST_F(ValidateReferencesTest, SensorsAddressUser) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
EXPECT_THAT(mj_validateReferences(model), IsNull());
|
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ValidateReferencesTest, SensorsObj) {
|
TEST_F(ValidateReferencesTest, SensorsObj) {
|
||||||
@@ -527,25 +508,24 @@ TEST_F(ValidateReferencesTest, SensorsObj) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
model->sensor_objtype[0] = -1;
|
model->sensor_objtype[0] = -1;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_objtype"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("sensor_objtype"));
|
||||||
model->sensor_objtype[0] = mjOBJ_SITE;
|
model->sensor_objtype[0] = mjOBJ_SITE;
|
||||||
|
|
||||||
model->sensor_objid[0] = model->nsite;
|
model->sensor_objid[0] = model->nsite;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_objid"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("sensor_objid"));
|
||||||
model->sensor_objid[0] = 0;
|
model->sensor_objid[0] = 0;
|
||||||
|
|
||||||
model->sensor_reftype[0] = -1;
|
model->sensor_reftype[0] = -1;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_reftype"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("sensor_reftype"));
|
||||||
model->sensor_reftype[0] = mjOBJ_BODY;
|
model->sensor_reftype[0] = mjOBJ_BODY;
|
||||||
|
|
||||||
model->sensor_refid[0] = model->nbody;
|
model->sensor_refid[0] = model->nbody;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_refid"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("sensor_refid"));
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ValidateReferencesTest, MoreBodiesThanGeoms) {
|
TEST_F(ValidateReferencesTest, MoreBodiesThanGeoms) {
|
||||||
@@ -565,10 +545,10 @@ TEST_F(ValidateReferencesTest, MoreBodiesThanGeoms) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
EXPECT_THAT(mj_validateReferences(model), IsNull());
|
<< "Failed to load model: " << error.data();
|
||||||
mj_deleteModel(model);
|
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ValidateReferencesTest, BodyExcludes) {
|
TEST_F(ValidateReferencesTest, BodyExcludes) {
|
||||||
@@ -585,18 +565,17 @@ TEST_F(ValidateReferencesTest, BodyExcludes) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
EXPECT_THAT(mj_validateReferences(model), IsNull());
|
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
|
||||||
|
|
||||||
// Invalid bodyid=3
|
// Invalid bodyid=3
|
||||||
model->exclude_signature[0] = (1 << 16) | 4;
|
model->exclude_signature[0] = (1 << 16) | 4;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("exclude_body1"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("exclude_body1"));
|
||||||
model->exclude_signature[0] = (4 << 16) | 2;
|
model->exclude_signature[0] = (4 << 16) | 2;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("exclude_body2"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("exclude_body2"));
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ValidateReferencesTest, EqualityConstraints) {
|
TEST_F(ValidateReferencesTest, EqualityConstraints) {
|
||||||
@@ -634,38 +613,37 @@ TEST_F(ValidateReferencesTest, EqualityConstraints) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
EXPECT_THAT(mj_validateReferences(model), IsNull());
|
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
|
||||||
|
|
||||||
// connect constraint
|
// connect constraint
|
||||||
model->eq_obj1id[0] = -1;
|
model->eq_obj1id[0] = -1;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj1id"));
|
||||||
model->eq_obj1id[0] = model->nbody;
|
model->eq_obj1id[0] = model->nbody;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj1id"));
|
||||||
model->eq_obj1id[0] = 1;
|
model->eq_obj1id[0] = 1;
|
||||||
|
|
||||||
model->eq_obj2id[0] = -2;
|
model->eq_obj2id[0] = -2;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj2id"));
|
||||||
model->eq_obj2id[0] = model->nbody;
|
model->eq_obj2id[0] = model->nbody;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj2id"));
|
||||||
model->eq_obj2id[0] = 0;
|
model->eq_obj2id[0] = 0;
|
||||||
|
|
||||||
// weld constraint
|
// weld constraint
|
||||||
model->eq_obj1id[1] = -1;
|
model->eq_obj1id[1] = -1;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj1id"));
|
||||||
model->eq_obj1id[1] = model->nbody;
|
model->eq_obj1id[1] = model->nbody;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj1id"));
|
||||||
model->eq_obj1id[1] = 1;
|
model->eq_obj1id[1] = 1;
|
||||||
|
|
||||||
model->eq_obj2id[1] = -2;
|
model->eq_obj2id[1] = -2;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj2id"));
|
||||||
model->eq_obj2id[1] = model->nbody;
|
model->eq_obj2id[1] = model->nbody;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj2id"));
|
||||||
model->eq_obj2id[1] = model->nbody - 1;
|
model->eq_obj2id[1] = model->nbody - 1;
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ValidateReferencesTest, Tuples) {
|
TEST_F(ValidateReferencesTest, Tuples) {
|
||||||
@@ -692,19 +670,19 @@ TEST_F(ValidateReferencesTest, Tuples) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
EXPECT_THAT(mj_validateReferences(model), IsNull());
|
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
|
||||||
|
|
||||||
model->tuple_objtype[0] = -1;
|
model->tuple_objtype[0] = -1;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("tuple_objtype"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("tuple_objtype"));
|
||||||
model->tuple_objtype[0] = mjOBJ_BODY;
|
model->tuple_objtype[0] = mjOBJ_BODY;
|
||||||
|
|
||||||
model->tuple_objid[0] = model->nbody;
|
model->tuple_objid[0] = model->nbody;
|
||||||
EXPECT_THAT(mj_validateReferences(model), HasSubstr("tuple_objid"));
|
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("tuple_objid"));
|
||||||
model->tuple_objid[0] = 1;
|
model->tuple_objid[0] = 1;
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EngineIoTest, CanMarkAndFreeStack) {
|
TEST_F(EngineIoTest, CanMarkAndFreeStack) {
|
||||||
@@ -716,20 +694,18 @@ TEST_F(EngineIoTest, CanMarkAndFreeStack) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
|
|
||||||
auto pstack_before = data->pstack;
|
auto pstack_before = data->pstack;
|
||||||
mj_markStack(data);
|
mj_markStack(data.get());
|
||||||
EXPECT_GT(data->pstack, pstack_before);
|
EXPECT_GT(data->pstack, pstack_before);
|
||||||
mj_freeStack(data);
|
mj_freeStack(data.get());
|
||||||
EXPECT_EQ(data->pstack, pstack_before);
|
EXPECT_EQ(data->pstack, pstack_before);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EngineIoTest, LargeMemory) {
|
TEST_F(EngineIoTest, LargeMemory) {
|
||||||
@@ -740,27 +716,25 @@ TEST_F(EngineIoTest, LargeMemory) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
mjData* data = mj_makeData(model);
|
<< "Failed to load model: " << error.data();
|
||||||
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
|
|
||||||
// allocate 2.3G of mjtNums
|
// allocate 2.3G of mjtNums
|
||||||
mj_markStack(data);
|
mj_markStack(data.get());
|
||||||
size_t num = 2300000000 / sizeof(mjtNum);
|
size_t num = 2300000000 / sizeof(mjtNum);
|
||||||
mjtNum* testNum = mj_stackAllocNum(data, num);
|
mjtNum* testNum = mj_stackAllocNum(data.get(), num);
|
||||||
testNum[num - 1] = 1;
|
testNum[num - 1] = 1;
|
||||||
mj_freeStack(data);
|
mj_freeStack(data.get());
|
||||||
|
|
||||||
// allocate 2.3G of bytes
|
// allocate 2.3G of bytes
|
||||||
mj_markStack(data);
|
mj_markStack(data.get());
|
||||||
num = 2300000000;
|
num = 2300000000;
|
||||||
char* testByte = (char*) mj_stackAllocByte(data, num, alignof(char));
|
char* testByte = (char*)mj_stackAllocByte(data.get(), num, alignof(char));
|
||||||
testByte[num - 1] = 1;
|
testByte[num - 1] = 1;
|
||||||
mj_freeStack(data);
|
mj_freeStack(data.get());
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EngineIoTest, VeryLargeMemory) {
|
TEST_F(EngineIoTest, VeryLargeMemory) {
|
||||||
@@ -771,31 +745,29 @@ TEST_F(EngineIoTest, VeryLargeMemory) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
if (!model) {
|
if (!model.get()) {
|
||||||
// in some test environments, 8GB is too large
|
// in some test environments, 8GB is too large
|
||||||
EXPECT_THAT(error.data(), HasSubstr("Could not allocate memory"));
|
EXPECT_THAT(error.data(), HasSubstr("Could not allocate memory"));
|
||||||
} else {
|
} else {
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
mjData* data = mj_makeData(model);
|
<< "Failed to load model: " << error.data();
|
||||||
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
|
|
||||||
// allocate 7G of mjtNums
|
// allocate 7G of mjtNums
|
||||||
mj_markStack(data);
|
mj_markStack(data.get());
|
||||||
size_t num = 7516192768ull / sizeof(mjtNum);
|
size_t num = 7516192768ull / sizeof(mjtNum);
|
||||||
mjtNum* testNum = mj_stackAllocNum(data, num);
|
mjtNum* testNum = mj_stackAllocNum(data.get(), num);
|
||||||
testNum[num - 1] = 1;
|
testNum[num - 1] = 1;
|
||||||
mj_freeStack(data);
|
mj_freeStack(data.get());
|
||||||
|
|
||||||
// allocate 7G of bytes
|
// allocate 7G of bytes
|
||||||
mj_markStack(data);
|
mj_markStack(data.get());
|
||||||
num = 7516192768ull;
|
num = 7516192768ull;
|
||||||
char* testByte = (char*) mj_stackAllocByte(data, num, alignof(char));
|
char* testByte = (char*)mj_stackAllocByte(data.get(), num, alignof(char));
|
||||||
testByte[num - 1] = 1;
|
testByte[num - 1] = 1;
|
||||||
mj_freeStack(data);
|
mj_freeStack(data.get());
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -822,23 +794,22 @@ TEST_F(EngineIoTest, TestStackShardingForThreads) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
mju_threadpool(data, 10);
|
mju_threadpool(data.get(), 10);
|
||||||
|
|
||||||
constexpr int kTasks = 1000;
|
constexpr int kTasks = 1000;
|
||||||
TestFunctionArgs test_function_args;
|
TestFunctionArgs test_function_args;
|
||||||
mju_dispatch(model, data, TestFunction, &test_function_args, kTasks);
|
mju_dispatch(model.get(), data.get(), TestFunction, &test_function_args,
|
||||||
|
kTasks);
|
||||||
|
|
||||||
for (int i = 0; i < kTasks; ++i) {
|
for (int i = 0; i < kTasks; ++i) {
|
||||||
EXPECT_EQ(i, test_function_args.stack_output[i]);
|
EXPECT_EQ(i, test_function_args.stack_output[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef ADDRESS_SANITIZER
|
#ifdef ADDRESS_SANITIZER
|
||||||
@@ -858,37 +829,36 @@ TEST_F(EngineIoTest, CanDetectStackFrameLeakage) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
|
|
||||||
// MarkFreeStack correctly calls mj_freeStack, should not error.
|
// MarkFreeStack correctly calls mj_freeStack, should not error.
|
||||||
MarkFreeStack(data, /* free= */ true);
|
MarkFreeStack(data.get(), /* free= */ true);
|
||||||
|
|
||||||
// MarkFreeStack calls mj_markStack without mj_freeStack, the next call to
|
// MarkFreeStack calls mj_markStack without mj_freeStack, the next call to
|
||||||
// mj_freeStack should detect the stack frame leakage.
|
// mj_freeStack should detect the stack frame leakage.
|
||||||
mj_markStack(data);
|
mj_markStack(data.get());
|
||||||
MarkFreeStack(data, /* free= */ false);
|
MarkFreeStack(data.get(), /* free= */ false);
|
||||||
EXPECT_THAT(
|
EXPECT_THAT(
|
||||||
MjuErrorMessageFrom(mj_freeStack)(data),
|
MjuErrorMessageFrom(mj_freeStack)(data.get()),
|
||||||
ContainsRegex(
|
ContainsRegex(
|
||||||
"mj_markStack in MarkFreeStack at .*engine_io_test\\.cc.* has no "
|
"mj_markStack in MarkFreeStack at .*engine_io_test\\.cc.* has no "
|
||||||
"corresponding mj_freeStack"));
|
"corresponding mj_freeStack"));
|
||||||
|
|
||||||
// Dangling stack frames should be detected in mj_deleteData.
|
// Dangling stack frames should be detected in mj_deleteData.
|
||||||
mj_resetData(model, data);
|
mj_resetData(model.get(), data.get());
|
||||||
mj_markStack(data);
|
mj_markStack(data.get());
|
||||||
EXPECT_THAT(
|
EXPECT_THAT(
|
||||||
MjuErrorMessageFrom(mj_deleteData)(data),
|
MjuErrorMessageFrom(mj_deleteData)(data.get()),
|
||||||
ContainsRegex(
|
ContainsRegex(
|
||||||
"mj_markStack in .+EngineIoTest_CanDetectStackFrameLeakage.+ has no "
|
"mj_markStack in .+EngineIoTest_CanDetectStackFrameLeakage.+ has no "
|
||||||
"corresponding mj_freeStack"));
|
"corresponding mj_freeStack"));
|
||||||
|
|
||||||
mj_resetData(model, data);
|
mj_resetData(model.get(), data.get());
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EngineIoTest, RedZoneAlignmentTest) {
|
TEST_F(EngineIoTest, RedZoneAlignmentTest) {
|
||||||
@@ -900,19 +870,17 @@ TEST_F(EngineIoTest, RedZoneAlignmentTest) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
|
|
||||||
mj_markStack(data);
|
mj_markStack(data.get());
|
||||||
mj_stackAllocByte(data, 1, 1);
|
mj_stackAllocByte(data.get(), 1, 1);
|
||||||
mj_stackAllocByte(data, 1, 1);
|
mj_stackAllocByte(data.get(), 1, 1);
|
||||||
mj_freeStack(data);
|
mj_freeStack(data.get());
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
@@ -930,15 +898,15 @@ TEST_F(EngineIoTest, LoadModelBufferRejectsOverflowingSizes) {
|
|||||||
|
|
||||||
constexpr char xml[] = "<mujoco />";
|
constexpr char xml[] = "<mujoco />";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
|
|
||||||
// save model to a buffer
|
// save model to a buffer
|
||||||
int bufsize = mj_sizeModel(model);
|
int bufsize = mj_sizeModel(model.get());
|
||||||
ASSERT_GT(bufsize, 0);
|
ASSERT_GT(bufsize, 0);
|
||||||
std::vector<char> buffer(bufsize);
|
std::vector<char> buffer(bufsize);
|
||||||
mj_saveModel(model, nullptr, buffer.data(), bufsize);
|
mj_saveModel(model.get(), nullptr, buffer.data(), bufsize);
|
||||||
mj_deleteModel(model);
|
|
||||||
|
|
||||||
// locate the size fields in the buffer (after 5-int header)
|
// locate the size fields in the buffer (after 5-int header)
|
||||||
const int header_bytes = 5 * sizeof(int);
|
const int header_bytes = 5 * sizeof(int);
|
||||||
|
|||||||
@@ -14,6 +14,8 @@
|
|||||||
|
|
||||||
// Tests for engine/engine_island.c.
|
// Tests for engine/engine_island.c.
|
||||||
|
|
||||||
|
#include "src/engine/engine_island.h"
|
||||||
|
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
@@ -21,7 +23,6 @@
|
|||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <mujoco/mjmodel.h>
|
#include <mujoco/mjmodel.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
#include "src/engine/engine_island.h"
|
|
||||||
#include "src/engine/engine_util_sparse.h"
|
#include "src/engine/engine_util_sparse.h"
|
||||||
#include "test/fixture.h"
|
#include "test/fixture.h"
|
||||||
|
|
||||||
@@ -34,16 +35,11 @@ using ::testing::NotNull;
|
|||||||
using ::testing::Pointwise;
|
using ::testing::Pointwise;
|
||||||
using IslandTest = MujocoTest;
|
using IslandTest = MujocoTest;
|
||||||
|
|
||||||
|
|
||||||
TEST_F(IslandTest, FloodFillSingleton) {
|
TEST_F(IslandTest, FloodFillSingleton) {
|
||||||
// adjacency matrix for the graph 0 1 2
|
// adjacency matrix for the graph 0 1 2
|
||||||
// U U
|
// U U
|
||||||
// (3 singletons, 0 and 2 have self-edges)
|
// (3 singletons, 0 and 2 have self-edges)
|
||||||
mjtNum mat[9] = {
|
mjtNum mat[9] = {1, 0, 0, 0, 0, 0, 0, 0, 1};
|
||||||
1, 0, 0,
|
|
||||||
0, 0, 0,
|
|
||||||
0, 0, 1
|
|
||||||
};
|
|
||||||
constexpr int nr = 3;
|
constexpr int nr = 3;
|
||||||
constexpr int nnz = 2;
|
constexpr int nnz = 2;
|
||||||
int rownnz[nr];
|
int rownnz[nr];
|
||||||
@@ -63,14 +59,9 @@ TEST_F(IslandTest, FloodFillSingleton) {
|
|||||||
EXPECT_THAT(island, ElementsAre(0, -1, 1));
|
EXPECT_THAT(island, ElementsAre(0, -1, 1));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(IslandTest, FloodFill1) {
|
TEST_F(IslandTest, FloodFill1) {
|
||||||
// adjacency matrix for the graph 0 - 1 - 2
|
// adjacency matrix for the graph 0 - 1 - 2
|
||||||
mjtNum mat[9] = {
|
mjtNum mat[9] = {0, 1, 0, 1, 0, 1, 0, 1, 0};
|
||||||
0, 1, 0,
|
|
||||||
1, 0, 1,
|
|
||||||
0, 1, 0
|
|
||||||
};
|
|
||||||
constexpr int nr = 3;
|
constexpr int nr = 3;
|
||||||
constexpr int nnz = 4;
|
constexpr int nnz = 4;
|
||||||
int rownnz[nr];
|
int rownnz[nr];
|
||||||
@@ -89,17 +80,11 @@ TEST_F(IslandTest, FloodFill1) {
|
|||||||
EXPECT_THAT(island, ElementsAre(0, 0, 0));
|
EXPECT_THAT(island, ElementsAre(0, 0, 0));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(IslandTest, FloodFill2) {
|
TEST_F(IslandTest, FloodFill2) {
|
||||||
// adjacency matrix for the graph 6 – 1 – 4 0 – 3 – 5 – 2
|
// adjacency matrix for the graph 6 – 1 – 4 0 – 3 – 5 – 2
|
||||||
mjtNum mat[49] = {
|
mjtNum mat[49] = {
|
||||||
0, 0, 0, 1, 0, 0, 0,
|
0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0,
|
||||||
0, 0, 0, 0, 1, 0, 1,
|
0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
|
||||||
0, 0, 0, 0, 0, 1, 0,
|
|
||||||
1, 0, 0, 0, 0, 1, 0,
|
|
||||||
0, 1, 0, 0, 0, 0, 0,
|
|
||||||
0, 0, 1, 1, 0, 0, 0,
|
|
||||||
0, 1, 0, 0, 0, 0, 0,
|
|
||||||
};
|
};
|
||||||
constexpr int nr = 7;
|
constexpr int nr = 7;
|
||||||
constexpr int nnz = 10;
|
constexpr int nnz = 10;
|
||||||
@@ -119,15 +104,11 @@ TEST_F(IslandTest, FloodFill2) {
|
|||||||
EXPECT_THAT(island, ElementsAre(0, 1, 0, 0, 1, 0, 1));
|
EXPECT_THAT(island, ElementsAre(0, 1, 0, 0, 1, 0, 1));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(IslandTest, FloodFill3a) {
|
TEST_F(IslandTest, FloodFill3a) {
|
||||||
// adjacency matrix for the graph 0 2 1 – 3
|
// adjacency matrix for the graph 0 2 1 – 3
|
||||||
// U
|
// U
|
||||||
mjtNum mat[16] = {
|
mjtNum mat[16] = {
|
||||||
0, 0, 0, 0,
|
0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0,
|
||||||
0, 0, 0, 1,
|
|
||||||
0, 0, 1, 0,
|
|
||||||
0, 1, 0, 0,
|
|
||||||
};
|
};
|
||||||
constexpr int nr = 4;
|
constexpr int nr = 4;
|
||||||
constexpr int nnz = 3;
|
constexpr int nnz = 3;
|
||||||
@@ -147,7 +128,6 @@ TEST_F(IslandTest, FloodFill3a) {
|
|||||||
EXPECT_THAT(island, ElementsAre(-1, 0, 1, 0));
|
EXPECT_THAT(island, ElementsAre(-1, 0, 1, 0));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(IslandTest, FloodFill3b) {
|
TEST_F(IslandTest, FloodFill3b) {
|
||||||
/*
|
/*
|
||||||
adjacency matrix for the graph 1 – 2 3 4 – 5
|
adjacency matrix for the graph 1 – 2 3 4 – 5
|
||||||
@@ -155,13 +135,8 @@ TEST_F(IslandTest, FloodFill3b) {
|
|||||||
0 – 6
|
0 – 6
|
||||||
*/
|
*/
|
||||||
mjtNum mat[49] = {
|
mjtNum mat[49] = {
|
||||||
0, 0, 0, 0, 1, 0, 1,
|
0, 0, 0, 0, 1, 0, 1, 0, 1, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||||
0, 1, 1, 0, 0, 0, 0,
|
0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0,
|
||||||
0, 1, 0, 0, 0, 0, 0,
|
|
||||||
0, 0, 0, 0, 0, 0, 0,
|
|
||||||
1, 0, 0, 0, 0, 1, 1,
|
|
||||||
0, 0, 0, 0, 1, 0, 1,
|
|
||||||
1, 0, 0, 0, 1, 1, 0,
|
|
||||||
};
|
};
|
||||||
constexpr int nr = 7;
|
constexpr int nr = 7;
|
||||||
constexpr int nnz = 13;
|
constexpr int nnz = 13;
|
||||||
@@ -181,8 +156,7 @@ TEST_F(IslandTest, FloodFill3b) {
|
|||||||
EXPECT_THAT(island, ElementsAre(0, 1, 1, -1, 0, 0, 0));
|
EXPECT_THAT(island, ElementsAre(0, 1, 1, -1, 0, 0, 0));
|
||||||
}
|
}
|
||||||
|
|
||||||
static const char* const kAbacusPath =
|
static const char* const kAbacusPath = "engine/testdata/island/abacus.xml";
|
||||||
"engine/testdata/island/abacus.xml";
|
|
||||||
|
|
||||||
TEST_F(IslandTest, Abacus) {
|
TEST_F(IslandTest, Abacus) {
|
||||||
const std::string xml_path = GetTestDataFilePath(kAbacusPath);
|
const std::string xml_path = GetTestDataFilePath(kAbacusPath);
|
||||||
@@ -327,8 +301,7 @@ TEST_F(IslandTest, DenseSparse) {
|
|||||||
AsVector(data2->island_idofadr, nisland));
|
AsVector(data2->island_idofadr, nisland));
|
||||||
EXPECT_EQ(AsVector(data1->island_nv, nisland),
|
EXPECT_EQ(AsVector(data1->island_nv, nisland),
|
||||||
AsVector(data2->island_nv, nisland));
|
AsVector(data2->island_nv, nisland));
|
||||||
EXPECT_EQ(AsVector(data1->dof_island, nv),
|
EXPECT_EQ(AsVector(data1->dof_island, nv), AsVector(data2->dof_island, nv));
|
||||||
AsVector(data2->dof_island, nv));
|
|
||||||
EXPECT_EQ(AsVector(data1->map_idof2dof, nv),
|
EXPECT_EQ(AsVector(data1->map_idof2dof, nv),
|
||||||
AsVector(data2->map_idof2dof, nv));
|
AsVector(data2->map_idof2dof, nv));
|
||||||
EXPECT_EQ(AsVector(data1->map_dof2idof, nv),
|
EXPECT_EQ(AsVector(data1->map_dof2idof, nv),
|
||||||
@@ -462,12 +435,10 @@ TEST_F(IslandTest, EqualityConstraintOfTendons) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(IslandTest, PGSIsland) {
|
TEST_F(IslandTest, PGSIsland) {
|
||||||
@@ -496,8 +467,7 @@ TEST_F(IslandTest, PGSIsland) {
|
|||||||
// solve without islands
|
// solve without islands
|
||||||
m->opt.disableflags |= mjDSBL_ISLAND;
|
m->opt.disableflags |= mjDSBL_ISLAND;
|
||||||
mj_forward(m, d);
|
mj_forward(m, d);
|
||||||
std::vector<mjtNum> qfrc_mono(d->qfrc_constraint,
|
std::vector<mjtNum> qfrc_mono(d->qfrc_constraint, d->qfrc_constraint + m->nv);
|
||||||
d->qfrc_constraint + m->nv);
|
|
||||||
|
|
||||||
// expect close match (inexact due to randomized constraint visitation order)
|
// expect close match (inexact due to randomized constraint visitation order)
|
||||||
EXPECT_THAT(qfrc_island, Pointwise(MjNear(1e-3, 1e-3), qfrc_mono));
|
EXPECT_THAT(qfrc_island, Pointwise(MjNear(1e-3, 1e-3), qfrc_mono));
|
||||||
|
|||||||
+130
-194
@@ -49,28 +49,25 @@ TEST_F(PassiveTest, DisableFlags) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(flex_xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(flex_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_resetDataKeyframe(m, d, 0);
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
EXPECT_MJTNUM_EQ(d->qacc[0], 11);
|
EXPECT_MJTNUM_EQ(d->qacc[0], 11);
|
||||||
|
|
||||||
m->opt.disableflags = mjDSBL_DAMPER;
|
m->opt.disableflags = mjDSBL_DAMPER;
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
EXPECT_MJTNUM_EQ(d->qacc[0], 10);
|
EXPECT_MJTNUM_EQ(d->qacc[0], 10);
|
||||||
|
|
||||||
m->opt.disableflags = mjDSBL_SPRING;
|
m->opt.disableflags = mjDSBL_SPRING;
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
EXPECT_MJTNUM_EQ(d->qacc[0], 1);
|
EXPECT_MJTNUM_EQ(d->qacc[0], 1);
|
||||||
|
|
||||||
m->opt.disableflags = mjDSBL_SPRING | mjDSBL_DAMPER;
|
m->opt.disableflags = mjDSBL_SPRING | mjDSBL_DAMPER;
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
EXPECT_EQ(d->qacc[0], -10);
|
EXPECT_EQ(d->qacc[0], -10);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PassiveTest, GravcompNestedBody) {
|
TEST_F(PassiveTest, GravcompNestedBody) {
|
||||||
@@ -89,17 +86,14 @@ TEST_F(PassiveTest, GravcompNestedBody) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
EXPECT_GT(d->qacc[2], 0);
|
EXPECT_GT(d->qacc[2], 0);
|
||||||
EXPECT_NEAR(d->qacc[2], 2.0, 0.1);
|
EXPECT_NEAR(d->qacc[2], 2.0, 0.1);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PassiveTest, PolyStiffnessSlide) {
|
TEST_F(PassiveTest, PolyStiffnessSlide) {
|
||||||
@@ -119,16 +113,13 @@ TEST_F(PassiveTest, PolyStiffnessSlide) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_resetDataKeyframe(m, d, 0);
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48);
|
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PassiveTest, PolyStiffnessAntiSymmetric) {
|
TEST_F(PassiveTest, PolyStiffnessAntiSymmetric) {
|
||||||
@@ -148,16 +139,13 @@ TEST_F(PassiveTest, PolyStiffnessAntiSymmetric) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_resetDataKeyframe(m, d, 0);
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], 8);
|
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], 8);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PassiveTest, PolyStiffnessTendon) {
|
TEST_F(PassiveTest, PolyStiffnessTendon) {
|
||||||
@@ -183,21 +171,18 @@ TEST_F(PassiveTest, PolyStiffnessTendon) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
|
|
||||||
m->tendon_stiffness[0] = 10;
|
m->tendon_stiffness[0] = 10;
|
||||||
m->tendon_stiffnesspoly[0] = 5;
|
m->tendon_stiffnesspoly[0] = 5;
|
||||||
m->tendon_stiffnesspoly[1] = 1;
|
m->tendon_stiffnesspoly[1] = 1;
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_resetDataKeyframe(m, d, 0);
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48);
|
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PassiveTest, PolyStiffnessEnergy) {
|
TEST_F(PassiveTest, PolyStiffnessEnergy) {
|
||||||
@@ -221,21 +206,18 @@ TEST_F(PassiveTest, PolyStiffnessEnergy) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_resetDataKeyframe(m, d, 0);
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
mjtNum total_energy = d->energy[0] + d->energy[1];
|
mjtNum total_energy = d->energy[0] + d->energy[1];
|
||||||
|
|
||||||
for (int i = 0; i < 100; i++) {
|
for (int i = 0; i < 100; i++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.002);
|
EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.002);
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ------------------------ ellipsoid fluid model ------------------------------
|
// ------------------------ ellipsoid fluid model ------------------------------
|
||||||
@@ -261,9 +243,9 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m2 = LoadModelFromString(two_bodies_xml, error, sizeof(error));
|
MjModelPtr m2 = LoadModelFromString(two_bodies_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m2, NotNull()) << error;
|
ASSERT_THAT(m2.get(), NotNull()) << error;
|
||||||
mjData* d2 = mj_makeData(m2);
|
MjDataPtr d2 = MakeData(m2);
|
||||||
for (int i = 0; i < 6; i++) {
|
for (int i = 0; i < 6; i++) {
|
||||||
d2->qvel[i] = (mjtNum)i + 1;
|
d2->qvel[i] = (mjtNum)i + 1;
|
||||||
}
|
}
|
||||||
@@ -285,9 +267,9 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
|
|
||||||
mjModel* m1 = LoadModelFromString(one_body_xml, error, sizeof(error));
|
MjModelPtr m1 = LoadModelFromString(one_body_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m1, NotNull()) << error;
|
ASSERT_THAT(m1.get(), NotNull()) << error;
|
||||||
mjData* d1 = mj_makeData(m1);
|
MjDataPtr d1 = MakeData(m1);
|
||||||
for (int i = 0; i < 6; i++) {
|
for (int i = 0; i < 6; i++) {
|
||||||
d1->qvel[i] = (mjtNum)i + 1;
|
d1->qvel[i] = (mjtNum)i + 1;
|
||||||
}
|
}
|
||||||
@@ -301,16 +283,11 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
|
|||||||
|
|
||||||
EXPECT_EQ(m1->nv, m2->nv);
|
EXPECT_EQ(m1->nv, m2->nv);
|
||||||
|
|
||||||
mj_forward(m2, d2);
|
mj_forward(m2.get(), d2.get());
|
||||||
mj_forward(m1, d1);
|
mj_forward(m1.get(), d1.get());
|
||||||
for (int i = 0; i < m1->nv; i++) {
|
for (int i = 0; i < m1->nv; i++) {
|
||||||
EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol);
|
EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol);
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d1);
|
|
||||||
mj_deleteModel(m1);
|
|
||||||
mj_deleteData(d2);
|
|
||||||
mj_deleteModel(m2);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
|
TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
|
||||||
@@ -334,16 +311,13 @@ TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
EXPECT_THAT(AsVector(model->geom_fluid, 6),
|
EXPECT_THAT(AsVector(model->geom_fluid, 6), ElementsAre(0, 0, 0, 0, 0, 0));
|
||||||
ElementsAre(0, 0, 0, 0, 0, 0));
|
|
||||||
EXPECT_THAT(AsVector(model->geom_fluid + mjNFLUID, 6),
|
EXPECT_THAT(AsVector(model->geom_fluid + mjNFLUID, 6),
|
||||||
ElementsAre(1, 2, 3, 4, 5, 6));
|
ElementsAre(1, 2, 3, 4, 5, 6));
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// ------------------------------ tendons --------------------------------------
|
// ------------------------------ tendons --------------------------------------
|
||||||
|
|
||||||
using TendonTest = MujocoTest;
|
using TendonTest = MujocoTest;
|
||||||
@@ -391,12 +365,10 @@ TEST_F(ElasticityTest, FlexCompatibility) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(flex_xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(flex_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// -------------------------------- shell -----------------------------------
|
// -------------------------------- shell -----------------------------------
|
||||||
@@ -413,11 +385,11 @@ TEST_F(ElasticityTest, ElasticEnergyShell) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_kinematics(m, d);
|
mj_kinematics(m.get(), d.get());
|
||||||
mj_flex(m, d);
|
mj_flex(m.get(), d.get());
|
||||||
|
|
||||||
// check that a plane is in the kernel of the energy
|
// check that a plane is in the kernel of the energy
|
||||||
for (mjtNum scale = 1; scale < 4; scale++) {
|
for (mjtNum scale = 1; scale < 4; scale++) {
|
||||||
@@ -435,17 +407,15 @@ TEST_F(ElasticityTest, ElasticEnergyShell) {
|
|||||||
for (int x = 0; x < 3; x++) {
|
for (int x = 0; x < 3; x++) {
|
||||||
mjtNum elongation1 = scale * d->flexvert_xpos[3 * v[i] + x];
|
mjtNum elongation1 = scale * d->flexvert_xpos[3 * v[i] + x];
|
||||||
mjtNum elongation2 = scale * d->flexvert_xpos[3 * v[j] + x];
|
mjtNum elongation2 = scale * d->flexvert_xpos[3 * v[j] + x];
|
||||||
energy += m->flex_bending[17*e+4*i+j] * elongation1 * elongation2;
|
energy +=
|
||||||
|
m->flex_bending[17 * e + 4 * i + j] * elongation1 * elongation2;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
EXPECT_NEAR(
|
EXPECT_NEAR(4 * energy / volume, 0,
|
||||||
4*energy/volume, 0, std::numeric_limits<float>::epsilon());
|
std::numeric_limits<float>::epsilon());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ElasticityTest, CurvedShell) {
|
TEST_F(ElasticityTest, CurvedShell) {
|
||||||
@@ -487,12 +457,12 @@ TEST_F(ElasticityTest, CurvedShell) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_kinematics(m, d);
|
mj_kinematics(m.get(), d.get());
|
||||||
mj_flex(m, d);
|
mj_flex(m.get(), d.get());
|
||||||
mj_passive(m, d);
|
mj_passive(m.get(), d.get());
|
||||||
|
|
||||||
// v1 force component is in-plane along v1-v0 edge (y-axis)
|
// v1 force component is in-plane along v1-v0 edge (y-axis)
|
||||||
EXPECT_NEAR(d->qfrc_spring[3], 0, 1e-6);
|
EXPECT_NEAR(d->qfrc_spring[3], 0, 1e-6);
|
||||||
@@ -505,9 +475,6 @@ TEST_F(ElasticityTest, CurvedShell) {
|
|||||||
// v3 force component is in-plane along v3-v0 edge (z-axis)
|
// v3 force component is in-plane along v3-v0 edge (z-axis)
|
||||||
EXPECT_NEAR(d->qfrc_spring[9], 0, 1e-6);
|
EXPECT_NEAR(d->qfrc_spring[9], 0, 1e-6);
|
||||||
EXPECT_NEAR(d->qfrc_spring[10], 0, 1e-6);
|
EXPECT_NEAR(d->qfrc_spring[10], 0, 1e-6);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// -------------------------------- membrane -----------------------------------
|
// -------------------------------- membrane -----------------------------------
|
||||||
@@ -525,12 +492,12 @@ TEST_F(ElasticityTest, ElasticEnergyMembrane) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
mj_kinematics(m, d);
|
mj_kinematics(m.get(), d.get());
|
||||||
mj_flex(m, d);
|
mj_flex(m.get(), d.get());
|
||||||
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0];
|
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0];
|
||||||
|
|
||||||
// check that if the entire geometry is rescaled by a factor "scale", then
|
// check that if the entire geometry is rescaled by a factor "scale", then
|
||||||
@@ -549,16 +516,14 @@ TEST_F(ElasticityTest, ElasticEnergyMembrane) {
|
|||||||
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
|
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
|
||||||
mjtNum elong2 =
|
mjtNum elong2 =
|
||||||
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2];
|
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2];
|
||||||
energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
|
energy +=
|
||||||
|
metric[21 * t + idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
const mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-5);
|
const mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-5);
|
||||||
EXPECT_NEAR(4 * energy / volume, 2 * scale * scale, tol);
|
EXPECT_NEAR(4 * energy / volume, 2 * scale * scale, tol);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// -------------------------------- solid -----------------------------------
|
// -------------------------------- solid -----------------------------------
|
||||||
@@ -576,12 +541,12 @@ TEST_F(ElasticityTest, ElasticEnergySolid) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
mj_kinematics(m, d);
|
mj_kinematics(m.get(), d.get());
|
||||||
mj_flex(m, d);
|
mj_flex(m.get(), d.get());
|
||||||
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0];
|
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0];
|
||||||
|
|
||||||
// check that if the entire geometry is rescaled by a factor "scale", then
|
// check that if the entire geometry is rescaled by a factor "scale", then
|
||||||
@@ -600,16 +565,14 @@ TEST_F(ElasticityTest, ElasticEnergySolid) {
|
|||||||
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
|
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
|
||||||
mjtNum elong2 =
|
mjtNum elong2 =
|
||||||
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2];
|
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2];
|
||||||
energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
|
energy +=
|
||||||
|
metric[21 * t + idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
const mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-4);
|
const mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-4);
|
||||||
EXPECT_NEAR(energy / volume, 3 * scale * scale, tol);
|
EXPECT_NEAR(energy / volume, 3 * scale * scale, tol);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PassiveTest, PolynomialStiffnessJoint) {
|
TEST_F(PassiveTest, PolynomialStiffnessJoint) {
|
||||||
@@ -626,19 +589,16 @@ TEST_F(PassiveTest, PolynomialStiffnessJoint) {
|
|||||||
</keyframe>
|
</keyframe>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_THAT(m, NotNull());
|
ASSERT_THAT(m.get(), NotNull());
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_resetDataKeyframe(m, d, 0);
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
mjtNum x = 0.5;
|
mjtNum x = 0.5;
|
||||||
mjtNum a = 2, b = 3, c = 4;
|
mjtNum a = 2, b = 3, c = 4;
|
||||||
mjtNum expected = -(a + b * x + c * x * x) * x;
|
mjtNum expected = -(a + b * x + c * x * x) * x;
|
||||||
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
|
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PassiveTest, PolynomialStiffnessNegativeDisplacement) {
|
TEST_F(PassiveTest, PolynomialStiffnessNegativeDisplacement) {
|
||||||
@@ -655,19 +615,16 @@ TEST_F(PassiveTest, PolynomialStiffnessNegativeDisplacement) {
|
|||||||
</keyframe>
|
</keyframe>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_THAT(m, NotNull());
|
ASSERT_THAT(m.get(), NotNull());
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_resetDataKeyframe(m, d, 0);
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
mjtNum x = -0.5;
|
mjtNum x = -0.5;
|
||||||
mjtNum a = 2, b = 3, c = 4;
|
mjtNum a = 2, b = 3, c = 4;
|
||||||
mjtNum expected = -(a + b * x + c * x * x) * x;
|
mjtNum expected = -(a + b * x + c * x * x) * x;
|
||||||
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
|
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PassiveTest, PolyStiffnessFixedTendon) {
|
TEST_F(PassiveTest, PolyStiffnessFixedTendon) {
|
||||||
@@ -693,19 +650,16 @@ TEST_F(PassiveTest, PolyStiffnessFixedTendon) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_resetDataKeyframe(m, d, 0);
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
mjtNum x = d->ten_length[0] - m->tendon_lengthspring[1];
|
mjtNum x = d->ten_length[0] - m->tendon_lengthspring[1];
|
||||||
mjtNum expected = -(10 + 5 * x + 1 * x * x) * x;
|
mjtNum expected = -(10 + 5 * x + 1 * x * x) * x;
|
||||||
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
|
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PassiveTest, PolyStiffnessSpatialTendon) {
|
TEST_F(PassiveTest, PolyStiffnessSpatialTendon) {
|
||||||
@@ -734,23 +688,18 @@ TEST_F(PassiveTest, PolyStiffnessSpatialTendon) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_resetDataKeyframe(m, d, 0);
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
mjtNum x = d->ten_length[0] - m->tendon_lengthspring[1];
|
mjtNum x = d->ten_length[0] - m->tendon_lengthspring[1];
|
||||||
mjtNum expected = -x * (10 + 5 * x + 1 * x * x);
|
mjtNum expected = -x * (10 + 5 * x + 1 * x * x);
|
||||||
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
|
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
TEST_F(PassiveTest, PolynomialDampingJoint) {
|
TEST_F(PassiveTest, PolynomialDampingJoint) {
|
||||||
static constexpr char xml[] = R"(
|
static constexpr char xml[] = R"(
|
||||||
<mujoco>
|
<mujoco>
|
||||||
@@ -765,19 +714,16 @@ TEST_F(PassiveTest, PolynomialDampingJoint) {
|
|||||||
</keyframe>
|
</keyframe>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_THAT(m, NotNull());
|
ASSERT_THAT(m.get(), NotNull());
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_resetDataKeyframe(m, d, 0);
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
mjtNum v = 0.5;
|
mjtNum v = 0.5;
|
||||||
mjtNum a = 2, b = 3, c = 4;
|
mjtNum a = 2, b = 3, c = 4;
|
||||||
mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v);
|
mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v);
|
||||||
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
|
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PassiveTest, PolynomialDampingNegativeVelocity) {
|
TEST_F(PassiveTest, PolynomialDampingNegativeVelocity) {
|
||||||
@@ -794,19 +740,16 @@ TEST_F(PassiveTest, PolynomialDampingNegativeVelocity) {
|
|||||||
</keyframe>
|
</keyframe>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_THAT(m, NotNull());
|
ASSERT_THAT(m.get(), NotNull());
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_resetDataKeyframe(m, d, 0);
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
mjtNum v = -0.5;
|
mjtNum v = -0.5;
|
||||||
mjtNum a = 2, b = 3, c = 4;
|
mjtNum a = 2, b = 3, c = 4;
|
||||||
mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v);
|
mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v);
|
||||||
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
|
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PassiveTest, PolynomialDampingTendon) {
|
TEST_F(PassiveTest, PolynomialDampingTendon) {
|
||||||
@@ -832,19 +775,16 @@ TEST_F(PassiveTest, PolynomialDampingTendon) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_resetDataKeyframe(m, d, 0);
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
mjtNum v = d->ten_velocity[0];
|
mjtNum v = d->ten_velocity[0];
|
||||||
mjtNum expected = -(10 * v + 5 * v * mju_abs(v) + 1 * v * v * v);
|
mjtNum expected = -(10 * v + 5 * v * mju_abs(v) + 1 * v * v * v);
|
||||||
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
|
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// shell-mode (elastic2d=stretch) flexcomp must have zero passive spring forces
|
// shell-mode (elastic2d=stretch) flexcomp must have zero passive spring forces
|
||||||
@@ -867,20 +807,17 @@ TEST_F(ElasticityTest, ShellModeZeroForceAtRest) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// all spring forces should be zero at rest
|
// all spring forces should be zero at rest
|
||||||
for (int i = 0; i < m->nv; i++) {
|
for (int i = 0; i < m->nv; i++) {
|
||||||
EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10)
|
EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10)
|
||||||
<< "nonzero spring force at DOF " << i;
|
<< "nonzero spring force at DOF " << i;
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// interpolated shell bending must produce zero spring forces at rest
|
// interpolated shell bending must produce zero spring forces at rest
|
||||||
@@ -901,16 +838,16 @@ TEST_F(ElasticityTest, InterpBendingZeroForceAtRest) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
// verify bending data was compiled
|
// verify bending data was compiled
|
||||||
const mjtNum* bdata = m->flex_bending + m->flex_bendingadr[0];
|
const mjtNum* bdata = m->flex_bending + m->flex_bendingadr[0];
|
||||||
int nedge = (int)bdata[0];
|
int nedge = (int)bdata[0];
|
||||||
EXPECT_GT(nedge, 0) << "no bending edges compiled";
|
EXPECT_GT(nedge, 0) << "no bending edges compiled";
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// all spring forces should be zero at rest
|
// all spring forces should be zero at rest
|
||||||
for (int i = 0; i < m->nv; i++) {
|
for (int i = 0; i < m->nv; i++) {
|
||||||
@@ -944,9 +881,6 @@ TEST_F(ElasticityTest, InterpBendingZeroForceAtRest) {
|
|||||||
EXPECT_GT(n_flat, 0) << "no intra-surface edges found";
|
EXPECT_GT(n_flat, 0) << "no intra-surface edges found";
|
||||||
EXPECT_GT(n_corner, 0) << "no corner edges found";
|
EXPECT_GT(n_corner, 0) << "no corner edges found";
|
||||||
EXPECT_EQ(n_flat + n_corner, nedge);
|
EXPECT_EQ(n_flat + n_corner, nedge);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// interpolated shell bending must produce zero forces after a rigid rotation
|
// interpolated shell bending must produce zero forces after a rigid rotation
|
||||||
@@ -967,9 +901,9 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
// compute geometric center from body positions (skip world body)
|
// compute geometric center from body positions (skip world body)
|
||||||
mjtNum center[3] = {0, 0, 0};
|
mjtNum center[3] = {0, 0, 0};
|
||||||
@@ -981,7 +915,9 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
|
|||||||
nnodes++;
|
nnodes++;
|
||||||
}
|
}
|
||||||
ASSERT_GT(nnodes, 0);
|
ASSERT_GT(nnodes, 0);
|
||||||
center[0] /= nnodes; center[1] /= nnodes; center[2] /= nnodes;
|
center[0] /= nnodes;
|
||||||
|
center[1] /= nnodes;
|
||||||
|
center[2] /= nnodes;
|
||||||
|
|
||||||
// rotation: 45 degrees about (1,1,1)/sqrt(3)
|
// rotation: 45 degrees about (1,1,1)/sqrt(3)
|
||||||
mjtNum angle = 45 * 3.14159265358979 / 180.0;
|
mjtNum angle = 45 * 3.14159265358979 / 180.0;
|
||||||
@@ -1007,7 +943,10 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
|
|||||||
int qadr = m->jnt_qposadr[jid];
|
int qadr = m->jnt_qposadr[jid];
|
||||||
int axis = -1;
|
int axis = -1;
|
||||||
for (int a = 0; a < 3; a++) {
|
for (int a = 0; a < 3; a++) {
|
||||||
if (m->jnt_axis[3*jid + a] != 0) { axis = a; break; }
|
if (m->jnt_axis[3 * jid + a] != 0) {
|
||||||
|
axis = a;
|
||||||
|
break;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
if (axis >= 0) {
|
if (axis >= 0) {
|
||||||
d->qpos[qadr] =
|
d->qpos[qadr] =
|
||||||
@@ -1016,7 +955,7 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// spring forces should still be zero after rigid rotation
|
// spring forces should still be zero after rigid rotation
|
||||||
const mjtNum tol = MjTol(1e-6, 1e-3);
|
const mjtNum tol = MjTol(1e-6, 1e-3);
|
||||||
@@ -1024,9 +963,6 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
|
|||||||
EXPECT_NEAR(d->qfrc_spring[i], 0, tol)
|
EXPECT_NEAR(d->qfrc_spring[i], 0, tol)
|
||||||
<< "nonzero spring force at DOF " << i << " after rigid rotation";
|
<< "nonzero spring force at DOF " << i << " after rigid rotation";
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
@@ -68,13 +68,9 @@ class BaseTestPlugin {
|
|||||||
advance_count = 0;
|
advance_count = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Compute() {
|
void Compute() { compute_count += stride; }
|
||||||
compute_count += stride;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Advance() {
|
void Advance() { advance_count += stride; }
|
||||||
advance_count += stride;
|
|
||||||
}
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
int stride;
|
int stride;
|
||||||
@@ -458,11 +454,12 @@ TEST_F(MujocoTest, EmptyPluginDisallowed) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, testing::IsNull()) << error.data();
|
EXPECT_THAT(m.get(), testing::IsNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(), HasSubstr(
|
EXPECT_THAT(
|
||||||
|
error.data(),
|
||||||
|
HasSubstr(
|
||||||
"neither 'plugin' nor 'instance' is specified for body 'world'"));
|
"neither 'plugin' nor 'instance' is specified for body 'world'"));
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, FirstPartyPlugins) {
|
TEST_F(MujocoTest, FirstPartyPlugins) {
|
||||||
@@ -480,13 +477,13 @@ TEST_F(EnginePluginTest, NoAttributePlugin) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, testing::NotNull()) << error.data();
|
EXPECT_THAT(m.get(), testing::NotNull()) << error.data();
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EnginePluginTest, MultiplePluginTableBlocks) {
|
TEST_F(EnginePluginTest, MultiplePluginTableBlocks) {
|
||||||
EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
|
EXPECT_EQ(mjp_pluginCount(),
|
||||||
|
kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
|
||||||
|
|
||||||
const mjpPlugin* last_plugin = nullptr;
|
const mjpPlugin* last_plugin = nullptr;
|
||||||
int table_count = 0;
|
int table_count = 0;
|
||||||
@@ -513,16 +510,17 @@ TEST_F(EnginePluginTest, RegisterIdenticalPlugin) {
|
|||||||
EXPECT_EQ(RegisterSensorPlugin(), kNumTruePlugins);
|
EXPECT_EQ(RegisterSensorPlugin(), kNumTruePlugins);
|
||||||
EXPECT_EQ(RegisterActuatorPlugin(), kNumTruePlugins + kNumFakePlugins + 1);
|
EXPECT_EQ(RegisterActuatorPlugin(), kNumTruePlugins + kNumFakePlugins + 1);
|
||||||
EXPECT_EQ(RegisterPassivePlugin(), kNumTruePlugins + kNumFakePlugins + 2);
|
EXPECT_EQ(RegisterPassivePlugin(), kNumTruePlugins + kNumFakePlugins + 2);
|
||||||
EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
|
EXPECT_EQ(mjp_pluginCount(),
|
||||||
|
kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EnginePluginTest, SaveXml) {
|
TEST_F(EnginePluginTest, SaveXml) {
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
|
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
|
|
||||||
std::string saved_xml = SaveAndReadXml(m);
|
std::string saved_xml = SaveAndReadXml(m.get());
|
||||||
std::string_view expected_xml(xml);
|
std::string_view expected_xml(xml);
|
||||||
|
|
||||||
const std::string extension_open = "<extension>";
|
const std::string extension_open = "<extension>";
|
||||||
@@ -561,12 +559,9 @@ TEST_F(EnginePluginTest, SaveXml) {
|
|||||||
EXPECT_THAT(saved_xml, HasSubstr(expected_sensor_section));
|
EXPECT_THAT(saved_xml, HasSubstr(expected_sensor_section));
|
||||||
EXPECT_THAT(saved_xml, HasSubstr(expected_actuator_section));
|
EXPECT_THAT(saved_xml, HasSubstr(expected_actuator_section));
|
||||||
|
|
||||||
mj_deleteModel(m);
|
|
||||||
|
|
||||||
// make sure that the saved XML can still be compiled
|
// make sure that the saved XML can still be compiled
|
||||||
mjModel* m2 = LoadModelFromString(saved_xml, error, sizeof(error));
|
MjModelPtr m2 = LoadModelFromString(saved_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
mj_deleteModel(m2);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EnginePluginTest, SensorPlugin) {
|
TEST_F(EnginePluginTest, SensorPlugin) {
|
||||||
@@ -575,8 +570,8 @@ TEST_F(EnginePluginTest, SensorPlugin) {
|
|||||||
EXPECT_EQ(expected_init_count, expected_destroy_count);
|
EXPECT_EQ(expected_init_count, expected_destroy_count);
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
|
|
||||||
// mj_makeModel calls mj_makeData and mj_deleteData internally
|
// mj_makeModel calls mj_makeData and mj_deleteData internally
|
||||||
expected_init_count += 3;
|
expected_init_count += 3;
|
||||||
@@ -585,10 +580,10 @@ TEST_F(EnginePluginTest, SensorPlugin) {
|
|||||||
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
|
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
|
||||||
|
|
||||||
EXPECT_EQ(m->nplugin, 7);
|
EXPECT_EQ(m->nplugin, 7);
|
||||||
EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "twosensors"), 0);
|
EXPECT_EQ(mj_name2id(m.get(), mjOBJ_PLUGIN, "twosensors"), 0);
|
||||||
EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "threesensors"), 1);
|
EXPECT_EQ(mj_name2id(m.get(), mjOBJ_PLUGIN, "threesensors"), 1);
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
expected_init_count += 3;
|
expected_init_count += 3;
|
||||||
EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
|
EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
|
||||||
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
|
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
|
||||||
@@ -605,24 +600,22 @@ TEST_F(EnginePluginTest, SensorPlugin) {
|
|||||||
m->plugin_stateadr[5]),
|
m->plugin_stateadr[5]),
|
||||||
testing::ElementsAreArray<int>({5 * (i + 1), 10 * j, 5 * j}));
|
testing::ElementsAreArray<int>({5 * (i + 1), 10 * j, 5 * j}));
|
||||||
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[18]>(d->sensordata),
|
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[18]>(d->sensordata),
|
||||||
testing::ElementsAreArray<int>({ i+1, 2*j, j,
|
testing::ElementsAreArray<int>(
|
||||||
5*(i+1), 10*j, 5*j,
|
{i + 1, 2 * j, j, 5 * (i + 1), 10 * j, 5 * j, 3 * (i + 1),
|
||||||
3*(i+1), 6*j, 3*j,
|
6 * j, 3 * j, i + 1, 2 * j, j, 3 * (i + 1), 6 * j, 3 * j,
|
||||||
i+1, 2*j, j,
|
|
||||||
3*(i+1), 6*j, 3*j,
|
|
||||||
3 * (i + 1), 6 * j, 3 * j}));
|
3 * (i + 1), 6 * j, 3 * j}));
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
}
|
}
|
||||||
mj_resetData(m, d);
|
mj_resetData(m.get(), d.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
d.reset();
|
||||||
|
m.reset();
|
||||||
|
|
||||||
expected_destroy_count += 3;
|
expected_destroy_count += 3;
|
||||||
EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
|
EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
|
||||||
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
|
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
|
||||||
|
|
||||||
mj_deleteModel(m);
|
|
||||||
EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
|
EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
|
||||||
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
|
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
|
||||||
}
|
}
|
||||||
@@ -633,8 +626,8 @@ TEST_F(EnginePluginTest, ActuatorPlugin) {
|
|||||||
EXPECT_EQ(expected_init_count, expected_destroy_count);
|
EXPECT_EQ(expected_init_count, expected_destroy_count);
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
|
|
||||||
// mj_makeModel calls mj_makeData and mj_deleteData internally
|
// mj_makeModel calls mj_makeData and mj_deleteData internally
|
||||||
expected_init_count += 2;
|
expected_init_count += 2;
|
||||||
@@ -643,9 +636,9 @@ TEST_F(EnginePluginTest, ActuatorPlugin) {
|
|||||||
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
|
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
|
||||||
|
|
||||||
EXPECT_EQ(m->nplugin, 7);
|
EXPECT_EQ(m->nplugin, 7);
|
||||||
EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "actuator2"), 2);
|
EXPECT_EQ(mj_name2id(m.get(), mjOBJ_PLUGIN, "actuator2"), 2);
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
expected_init_count += 2;
|
expected_init_count += 2;
|
||||||
EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
|
EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
|
||||||
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
|
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
|
||||||
@@ -661,26 +654,26 @@ TEST_F(EnginePluginTest, ActuatorPlugin) {
|
|||||||
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[2]>(d->actuator_force),
|
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[2]>(d->actuator_force),
|
||||||
testing::ElementsAreArray<mjtNum>(
|
testing::ElementsAreArray<mjtNum>(
|
||||||
{(mjtNum)0.125 * j, (mjtNum)0.25 * j}));
|
{(mjtNum)0.125 * j, (mjtNum)0.25 * j}));
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
}
|
}
|
||||||
mj_resetData(m, d);
|
mj_resetData(m.get(), d.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
d.reset();
|
||||||
|
m.reset();
|
||||||
|
|
||||||
expected_destroy_count += 2;
|
expected_destroy_count += 2;
|
||||||
EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
|
EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
|
||||||
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
|
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
|
||||||
|
|
||||||
mj_deleteModel(m);
|
|
||||||
EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
|
EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
|
||||||
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
|
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(EnginePluginTest, FilteredActuatorPlugin) {
|
TEST_F(EnginePluginTest, FilteredActuatorPlugin) {
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
|
|
||||||
// Expecting 7 actuator state variables: 3x2 from actuator2 instances, and 1
|
// Expecting 7 actuator state variables: 3x2 from actuator2 instances, and 1
|
||||||
// from setting dyntype="filter" on one of the plugin actuators
|
// from setting dyntype="filter" on one of the plugin actuators
|
||||||
@@ -692,12 +685,12 @@ TEST_F(EnginePluginTest, FilteredActuatorPlugin) {
|
|||||||
EXPECT_EQ(m->actuator_actadr[1], 0);
|
EXPECT_EQ(m->actuator_actadr[1], 0);
|
||||||
EXPECT_EQ(m->actuator_actadr[2], 3);
|
EXPECT_EQ(m->actuator_actadr[2], 3);
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
EXPECT_EQ(d->act[0], 0.0);
|
EXPECT_EQ(d->act[0], 0.0);
|
||||||
mju_fill(d->ctrl, 1, m->nu);
|
mju_fill(d->ctrl, 1, m->nu);
|
||||||
// start with nonzero act for the filter
|
// start with nonzero act for the filter
|
||||||
d->act[6] = 0.5;
|
d->act[6] = 0.5;
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
|
|
||||||
for (int i = 0; i < 6; ++i) {
|
for (int i = 0; i < 6; ++i) {
|
||||||
// act_dot should be computed by the plugin
|
// act_dot should be computed by the plugin
|
||||||
@@ -715,9 +708,6 @@ TEST_F(EnginePluginTest, FilteredActuatorPlugin) {
|
|||||||
EXPECT_THAT(d->act_dot[6], MjNear(expected_act_dot, 1e-6, 1e-4));
|
EXPECT_THAT(d->act_dot[6], MjNear(expected_act_dot, 1e-6, 1e-4));
|
||||||
EXPECT_THAT(d->act[6],
|
EXPECT_THAT(d->act[6],
|
||||||
MjNear(0.5 + expected_act_dot * m->opt.timestep, 1e-6, 1e-4));
|
MjNear(0.5 + expected_act_dot * m->opt.timestep, 1e-6, 1e-4));
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
@@ -38,7 +38,6 @@ using ::testing::NotNull;
|
|||||||
|
|
||||||
using EnginePrintTest = MujocoTest;
|
using EnginePrintTest = MujocoTest;
|
||||||
|
|
||||||
|
|
||||||
constexpr const char* NullFile() {
|
constexpr const char* NullFile() {
|
||||||
#ifdef _WIN32
|
#ifdef _WIN32
|
||||||
return "NUL";
|
return "NUL";
|
||||||
@@ -60,27 +59,26 @@ TEST_F(EnginePrintTest, PrintDataWorksWithMsan) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
mjData* data = mj_makeData(model);
|
<< "Failed to load model: " << error.data();
|
||||||
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
|
|
||||||
// Mark qacc_smooth[0] as initialized.
|
// Mark qacc_smooth[0] as initialized.
|
||||||
data->qacc_smooth[0] = 0;
|
data->qacc_smooth[0] = 0;
|
||||||
|
|
||||||
// This will read "uninitialized" values from mjData, but shouldn't fail.
|
// This will read "uninitialized" values from mjData, but shouldn't fail.
|
||||||
mj_printData(model, data, NullFile());
|
mj_printData(model.get(), data.get(), NullFile());
|
||||||
|
|
||||||
// After mj_printData, poisoned status should be restored correctly, so
|
// After mj_printData, poisoned status should be restored correctly, so
|
||||||
// qacc_smooth[0] should be marked initialzed.
|
// qacc_smooth[0] should be marked initialzed.
|
||||||
EXPECT_EQ(data->qacc_smooth[0], 0);
|
EXPECT_EQ(data->qacc_smooth[0], 0);
|
||||||
|
|
||||||
#ifdef MEMORY_SANITIZER
|
#ifdef MEMORY_SANITIZER
|
||||||
EXPECT_THAT(__msan_test_shadow(data->buffer, data->nbuffer), Not(Eq(-1))) <<
|
EXPECT_THAT(__msan_test_shadow(data->buffer, data->nbuffer), Not(Eq(-1)))
|
||||||
"Expecting some of data->buffer to be marked uninitialized";
|
<< "Expecting some of data->buffer to be marked uninitialized";
|
||||||
#endif
|
#endif
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
+95
-105
@@ -14,6 +14,8 @@
|
|||||||
|
|
||||||
// Tests for ray casting.
|
// Tests for ray casting.
|
||||||
|
|
||||||
|
#include "src/engine/engine_ray.h"
|
||||||
|
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
@@ -23,7 +25,6 @@
|
|||||||
#include <mujoco/mjmodel.h>
|
#include <mujoco/mjmodel.h>
|
||||||
#include <mujoco/mjtype.h>
|
#include <mujoco/mjtype.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
#include "src/engine/engine_ray.h"
|
|
||||||
#include "test/fixture.h"
|
#include "test/fixture.h"
|
||||||
|
|
||||||
namespace mujoco {
|
namespace mujoco {
|
||||||
@@ -85,9 +86,10 @@ using RayTest = MujocoTest;
|
|||||||
|
|
||||||
TEST_F(RayTest, NoExclusions) {
|
TEST_F(RayTest, NoExclusions) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error));
|
MjModelPtr model =
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
LoadModelFromString(kRayCastingModel, error, sizeof(error));
|
||||||
mjData* data = mj_makeData(model);
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
|
|
||||||
mjtNum pnt[] = {0.0, 0.0, 0.0};
|
mjtNum pnt[] = {0.0, 0.0, 0.0};
|
||||||
@@ -97,20 +99,19 @@ TEST_F(RayTest, NoExclusions) {
|
|||||||
int bodyexclude = -1;
|
int bodyexclude = -1;
|
||||||
int geomid = -1;
|
int geomid = -1;
|
||||||
|
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
|
mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup,
|
||||||
bodyexclude, &geomid, nullptr);
|
flg_static, bodyexclude, &geomid, nullptr);
|
||||||
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
|
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "static_group1");
|
||||||
EXPECT_MJTNUM_EQ(distance, 0.9);
|
EXPECT_MJTNUM_EQ(distance, 0.9);
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(RayTest, Exclusions) {
|
TEST_F(RayTest, Exclusions) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error));
|
MjModelPtr model =
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
LoadModelFromString(kRayCastingModel, error, sizeof(error));
|
||||||
mjData* data = mj_makeData(model);
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
|
|
||||||
mjtNum pnt[] = {0.0, 0.0, 0.0};
|
mjtNum pnt[] = {0.0, 0.0, 0.0};
|
||||||
@@ -120,40 +121,38 @@ TEST_F(RayTest, Exclusions) {
|
|||||||
int bodyexclude = -1;
|
int bodyexclude = -1;
|
||||||
int geomid = -1;
|
int geomid = -1;
|
||||||
|
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
|
mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup,
|
||||||
bodyexclude, &geomid, nullptr);
|
flg_static, bodyexclude, &geomid, nullptr);
|
||||||
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
|
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "static_group1");
|
||||||
EXPECT_NEAR(distance, 0.9, MjTol(1e-12, 1e-5));
|
EXPECT_NEAR(distance, 0.9, MjTol(1e-12, 1e-5));
|
||||||
|
|
||||||
// Exclude nearest geom
|
// Exclude nearest geom
|
||||||
geomgroup[1] = 0;
|
geomgroup[1] = 0;
|
||||||
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
|
distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static,
|
||||||
&geomid, nullptr);
|
bodyexclude, &geomid, nullptr);
|
||||||
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
|
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group0");
|
||||||
EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5));
|
EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5));
|
||||||
|
|
||||||
geomgroup[0] = 0;
|
geomgroup[0] = 0;
|
||||||
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
|
distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static,
|
||||||
&geomid, nullptr);
|
bodyexclude, &geomid, nullptr);
|
||||||
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group2");
|
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group2");
|
||||||
EXPECT_NEAR(distance, 4.9, MjTol(1e-12, 1e-5));
|
EXPECT_NEAR(distance, 4.9, MjTol(1e-12, 1e-5));
|
||||||
|
|
||||||
geomgroup[2] = 0;
|
geomgroup[2] = 0;
|
||||||
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
|
distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static,
|
||||||
&geomid, nullptr);
|
bodyexclude, &geomid, nullptr);
|
||||||
EXPECT_EQ(geomid, -1);
|
EXPECT_EQ(geomid, -1);
|
||||||
EXPECT_NEAR(distance, -1, MjTol(1e-12, 1e-5));
|
EXPECT_NEAR(distance, -1, MjTol(1e-12, 1e-5));
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(RayTest, ExcludeStatic) {
|
TEST_F(RayTest, ExcludeStatic) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error));
|
MjModelPtr model =
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
LoadModelFromString(kRayCastingModel, error, sizeof(error));
|
||||||
mjData* data = mj_makeData(model);
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
MjDataPtr data = MakeData(model);
|
||||||
ASSERT_THAT(data, NotNull());
|
ASSERT_THAT(data, NotNull());
|
||||||
|
|
||||||
mjtNum pnt[] = {0.0, 0.0, 0.0};
|
mjtNum pnt[] = {0.0, 0.0, 0.0};
|
||||||
@@ -163,24 +162,22 @@ TEST_F(RayTest, ExcludeStatic) {
|
|||||||
int bodyexclude = -1;
|
int bodyexclude = -1;
|
||||||
int geomid = -1;
|
int geomid = -1;
|
||||||
|
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
|
mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup,
|
||||||
bodyexclude, &geomid, nullptr);
|
flg_static, bodyexclude, &geomid, nullptr);
|
||||||
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
|
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group0");
|
||||||
EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5));
|
EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5));
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ------------------------------- mj_multiRay --------------------------------
|
// ------------------------------- mj_multiRay --------------------------------
|
||||||
|
|
||||||
TEST_F(RayTest, MultiRayEqualsSingleRay) {
|
TEST_F(RayTest, MultiRayEqualsSingleRay) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
ASSERT_THAT(d, NotNull());
|
ASSERT_THAT(d, NotNull());
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// create ray array
|
// create ray array
|
||||||
constexpr int N = 80;
|
constexpr int N = 80;
|
||||||
@@ -204,8 +201,8 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) {
|
|||||||
// compute intersections with multiray functions
|
// compute intersections with multiray functions
|
||||||
mjtNum dist_multiray[N * M];
|
mjtNum dist_multiray[N * M];
|
||||||
int rgeomid_multiray[N * M];
|
int rgeomid_multiray[N * M];
|
||||||
mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, dist_multiray,
|
mj_multiRay(m.get(), d.get(), pnt, vec, NULL, 1, -1, rgeomid_multiray,
|
||||||
nullptr, N * M, mjMAXVAL);
|
dist_multiray, nullptr, N * M, mjMAXVAL);
|
||||||
|
|
||||||
// compare results with single ray function
|
// compare results with single ray function
|
||||||
mjtNum dist;
|
mjtNum dist;
|
||||||
@@ -214,25 +211,23 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) {
|
|||||||
for (int i = 0; i < N; ++i) {
|
for (int i = 0; i < N; ++i) {
|
||||||
for (int j = 0; j < M; ++j) {
|
for (int j = 0; j < M; ++j) {
|
||||||
int idx = i * M + j;
|
int idx = i * M + j;
|
||||||
dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid, nullptr);
|
dist = mj_ray(m.get(), d.get(), pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid,
|
||||||
|
nullptr);
|
||||||
EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]);
|
EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]);
|
||||||
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
|
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
|
||||||
nhits += dist >= 0;
|
nhits += dist >= 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
EXPECT_GT(nhits, 10);
|
EXPECT_GT(nhits, 10);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
|
TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
ASSERT_THAT(d, NotNull());
|
ASSERT_THAT(d, NotNull());
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// create ray array
|
// create ray array
|
||||||
constexpr int N = 80;
|
constexpr int N = 80;
|
||||||
@@ -257,7 +252,7 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
|
|||||||
mjtNum dist_multiray[N * M];
|
mjtNum dist_multiray[N * M];
|
||||||
int rgeomid_multiray[N * M];
|
int rgeomid_multiray[N * M];
|
||||||
mjtNum normal_multiray[3 * N * M];
|
mjtNum normal_multiray[3 * N * M];
|
||||||
mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray,
|
mj_multiRay(m.get(), d.get(), pnt, vec, NULL, 1, -1, rgeomid_multiray,
|
||||||
dist_multiray, normal_multiray, N * M, mjMAXVAL);
|
dist_multiray, normal_multiray, N * M, mjMAXVAL);
|
||||||
|
|
||||||
// compare results with single ray normal function
|
// compare results with single ray normal function
|
||||||
@@ -268,7 +263,7 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
|
|||||||
for (int i = 0; i < N; ++i) {
|
for (int i = 0; i < N; ++i) {
|
||||||
for (int j = 0; j < M; ++j) {
|
for (int j = 0; j < M; ++j) {
|
||||||
int idx = i * M + j;
|
int idx = i * M + j;
|
||||||
dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid,
|
dist = mj_ray(m.get(), d.get(), pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid,
|
||||||
normal);
|
normal);
|
||||||
EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]);
|
EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]);
|
||||||
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
|
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
|
||||||
@@ -279,19 +274,16 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
EXPECT_GT(nhits, 10);
|
EXPECT_GT(nhits, 10);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(RayTest, EdgeCases) {
|
TEST_F(RayTest, EdgeCases) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(kSingleGeomModel, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kSingleGeomModel, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
ASSERT_THAT(m->nbvh, 1);
|
ASSERT_THAT(m->nbvh, 1);
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
ASSERT_THAT(d, NotNull());
|
ASSERT_THAT(d, NotNull());
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
|
|
||||||
// spherical bounding box and result arrays
|
// spherical bounding box and result arrays
|
||||||
mjtNum geom_ba[4];
|
mjtNum geom_ba[4];
|
||||||
@@ -301,51 +293,57 @@ TEST_F(RayTest, EdgeCases) {
|
|||||||
|
|
||||||
// pnt contained in bounding box
|
// pnt contained in bounding box
|
||||||
mjtNum pnt1[] = {-1, 0, 0};
|
mjtNum pnt1[] = {-1, 0, 0};
|
||||||
mju_multiRayPrepare(m, d, pnt1, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
|
mju_multiRayPrepare(m.get(), d.get(), pnt1, NULL, NULL, 1, -1, mjMAXVAL,
|
||||||
|
geom_ba, flags);
|
||||||
EXPECT_MJTNUM_EQ(geom_ba[0], -mjPI);
|
EXPECT_MJTNUM_EQ(geom_ba[0], -mjPI);
|
||||||
EXPECT_MJTNUM_EQ(geom_ba[1], 0);
|
EXPECT_MJTNUM_EQ(geom_ba[1], 0);
|
||||||
EXPECT_MJTNUM_EQ(geom_ba[2], mjPI);
|
EXPECT_MJTNUM_EQ(geom_ba[2], mjPI);
|
||||||
EXPECT_MJTNUM_EQ(geom_ba[3], mjPI);
|
EXPECT_MJTNUM_EQ(geom_ba[3], mjPI);
|
||||||
mjtNum vec1[] = {1, 0, 0};
|
mjtNum vec1[] = {1, 0, 0};
|
||||||
mj_multiRay(m, d, pnt1, vec1, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
|
mj_multiRay(m.get(), d.get(), pnt1, vec1, NULL, 1, -1, &rgeomid, &dist,
|
||||||
mjMAXVAL);
|
nullptr, 1, mjMAXVAL);
|
||||||
EXPECT_FLOAT_EQ(dist, 0.1);
|
EXPECT_FLOAT_EQ(dist, 0.1);
|
||||||
|
|
||||||
// pnt at phi = Pi, -Pi
|
// pnt at phi = Pi, -Pi
|
||||||
mjtNum pnt2[] = {-.5, 0, 0};
|
mjtNum pnt2[] = {-.5, 0, 0};
|
||||||
mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
|
mju_multiRayPrepare(m.get(), d.get(), pnt2, NULL, NULL, 1, -1, mjMAXVAL,
|
||||||
|
geom_ba, flags);
|
||||||
EXPECT_FLOAT_EQ(geom_ba[0], -mjPI); // atan(y<0, x<0)
|
EXPECT_FLOAT_EQ(geom_ba[0], -mjPI); // atan(y<0, x<0)
|
||||||
EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0)
|
EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0)
|
||||||
mjtNum vec2[] = {-1, 0, 0};
|
mjtNum vec2[] = {-1, 0, 0};
|
||||||
mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
|
mj_multiRay(m.get(), d.get(), pnt2, vec2, NULL, 1, -1, &rgeomid, &dist,
|
||||||
mjMAXVAL);
|
nullptr, 1, mjMAXVAL);
|
||||||
EXPECT_FLOAT_EQ(dist, 0.4);
|
EXPECT_FLOAT_EQ(dist, 0.4);
|
||||||
|
|
||||||
// with cutoff
|
// with cutoff
|
||||||
mjtNum cutoff1 = 0.41, cutoff2 = 0.39;
|
mjtNum cutoff1 = 0.41, cutoff2 = 0.39;
|
||||||
mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, cutoff1, geom_ba, flags);
|
mju_multiRayPrepare(m.get(), d.get(), pnt2, NULL, NULL, 1, -1, cutoff1,
|
||||||
|
geom_ba, flags);
|
||||||
EXPECT_EQ(flags[0], 0);
|
EXPECT_EQ(flags[0], 0);
|
||||||
mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, cutoff2, geom_ba, flags);
|
mju_multiRayPrepare(m.get(), d.get(), pnt2, NULL, NULL, 1, -1, cutoff2,
|
||||||
|
geom_ba, flags);
|
||||||
EXPECT_EQ(flags[0], 1);
|
EXPECT_EQ(flags[0], 1);
|
||||||
mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
|
mj_multiRay(m.get(), d.get(), pnt2, vec2, NULL, 1, -1, &rgeomid, &dist,
|
||||||
cutoff2);
|
nullptr, 1, cutoff2);
|
||||||
EXPECT_FLOAT_EQ(dist, -1);
|
EXPECT_FLOAT_EQ(dist, -1);
|
||||||
|
|
||||||
// pnt on the boundary of the box
|
// pnt on the boundary of the box
|
||||||
mjtNum pnt3[] = {.1, .1, .05};
|
mjtNum pnt3[] = {.1, .1, .05};
|
||||||
mju_multiRayPrepare(m, d, pnt3, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
|
mju_multiRayPrepare(m.get(), d.get(), pnt3, NULL, NULL, 1, -1, mjMAXVAL,
|
||||||
|
geom_ba, flags);
|
||||||
EXPECT_FLOAT_EQ(geom_ba[1], 0);
|
EXPECT_FLOAT_EQ(geom_ba[1], 0);
|
||||||
EXPECT_FLOAT_EQ(geom_ba[3], mjPI);
|
EXPECT_FLOAT_EQ(geom_ba[3], mjPI);
|
||||||
mjtNum vec3[] = {1, 1, 0};
|
mjtNum vec3[] = {1, 1, 0};
|
||||||
mj_multiRay(m, d, pnt3, vec3, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
|
mj_multiRay(m.get(), d.get(), pnt3, vec3, NULL, 1, -1, &rgeomid, &dist,
|
||||||
mjMAXVAL);
|
nullptr, 1, mjMAXVAL);
|
||||||
EXPECT_FLOAT_EQ(dist, -1);
|
EXPECT_FLOAT_EQ(dist, -1);
|
||||||
|
|
||||||
// size 0 geom
|
// size 0 geom
|
||||||
mjtNum pnt4[] = {-2, 0, 0};
|
mjtNum pnt4[] = {-2, 0, 0};
|
||||||
m->geom_aabb[0] = m->geom_aabb[1] = m->geom_aabb[2] = 0;
|
m->geom_aabb[0] = m->geom_aabb[1] = m->geom_aabb[2] = 0;
|
||||||
m->geom_aabb[3] = m->geom_aabb[4] = m->geom_aabb[5] = 0;
|
m->geom_aabb[3] = m->geom_aabb[4] = m->geom_aabb[5] = 0;
|
||||||
mju_multiRayPrepare(m, d, pnt4, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
|
mju_multiRayPrepare(m.get(), d.get(), pnt4, NULL, NULL, 1, -1, mjMAXVAL,
|
||||||
|
geom_ba, flags);
|
||||||
// margin = atan(max_half / dist) where max_half = max(aabb[3..5])
|
// margin = atan(max_half / dist) where max_half = max(aabb[3..5])
|
||||||
// For a zero-size AABB: max_half = 0, so margin = 0
|
// For a zero-size AABB: max_half = 0, so margin = 0
|
||||||
mjtNum dist4 = mju_dist3(pnt4, d->geom_xpos);
|
mjtNum dist4 = mju_dist3(pnt4, d->geom_xpos);
|
||||||
@@ -357,12 +355,9 @@ TEST_F(RayTest, EdgeCases) {
|
|||||||
EXPECT_NEAR(geom_ba[2], 0 + margin4, 1e-6);
|
EXPECT_NEAR(geom_ba[2], 0 + margin4, 1e-6);
|
||||||
EXPECT_NEAR(geom_ba[3], mjPI / 2 + margin4, 1e-6);
|
EXPECT_NEAR(geom_ba[3], mjPI / 2 + margin4, 1e-6);
|
||||||
mjtNum vec4[] = {1, 0, 0};
|
mjtNum vec4[] = {1, 0, 0};
|
||||||
mj_multiRay(m, d, pnt4, vec4, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
|
mj_multiRay(m.get(), d.get(), pnt4, vec4, NULL, 1, -1, &rgeomid, &dist,
|
||||||
mjMAXVAL);
|
nullptr, 1, mjMAXVAL);
|
||||||
EXPECT_FLOAT_EQ(dist, 0.9);
|
EXPECT_FLOAT_EQ(dist, 0.9);
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ------------------------------- mj_rayMesh ---------------------------------
|
// ------------------------------- mj_rayMesh ---------------------------------
|
||||||
@@ -403,13 +398,15 @@ mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid,
|
|||||||
// process all triangles
|
// process all triangles
|
||||||
int face, meshid = m->geom_dataid[geomid];
|
int face, meshid = m->geom_dataid[geomid];
|
||||||
for (face = m->mesh_faceadr[meshid];
|
for (face = m->mesh_faceadr[meshid];
|
||||||
face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid];
|
face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid]; face++) {
|
||||||
face++) {
|
|
||||||
// get float vertices
|
// get float vertices
|
||||||
float* vf[3];
|
float* vf[3];
|
||||||
vf[0] = m->mesh_vert + 3*(m->mesh_face[3*face] + m->mesh_vertadr[meshid]);
|
vf[0] =
|
||||||
vf[1] = m->mesh_vert + 3*(m->mesh_face[3*face+1] + m->mesh_vertadr[meshid]);
|
m->mesh_vert + 3 * (m->mesh_face[3 * face] + m->mesh_vertadr[meshid]);
|
||||||
vf[2] = m->mesh_vert + 3*(m->mesh_face[3*face+2] + m->mesh_vertadr[meshid]);
|
vf[1] = m->mesh_vert +
|
||||||
|
3 * (m->mesh_face[3 * face + 1] + m->mesh_vertadr[meshid]);
|
||||||
|
vf[2] = m->mesh_vert +
|
||||||
|
3 * (m->mesh_face[3 * face + 2] + m->mesh_vertadr[meshid]);
|
||||||
|
|
||||||
// convert to mjtNum
|
// convert to mjtNum
|
||||||
mjtNum v[3][3];
|
mjtNum v[3][3];
|
||||||
@@ -466,7 +463,6 @@ void _rayMeshTest(const mjModel* m) {
|
|||||||
EXPECT_FLOAT_EQ(dist_new, dist_old);
|
EXPECT_FLOAT_EQ(dist_new, dist_old);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -478,15 +474,13 @@ TEST_F(RayTest, RayMeshPruning) {
|
|||||||
const string xml_path =
|
const string xml_path =
|
||||||
GetTestDataFilePath("engine/testdata/ray/stanford_bunny.xml");
|
GetTestDataFilePath("engine/testdata/ray/stanford_bunny.xml");
|
||||||
|
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), NULL, error, sizeof(error));
|
MjModelPtr m(mj_loadXML(xml_path.c_str(), NULL, error, sizeof(error)));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
_rayMeshTest(m);
|
_rayMeshTest(m.get());
|
||||||
mj_deleteModel(m);
|
|
||||||
|
|
||||||
m = LoadModelFromString(kCubeletModel, error, sizeof(error));
|
m = LoadModelFromString(kCubeletModel, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
_rayMeshTest(m);
|
_rayMeshTest(m.get());
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(RayTest, RayHfield) {
|
TEST_F(RayTest, RayHfield) {
|
||||||
@@ -523,21 +517,17 @@ TEST_F(RayTest, RayHfield) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
|
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
EXPECT_THAT(data->sensordata[0], MjNear(1, 1e-8, 1e-5));
|
EXPECT_THAT(data->sensordata[0], MjNear(1, 1e-8, 1e-5));
|
||||||
EXPECT_THAT(data->sensordata[1], MjNear(1, 1e-8, 1e-5));
|
EXPECT_THAT(data->sensordata[1], MjNear(1, 1e-8, 1e-5));
|
||||||
EXPECT_THAT(data->sensordata[2], MjNear(1, 1e-8, 1e-5));
|
EXPECT_THAT(data->sensordata[2], MjNear(1, 1e-8, 1e-5));
|
||||||
EXPECT_THAT(data->sensordata[3], MjNear(0.5, 1e-8, 1e-5));
|
EXPECT_THAT(data->sensordata[3], MjNear(0.5, 1e-8, 1e-5));
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static const char* const kPlaneModel = "engine/testdata/ray/plane.xml";
|
static const char* const kPlaneModel = "engine/testdata/ray/plane.xml";
|
||||||
@@ -603,8 +593,8 @@ TEST_F(RayTest, RayNormal) {
|
|||||||
} else {
|
} else {
|
||||||
r = mj_rayFlex(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1,
|
r = mj_rayFlex(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1,
|
||||||
/*flg_edge*/ 1, /*flg_face*/ 1,
|
/*flg_edge*/ 1, /*flg_face*/ 1,
|
||||||
/*flg_skin*/ 1, /*flex_id*/ 0,
|
/*flg_skin*/ 1, /*flex_id*/ 0, pnt, vec, nullptr,
|
||||||
pnt, vec, nullptr, normal);
|
normal);
|
||||||
// no sensor comparison: rangefinders only intersect with geoms
|
// no sensor comparison: rangefinders only intersect with geoms
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -623,8 +613,7 @@ TEST_F(RayTest, RayNormal) {
|
|||||||
mjtNum eps = 1e-6;
|
mjtNum eps = 1e-6;
|
||||||
mjtNum ds[2][3];
|
mjtNum ds[2][3];
|
||||||
for (int i = 0; i < 2; ++i) {
|
for (int i = 0; i < 2; ++i) {
|
||||||
mjtNum nudge[3] = {d->site_xmat[0 + i],
|
mjtNum nudge[3] = {d->site_xmat[0 + i], d->site_xmat[3 + i],
|
||||||
d->site_xmat[3 + i],
|
|
||||||
d->site_xmat[6 + i]};
|
d->site_xmat[6 + i]};
|
||||||
mjtNum dr, dpnt[3];
|
mjtNum dr, dpnt[3];
|
||||||
mju_addScl3(dpnt, pnt, nudge, eps);
|
mju_addScl3(dpnt, pnt, nudge, eps);
|
||||||
@@ -650,7 +639,8 @@ TEST_F(RayTest, RayNormal) {
|
|||||||
mjtNum expected_neg[3] = {-expected[0], -expected[1], -expected[2]};
|
mjtNum expected_neg[3] = {-expected[0], -expected[1], -expected[2]};
|
||||||
|
|
||||||
// compare analytic with fin-diff approximation
|
// compare analytic with fin-diff approximation
|
||||||
EXPECT_THAT(normal, AnyOf(Pointwise(MjNear(100*eps, 1e-3), expected),
|
EXPECT_THAT(normal,
|
||||||
|
AnyOf(Pointwise(MjNear(100 * eps, 1e-3), expected),
|
||||||
Pointwise(MjNear(100 * eps, 1e-3), expected_neg)))
|
Pointwise(MjNear(100 * eps, 1e-3), expected_neg)))
|
||||||
<< path << ", time " << d->time;
|
<< path << ", time " << d->time;
|
||||||
|
|
||||||
|
|||||||
+183
-276
File diff suppressed because it is too large
Load Diff
@@ -53,7 +53,7 @@ TEST_F(SetConstTest, AwakeActuatedJoint) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m;
|
MjModelPtr m;
|
||||||
|
|
||||||
string sleep[] = {"auto", "never", "allowed", "init"};
|
string sleep[] = {"auto", "never", "allowed", "init"};
|
||||||
int tsp0[] = {mjSLEEP_AUTO_NEVER, mjSLEEP_NEVER, mjSLEEP_ALLOWED,
|
int tsp0[] = {mjSLEEP_AUTO_NEVER, mjSLEEP_NEVER, mjSLEEP_ALLOWED,
|
||||||
@@ -69,10 +69,9 @@ TEST_F(SetConstTest, AwakeActuatedJoint) {
|
|||||||
size_t pos2 = xml_copy.find("POLICY2");
|
size_t pos2 = xml_copy.find("POLICY2");
|
||||||
xml_copy.replace(pos2, 7, sleep[j]);
|
xml_copy.replace(pos2, 7, sleep[j]);
|
||||||
m = LoadModelFromString(xml_copy.c_str(), error, sizeof(error));
|
m = LoadModelFromString(xml_copy.c_str(), error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
EXPECT_EQ(m->tree_sleep_policy[0], tsp0[i]);
|
EXPECT_EQ(m->tree_sleep_policy[0], tsp0[i]);
|
||||||
EXPECT_EQ(m->tree_sleep_policy[1], tsp1[j]);
|
EXPECT_EQ(m->tree_sleep_policy[1], tsp1[j]);
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -97,13 +96,11 @@ TEST_F(SetConstTest, AwakeActuatedSite) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
|
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
|
||||||
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
|
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SetConstTest, AwakeActuatedBody) {
|
TEST_F(SetConstTest, AwakeActuatedBody) {
|
||||||
@@ -125,13 +122,11 @@ TEST_F(SetConstTest, AwakeActuatedBody) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
|
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
|
||||||
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
|
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SetConstTest, AwakeActuatedTendon) {
|
TEST_F(SetConstTest, AwakeActuatedTendon) {
|
||||||
@@ -161,13 +156,11 @@ TEST_F(SetConstTest, AwakeActuatedTendon) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
|
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
|
||||||
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
|
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SetConstTest, AwakeStiffTendonMultiTree) {
|
TEST_F(SetConstTest, AwakeStiffTendonMultiTree) {
|
||||||
@@ -194,13 +187,11 @@ TEST_F(SetConstTest, AwakeStiffTendonMultiTree) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
|
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
|
||||||
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_NEVER);
|
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_NEVER);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SetConstTest, SleepyTendonSingleTree) {
|
TEST_F(SetConstTest, SleepyTendonSingleTree) {
|
||||||
@@ -227,12 +218,10 @@ TEST_F(SetConstTest, SleepyTendonSingleTree) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED);
|
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SetConstTest, SleepyTendonZeroStiffness) {
|
TEST_F(SetConstTest, SleepyTendonZeroStiffness) {
|
||||||
@@ -259,13 +248,11 @@ TEST_F(SetConstTest, SleepyTendonZeroStiffness) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED);
|
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED);
|
||||||
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
|
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SetConstTest, TendonTreeId) {
|
TEST_F(SetConstTest, TendonTreeId) {
|
||||||
@@ -336,16 +323,16 @@ TEST_F(SetConstTest, TendonTreeId) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
int t_static_id = mj_name2id(model, mjOBJ_TENDON, "T_static");
|
int t_static_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_static");
|
||||||
int t_tree1_id = mj_name2id(model, mjOBJ_TENDON, "T_tree1");
|
int t_tree1_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_tree1");
|
||||||
int t_intertree12_id = mj_name2id(model, mjOBJ_TENDON, "T_intertree12");
|
int t_intertree12_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_intertree12");
|
||||||
int t_intertree123_id = mj_name2id(model, mjOBJ_TENDON, "T_intertree123");
|
int t_intertree123_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_intertree123");
|
||||||
|
|
||||||
int b1_1_treeid = model->body_treeid[mj_name2id(model, mjOBJ_BODY, "B1_1")];
|
int b1_1_treeid = model->body_treeid[mj_name2id(model.get(), mjOBJ_BODY, "B1_1")];
|
||||||
int b2_1_treeid = model->body_treeid[mj_name2id(model, mjOBJ_BODY, "B2_1")];
|
int b2_1_treeid = model->body_treeid[mj_name2id(model.get(), mjOBJ_BODY, "B2_1")];
|
||||||
|
|
||||||
// Tendon 1: Not associated with any tree
|
// Tendon 1: Not associated with any tree
|
||||||
EXPECT_EQ(model->tendon_treenum[t_static_id], 0);
|
EXPECT_EQ(model->tendon_treenum[t_static_id], 0);
|
||||||
@@ -368,8 +355,6 @@ TEST_F(SetConstTest, TendonTreeId) {
|
|||||||
EXPECT_EQ(model->tendon_treeid[2*t_intertree123_id], b1_1_treeid);
|
EXPECT_EQ(model->tendon_treeid[2*t_intertree123_id], b1_1_treeid);
|
||||||
EXPECT_EQ(model->tendon_treeid[2*t_intertree123_id+1], b2_1_treeid);
|
EXPECT_EQ(model->tendon_treeid[2*t_intertree123_id+1], b2_1_treeid);
|
||||||
// The third tree ID is not stored in tendon_treeid
|
// The third tree ID is not stored in tendon_treeid
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SetConstTest, SleepingNotAllowed) {
|
TEST_F(SetConstTest, SleepingNotAllowed) {
|
||||||
@@ -415,8 +400,8 @@ TEST_F(SetConstTest, SleepingNotAllowed) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
EXPECT_THAT(model, IsNull()) << error;
|
EXPECT_THAT(model.get(), IsNull()) << error;
|
||||||
EXPECT_THAT(string(error), HasSubstr(
|
EXPECT_THAT(string(error), HasSubstr(
|
||||||
"tree 1 connected to tendon 0 which spans more than 2 trees, "
|
"tree 1 connected to tendon 0 which spans more than 2 trees, "
|
||||||
"sleeping not allowed"));
|
"sleeping not allowed"));
|
||||||
@@ -447,8 +432,8 @@ TEST_F(SetConstTest, DofLength) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
mjtNum tol = 1e-5;
|
mjtNum tol = 1e-5;
|
||||||
|
|
||||||
@@ -470,8 +455,6 @@ TEST_F(SetConstTest, DofLength) {
|
|||||||
EXPECT_NEAR(model->dof_length[8], 5, tol);
|
EXPECT_NEAR(model->dof_length[8], 5, tol);
|
||||||
EXPECT_NEAR(model->dof_length[9], 5, tol);
|
EXPECT_NEAR(model->dof_length[9], 5, tol);
|
||||||
EXPECT_NEAR(model->dof_length[10], 5, tol);
|
EXPECT_NEAR(model->dof_length[10], 5, tol);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
@@ -14,26 +14,27 @@
|
|||||||
|
|
||||||
// Tests for engine/engine_sleep.c.
|
// Tests for engine/engine_sleep.c.
|
||||||
|
|
||||||
|
#include "src/engine/engine_sleep.h"
|
||||||
|
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
#include <gmock/gmock.h>
|
#include <gmock/gmock.h>
|
||||||
#include <gtest/gtest.h>
|
|
||||||
#include <gtest/gtest-spi.h>
|
#include <gtest/gtest-spi.h>
|
||||||
|
#include <gtest/gtest.h>
|
||||||
#include <mujoco/mjmodel.h>
|
#include <mujoco/mjmodel.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
#include "src/engine/engine_sleep.h"
|
|
||||||
#include "test/fixture.h"
|
#include "test/fixture.h"
|
||||||
|
|
||||||
namespace mujoco {
|
namespace mujoco {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
using ::testing::ElementsAre;
|
|
||||||
using ::testing::IsNull;
|
|
||||||
using ::testing::HasSubstr;
|
|
||||||
using ::testing::NotNull;
|
|
||||||
using ::std::string;
|
using ::std::string;
|
||||||
using ::std::vector;
|
using ::std::vector;
|
||||||
|
using ::testing::ElementsAre;
|
||||||
|
using ::testing::HasSubstr;
|
||||||
|
using ::testing::IsNull;
|
||||||
|
using ::testing::NotNull;
|
||||||
|
|
||||||
using SleepTest = MujocoTest;
|
using SleepTest = MujocoTest;
|
||||||
|
|
||||||
@@ -76,9 +77,9 @@ static constexpr int kAwake = -(1+mjMINAWAKE);
|
|||||||
|
|
||||||
TEST_F(SleepTest, MjSleepUpdate) {
|
TEST_F(SleepTest, MjSleepUpdate) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(kSimple, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kSimple, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
// ntree = 2, nbody = 5, nv = 5, njnt = 3, ngeom = 6
|
// ntree = 2, nbody = 5, nv = 5, njnt = 3, ngeom = 6
|
||||||
// body 0: world, 1 geom
|
// body 0: world, 1 geom
|
||||||
@@ -93,96 +94,69 @@ TEST_F(SleepTest, MjSleepUpdate) {
|
|||||||
EXPECT_EQ(m->njnt, 3);
|
EXPECT_EQ(m->njnt, 3);
|
||||||
EXPECT_EQ(m->ngeom, 6);
|
EXPECT_EQ(m->ngeom, 6);
|
||||||
|
|
||||||
EXPECT_THAT(AsVector(m->body_treeid, m->nbody),
|
EXPECT_THAT(AsVector(m->body_treeid, m->nbody), ElementsAre(-1, 0, -1, 1, 1));
|
||||||
ElementsAre(-1, 0, -1, 1, 1));
|
EXPECT_THAT(AsVector(m->dof_bodyid, m->nv), ElementsAre(1, 1, 1, 3, 4));
|
||||||
EXPECT_THAT(AsVector(m->dof_bodyid, m->nv),
|
|
||||||
ElementsAre(1, 1, 1, 3, 4));
|
|
||||||
EXPECT_THAT(AsVector(m->geom_bodyid, m->ngeom),
|
EXPECT_THAT(AsVector(m->geom_bodyid, m->ngeom),
|
||||||
ElementsAre(0, 1, 2, 3, 3, 4));
|
ElementsAre(0, 1, 2, 3, 3, 4));
|
||||||
EXPECT_THAT(AsVector(m->jnt_bodyid, m->njnt),
|
EXPECT_THAT(AsVector(m->jnt_bodyid, m->njnt), ElementsAre(1, 3, 4));
|
||||||
ElementsAre(1, 3, 4));
|
|
||||||
|
|
||||||
// Test Case 1: Initial state
|
// Test Case 1: Initial state
|
||||||
EXPECT_THAT(AsVector(d->tree_asleep, m->ntree),
|
EXPECT_THAT(AsVector(d->tree_asleep, m->ntree), ElementsAre(kAwake, kAwake));
|
||||||
ElementsAre(kAwake, kAwake));
|
|
||||||
EXPECT_EQ(d->ntree_awake, 2);
|
EXPECT_EQ(d->ntree_awake, 2);
|
||||||
EXPECT_EQ(d->nv_awake, 5);
|
EXPECT_EQ(d->nv_awake, 5);
|
||||||
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
|
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
|
||||||
ElementsAre(0, 1, 2, 3, 4));
|
ElementsAre(0, 1, 2, 3, 4));
|
||||||
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
|
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(1, 1));
|
||||||
ElementsAre(1, 1));
|
EXPECT_THAT(
|
||||||
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
|
AsVector(d->body_awake, m->nbody),
|
||||||
ElementsAre(mjS_STATIC,
|
ElementsAre(mjS_STATIC, mjS_AWAKE, mjS_STATIC, mjS_AWAKE, mjS_AWAKE));
|
||||||
mjS_AWAKE,
|
|
||||||
mjS_STATIC,
|
|
||||||
mjS_AWAKE,
|
|
||||||
mjS_AWAKE));
|
|
||||||
|
|
||||||
// Test Case 2: Call mj_sleepUpdate, expect no changes
|
// Test Case 2: Call mj_sleepUpdate, expect no changes
|
||||||
mj_updateSleep(m, d);
|
mj_updateSleep(m.get(), d.get());
|
||||||
EXPECT_EQ(d->ntree_awake, 2);
|
EXPECT_EQ(d->ntree_awake, 2);
|
||||||
EXPECT_EQ(d->nv_awake, 5);
|
EXPECT_EQ(d->nv_awake, 5);
|
||||||
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
|
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
|
||||||
ElementsAre(0, 1, 2, 3, 4));
|
ElementsAre(0, 1, 2, 3, 4));
|
||||||
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
|
EXPECT_THAT(
|
||||||
ElementsAre(mjS_STATIC,
|
AsVector(d->body_awake, m->nbody),
|
||||||
mjS_AWAKE,
|
ElementsAre(mjS_STATIC, mjS_AWAKE, mjS_STATIC, mjS_AWAKE, mjS_AWAKE));
|
||||||
mjS_STATIC,
|
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(1, 1));
|
||||||
mjS_AWAKE,
|
|
||||||
mjS_AWAKE));
|
|
||||||
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
|
|
||||||
ElementsAre(1, 1));
|
|
||||||
|
|
||||||
// Test Case 3: Tree 0 asleep
|
// Test Case 3: Tree 0 asleep
|
||||||
d->tree_asleep[0] = 0; d->tree_asleep[1] = -1;
|
d->tree_asleep[0] = 0;
|
||||||
mj_updateSleep(m, d);
|
d->tree_asleep[1] = -1;
|
||||||
|
mj_updateSleep(m.get(), d.get());
|
||||||
EXPECT_EQ(d->ntree_awake, 1);
|
EXPECT_EQ(d->ntree_awake, 1);
|
||||||
EXPECT_EQ(d->nv_awake, 2);
|
EXPECT_EQ(d->nv_awake, 2);
|
||||||
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
|
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre(3, 4));
|
||||||
ElementsAre(3, 4));
|
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(0, 1));
|
||||||
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
|
EXPECT_THAT(
|
||||||
ElementsAre(0, 1));
|
AsVector(d->body_awake, m->nbody),
|
||||||
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
|
ElementsAre(mjS_STATIC, mjS_ASLEEP, mjS_STATIC, mjS_AWAKE, mjS_AWAKE));
|
||||||
ElementsAre(mjS_STATIC,
|
|
||||||
mjS_ASLEEP,
|
|
||||||
mjS_STATIC,
|
|
||||||
mjS_AWAKE,
|
|
||||||
mjS_AWAKE));
|
|
||||||
|
|
||||||
// Test Case 4: Tree 1 asleep
|
// Test Case 4: Tree 1 asleep
|
||||||
d->tree_asleep[0] = -1; d->tree_asleep[1] = 1;
|
d->tree_asleep[0] = -1;
|
||||||
mj_updateSleep(m, d);
|
d->tree_asleep[1] = 1;
|
||||||
|
mj_updateSleep(m.get(), d.get());
|
||||||
EXPECT_EQ(d->ntree_awake, 1);
|
EXPECT_EQ(d->ntree_awake, 1);
|
||||||
EXPECT_EQ(d->nv_awake, 3);
|
EXPECT_EQ(d->nv_awake, 3);
|
||||||
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
|
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre(0, 1, 2));
|
||||||
ElementsAre(0, 1, 2));
|
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(1, 0));
|
||||||
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
|
EXPECT_THAT(
|
||||||
ElementsAre(1, 0));
|
AsVector(d->body_awake, m->nbody),
|
||||||
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
|
ElementsAre(mjS_STATIC, mjS_AWAKE, mjS_STATIC, mjS_ASLEEP, mjS_ASLEEP));
|
||||||
ElementsAre(mjS_STATIC,
|
|
||||||
mjS_AWAKE,
|
|
||||||
mjS_STATIC,
|
|
||||||
mjS_ASLEEP,
|
|
||||||
mjS_ASLEEP));
|
|
||||||
|
|
||||||
// Test Case 5: All trees asleep
|
// Test Case 5: All trees asleep
|
||||||
d->tree_asleep[0] = 0; d->tree_asleep[1] = 1;
|
d->tree_asleep[0] = 0;
|
||||||
mj_updateSleep(m, d);
|
d->tree_asleep[1] = 1;
|
||||||
|
mj_updateSleep(m.get(), d.get());
|
||||||
EXPECT_EQ(d->ntree_awake, 0);
|
EXPECT_EQ(d->ntree_awake, 0);
|
||||||
EXPECT_EQ(d->nv_awake, 0);
|
EXPECT_EQ(d->nv_awake, 0);
|
||||||
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
|
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre());
|
||||||
ElementsAre());
|
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(0, 0));
|
||||||
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
|
EXPECT_THAT(
|
||||||
ElementsAre(0, 0));
|
AsVector(d->body_awake, m->nbody),
|
||||||
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
|
ElementsAre(mjS_STATIC, mjS_ASLEEP, mjS_STATIC, mjS_ASLEEP, mjS_ASLEEP));
|
||||||
ElementsAre(mjS_STATIC,
|
|
||||||
mjS_ASLEEP,
|
|
||||||
mjS_STATIC,
|
|
||||||
mjS_ASLEEP,
|
|
||||||
mjS_ASLEEP));
|
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SleepTest, MjWakeIsland) {
|
TEST_F(SleepTest, MjWakeIsland) {
|
||||||
@@ -191,30 +165,25 @@ TEST_F(SleepTest, MjWakeIsland) {
|
|||||||
EXPECT_EQ(mj_wakeIsland(asleep, 4, 0, kAwake, nullptr, 0), 0);
|
EXPECT_EQ(mj_wakeIsland(asleep, 4, 0, kAwake, nullptr, 0), 0);
|
||||||
EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, 2, 1, 3));
|
EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, 2, 1, 3));
|
||||||
EXPECT_EQ(mj_wakeIsland(asleep, 4, 1, kAwake, nullptr, 0), 2);
|
EXPECT_EQ(mj_wakeIsland(asleep, 4, 1, kAwake, nullptr, 0), 2);
|
||||||
EXPECT_THAT(AsVector(asleep, 4),
|
EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, kAwake, kAwake, 3));
|
||||||
ElementsAre(kAwake, kAwake, kAwake, 3));
|
|
||||||
EXPECT_EQ(mj_wakeIsland(asleep, 4, 3, kAwake, nullptr, 0), 1);
|
EXPECT_EQ(mj_wakeIsland(asleep, 4, 3, kAwake, nullptr, 0), 1);
|
||||||
EXPECT_THAT(AsVector(asleep, 4),
|
EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, kAwake, kAwake, kAwake));
|
||||||
ElementsAre(kAwake, kAwake, kAwake, kAwake));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SleepTest, BadWakeIsland) {
|
TEST_F(SleepTest, BadWakeIsland) {
|
||||||
EXPECT_FATAL_FAILURE(
|
EXPECT_FATAL_FAILURE(([] {
|
||||||
([] {
|
|
||||||
int asleep_bad1[] = {-1, 0};
|
int asleep_bad1[] = {-1, 0};
|
||||||
mj_wakeIsland(asleep_bad1, 2, 1, kAwake, nullptr, 0);
|
mj_wakeIsland(asleep_bad1, 2, 1, kAwake, nullptr, 0);
|
||||||
}()),
|
}()),
|
||||||
"invalid sleep state index -1 when waking tree 1");
|
"invalid sleep state index -1 when waking tree 1");
|
||||||
|
|
||||||
EXPECT_FATAL_FAILURE(
|
EXPECT_FATAL_FAILURE(([] {
|
||||||
([] {
|
|
||||||
int asleep_bad2[] = {-1, 2};
|
int asleep_bad2[] = {-1, 2};
|
||||||
mj_wakeIsland(asleep_bad2, 2, 1, kAwake, nullptr, 0);
|
mj_wakeIsland(asleep_bad2, 2, 1, kAwake, nullptr, 0);
|
||||||
}()),
|
}()),
|
||||||
"invalid sleep state index 2 when waking tree 1");
|
"invalid sleep state index 2 when waking tree 1");
|
||||||
|
|
||||||
EXPECT_FATAL_FAILURE(
|
EXPECT_FATAL_FAILURE(([] {
|
||||||
([] {
|
|
||||||
int asleep_bad3[] = {1, 2, 1};
|
int asleep_bad3[] = {1, 2, 1};
|
||||||
mj_wakeIsland(asleep_bad3, 3, 0, kAwake, nullptr, 0);
|
mj_wakeIsland(asleep_bad3, 3, 0, kAwake, nullptr, 0);
|
||||||
}()),
|
}()),
|
||||||
@@ -361,7 +330,6 @@ TEST_F(SleepTest, WakingUnaffectedBySleeping) {
|
|||||||
mj_deleteModel(m);
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// Test that waking does not affect sleeping trees for pos/vel-dependent arrays.
|
// Test that waking does not affect sleeping trees for pos/vel-dependent arrays.
|
||||||
// Roll out models where some trees wake and/or sleep. At kCompare intervals,
|
// Roll out models where some trees wake and/or sleep. At kCompare intervals,
|
||||||
// copy the state from the mjData with sleeping enabled to another mjData and
|
// copy the state from the mjData with sleeping enabled to another mjData and
|
||||||
@@ -555,9 +523,9 @@ TEST_F(SleepTest, MidpointSleepZeroVelocity) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
// give initial velocity (both translational and angular)
|
// give initial velocity (both translational and angular)
|
||||||
d->qvel[0] = 0.5;
|
d->qvel[0] = 0.5;
|
||||||
@@ -569,7 +537,7 @@ TEST_F(SleepTest, MidpointSleepZeroVelocity) {
|
|||||||
|
|
||||||
// step until body goes to sleep
|
// step until body goes to sleep
|
||||||
for (int step = 0; step < 1000; step++) {
|
for (int step = 0; step < 1000; step++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
if (d->ntree_awake == 0) break;
|
if (d->ntree_awake == 0) break;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -581,9 +549,6 @@ TEST_F(SleepTest, MidpointSleepZeroVelocity) {
|
|||||||
EXPECT_EQ(d->qvel[i], 0.0) << "qvel[" << i << "] not zero after sleep";
|
EXPECT_EQ(d->qvel[i], 0.0) << "qvel[" << i << "] not zero after sleep";
|
||||||
EXPECT_EQ(d->qacc[i], 0.0) << "qacc[" << i << "] not zero after sleep";
|
EXPECT_EQ(d->qacc[i], 0.0) << "qacc[" << i << "] not zero after sleep";
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static const char* const kInitIslandFailModel =
|
static const char* const kInitIslandFailModel =
|
||||||
|
|||||||
+125
-165
@@ -38,10 +38,6 @@ using ::testing::Ne;
|
|||||||
using ::testing::NotNull;
|
using ::testing::NotNull;
|
||||||
using ::testing::Pointwise;
|
using ::testing::Pointwise;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
using Name2idTest = MujocoTest;
|
using Name2idTest = MujocoTest;
|
||||||
|
|
||||||
static constexpr char name2idTestingModel[] = R"(
|
static constexpr char name2idTestingModel[] = R"(
|
||||||
@@ -95,78 +91,68 @@ static constexpr char name2idTestingModel[] = R"(
|
|||||||
|
|
||||||
TEST_F(Name2idTest, FindIds) {
|
TEST_F(Name2idTest, FindIds) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model =
|
MjModelPtr model =
|
||||||
LoadModelFromString(name2idTestingModel, error, sizeof(error));
|
LoadModelFromString(name2idTestingModel, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "world"), 0);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "world"), 0);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body1"), 1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "body1"), 1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body2"), 2);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "body2"), 2);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom1"), 0);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "body1_geom1"), 0);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom2"), 1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "body1_geom2"), 1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint2"), 1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_JOINT, "joint2"), 1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "mesh1"), 0);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MESH, "mesh1"), 0);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "light1"), 0);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_LIGHT, "light1"), 0);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "camera1"), 0);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_CAMERA, "camera1"), 0);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "site2"), 1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SITE, "site2"), 1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "material1"), 0);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MATERIAL, "material1"), 0);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "texture1"), 0);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TEXTURE, "texture1"), 0);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "tendon1"), 0);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TENDON, "tendon1"), 0);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "actuator1"), 0);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_ACTUATOR, "actuator1"), 0);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "sensor1"), 0);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SENSOR, "sensor1"), 0);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(Name2idTest, MissingIds) {
|
TEST_F(Name2idTest, MissingIds) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model =
|
MjModelPtr model =
|
||||||
LoadModelFromString(name2idTestingModel, error, sizeof(error));
|
LoadModelFromString(name2idTestingModel, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "abody3"), -1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "abody3"), -1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "abody2_geom2"), -1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "abody2_geom2"), -1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint3"), -1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_JOINT, "joint3"), -1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "amesh2"), -1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MESH, "amesh2"), -1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "alight2"), -1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_LIGHT, "alight2"), -1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "acamera2"), -1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_CAMERA, "acamera2"), -1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "asite3"), -1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SITE, "asite3"), -1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "amaterial2"), -1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MATERIAL, "amaterial2"), -1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "atexture2"), -1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TEXTURE, "atexture2"), -1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "atendon2"), -1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TENDON, "atendon2"), -1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "aactuator2"), -1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_ACTUATOR, "aactuator2"), -1);
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "asensor2"), -1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SENSOR, "asensor2"), -1);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(Name2idTest, EmptyIds) {
|
TEST_F(Name2idTest, EmptyIds) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model =
|
MjModelPtr model =
|
||||||
LoadModelFromString(name2idTestingModel, error, sizeof(error));
|
LoadModelFromString(name2idTestingModel, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, ""), -1);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, ""), -1);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(Name2idTest, Namespaces) {
|
TEST_F(Name2idTest, Namespaces) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model =
|
MjModelPtr model =
|
||||||
LoadModelFromString(name2idTestingModel, error, sizeof(error));
|
LoadModelFromString(name2idTestingModel, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "camera1"), 3);
|
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "camera1"), 3);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
using VersionTest = MujocoTest;
|
using VersionTest = MujocoTest;
|
||||||
|
|
||||||
TEST_F(VersionTest, MjVersion) {
|
TEST_F(VersionTest, MjVersion) { EXPECT_EQ(mj_version(), mjVERSION_HEADER); }
|
||||||
EXPECT_EQ(mj_version(), mjVERSION_HEADER);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_F(VersionTest, MjVersionString) {
|
TEST_F(VersionTest, MjVersionString) {
|
||||||
#if GTEST_USES_SIMPLE_RE == 1
|
#if GTEST_USES_SIMPLE_RE == 1
|
||||||
@@ -177,7 +163,6 @@ TEST_F(VersionTest, MjVersionString) {
|
|||||||
EXPECT_THAT(std::string(mj_versionString()), regex_matcher);
|
EXPECT_THAT(std::string(mj_versionString()), regex_matcher);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
using SupportTest = MujocoTest;
|
using SupportTest = MujocoTest;
|
||||||
|
|
||||||
// utility: generate two random quaternions with a given angle difference
|
// utility: generate two random quaternions with a given angle difference
|
||||||
@@ -218,8 +203,8 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) {
|
|||||||
const mjtNum eps = 1e-12; // epsilon for float comparison
|
const mjtNum eps = 1e-12; // epsilon for float comparison
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(ballJointModel, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(ballJointModel, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
int seed = 1;
|
int seed = 1;
|
||||||
for (mjtNum angle : {0.0, 1e-5, 1e-2}) {
|
for (mjtNum angle : {0.0, 1e-5, 1e-2}) {
|
||||||
@@ -230,7 +215,7 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) {
|
|||||||
|
|
||||||
// get velocity given timestep
|
// get velocity given timestep
|
||||||
mjtNum qvel[3];
|
mjtNum qvel[3];
|
||||||
mj_differentiatePos(model, qvel, dt, qpos1, qpos2);
|
mj_differentiatePos(model.get(), qvel, dt, qpos1, qpos2);
|
||||||
|
|
||||||
// equivalent computation
|
// equivalent computation
|
||||||
mjtNum qneg[4], qdif[4], qvel_expect[3];
|
mjtNum qneg[4], qdif[4], qvel_expect[3];
|
||||||
@@ -242,8 +227,6 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) {
|
|||||||
EXPECT_THAT(AsVector(qvel, 3), Pointwise(MjNear(eps, 1e-3), qvel_expect));
|
EXPECT_THAT(AsVector(qvel, 3), Pointwise(MjNear(eps, 1e-3), qvel_expect));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static const char* const kDefaultModel = "testdata/model.xml";
|
static const char* const kDefaultModel = "testdata/model.xml";
|
||||||
@@ -320,38 +303,35 @@ TEST_F(StateTest, GetSetStateDelay) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// verify history buffer exists: nhistory = 2 + 2*5 = 12
|
// verify history buffer exists: nhistory = 2 + 2*5 = 12
|
||||||
EXPECT_EQ(model->nhistory, 12); // [user, cursor, times(5), values(5)]
|
EXPECT_EQ(model->nhistory, 12); // [user, cursor, times(5), values(5)]
|
||||||
|
|
||||||
// state size should include history buffer
|
// state size should include history buffer
|
||||||
int size = mj_stateSize(model, mjSTATE_HISTORY);
|
int size = mj_stateSize(model.get(), mjSTATE_HISTORY);
|
||||||
EXPECT_EQ(size, model->nhistory);
|
EXPECT_EQ(size, model->nhistory);
|
||||||
|
|
||||||
// step to populate history buffer
|
// step to populate history buffer
|
||||||
data->ctrl[0] = 1.0;
|
data->ctrl[0] = 1.0;
|
||||||
mj_step(model, data);
|
mj_step(model.get(), data.get());
|
||||||
data->ctrl[0] = 2.0;
|
data->ctrl[0] = 2.0;
|
||||||
mj_step(model, data);
|
mj_step(model.get(), data.get());
|
||||||
|
|
||||||
// get history state
|
// get history state
|
||||||
vector<mjtNum> history_state(size);
|
vector<mjtNum> history_state(size);
|
||||||
mj_getState(model, data, history_state.data(), mjSTATE_HISTORY);
|
mj_getState(model.get(), data.get(), history_state.data(), mjSTATE_HISTORY);
|
||||||
|
|
||||||
// modify the history buffer manually (value at index 7 = 2+5 = after times)
|
// modify the history buffer manually (value at index 7 = 2+5 = after times)
|
||||||
data->history[7] = 99.0; // first value
|
data->history[7] = 99.0; // first value
|
||||||
|
|
||||||
// set history state back - should restore original
|
// set history state back - should restore original
|
||||||
mj_setState(model, data, history_state.data(), mjSTATE_HISTORY);
|
mj_setState(model.get(), data.get(), history_state.data(), mjSTATE_HISTORY);
|
||||||
|
|
||||||
// verify restoration
|
// verify restoration
|
||||||
EXPECT_NE(data->history[7], 99.0);
|
EXPECT_NE(data->history[7], 99.0);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(StateTest, CopyState) {
|
TEST_F(StateTest, CopyState) {
|
||||||
@@ -518,8 +498,7 @@ TEST_F(InertiaTest, AddMdenseSameAsSparse) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// sparse addM
|
// sparse addM
|
||||||
mj_addM(m, d, dst_sparse.data(), rownnz.data(),
|
mj_addM(m, d, dst_sparse.data(), rownnz.data(), rowadr.data(), colind.data());
|
||||||
rowadr.data(), colind.data());
|
|
||||||
|
|
||||||
// dense addM
|
// dense addM
|
||||||
mj_addM(m, d, dst_dense.data(), nullptr, nullptr, nullptr);
|
mj_addM(m, d, dst_dense.data(), nullptr, nullptr, nullptr);
|
||||||
@@ -548,8 +527,8 @@ TEST_F(InertiaTest, mulM) {
|
|||||||
|
|
||||||
// dense M matrix
|
// dense M matrix
|
||||||
vector<mjtNum> Mdense(nv * nv);
|
vector<mjtNum> Mdense(nv * nv);
|
||||||
mju_sym2dense(Mdense.data(), data->M, nv,
|
mju_sym2dense(Mdense.data(), data->M, nv, model->M_rownnz, model->M_rowadr,
|
||||||
model->M_rownnz, model->M_rowadr, model->M_colind);
|
model->M_colind);
|
||||||
|
|
||||||
// arbitrary RHS vector
|
// arbitrary RHS vector
|
||||||
vector<mjtNum> vec(nv);
|
vector<mjtNum> vec(nv);
|
||||||
@@ -618,8 +597,8 @@ TEST_F(InertiaTest, FullM) {
|
|||||||
|
|
||||||
// get dense mass matrix from M using mju_sparse2dense
|
// get dense mass matrix from M using mju_sparse2dense
|
||||||
vector<mjtNum> M_CSR(nv * nv);
|
vector<mjtNum> M_CSR(nv * nv);
|
||||||
mju_sparse2dense(M_CSR.data(), d->M, nv, nv,
|
mju_sparse2dense(M_CSR.data(), d->M, nv, nv, m->M_rownnz, m->M_rowadr,
|
||||||
m->M_rownnz, m->M_rowadr, m->M_colind);
|
m->M_colind);
|
||||||
|
|
||||||
// expect lower triangles to match exactly
|
// expect lower triangles to match exactly
|
||||||
for (int i = 0; i < nv; ++i) {
|
for (int i = 0; i < nv; ++i) {
|
||||||
@@ -630,8 +609,8 @@ TEST_F(InertiaTest, FullM) {
|
|||||||
|
|
||||||
// get dense LTDL factor (D on the diagonal)
|
// get dense LTDL factor (D on the diagonal)
|
||||||
vector<mjtNum> LD(nv * nv);
|
vector<mjtNum> LD(nv * nv);
|
||||||
mju_sparse2dense(LD.data(), d->qLD, nv, nv,
|
mju_sparse2dense(LD.data(), d->qLD, nv, nv, m->M_rownnz, m->M_rowadr,
|
||||||
m->M_rownnz, m->M_rowadr, m->M_colind);
|
m->M_colind);
|
||||||
|
|
||||||
// extract L and D from LD
|
// extract L and D from LD
|
||||||
vector<mjtNum> L = LD;
|
vector<mjtNum> L = LD;
|
||||||
@@ -686,88 +665,85 @@ static constexpr char GeomDistanceTestingModel2[] = R"(
|
|||||||
|
|
||||||
TEST_F(SupportTest, GeomDistance) {
|
TEST_F(SupportTest, GeomDistance) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model =
|
MjModelPtr model =
|
||||||
LoadModelFromString(GeomDistanceTestingModel1, error, sizeof(error));
|
LoadModelFromString(GeomDistanceTestingModel1, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
|
|
||||||
// plane-sphere, distmax too small
|
// plane-sphere, distmax too small
|
||||||
mjtNum distmax = 0.5;
|
mjtNum distmax = 0.5;
|
||||||
EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, nullptr), 0.5);
|
EXPECT_EQ(mj_geomDistance(model.get(), data.get(), 0, 1, distmax, nullptr),
|
||||||
|
0.5);
|
||||||
mjtNum fromto[6];
|
mjtNum fromto[6];
|
||||||
EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, fromto), 0.5);
|
EXPECT_EQ(mj_geomDistance(model.get(), data.get(), 0, 1, distmax, fromto),
|
||||||
|
0.5);
|
||||||
EXPECT_THAT(fromto, Pointwise(Eq(), vector<mjtNum>{0, 0, 0, 0, 0, 0}));
|
EXPECT_THAT(fromto, Pointwise(Eq(), vector<mjtNum>{0, 0, 0, 0, 0, 0}));
|
||||||
|
|
||||||
// plane-sphere
|
// plane-sphere
|
||||||
distmax = 1.0;
|
distmax = 1.0;
|
||||||
EXPECT_THAT(mj_geomDistance(model, data, 0, 1, 1.0, fromto),
|
EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 0, 1, 1.0, fromto),
|
||||||
MjNear(0.8, 1e-12, 1e-5));
|
MjNear(0.8, 1e-12, 1e-5));
|
||||||
mjtNum eps = 1e-12;
|
mjtNum eps = 1e-12;
|
||||||
EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5),
|
EXPECT_THAT(fromto,
|
||||||
vector<mjtNum>{0, 0, 0, 0, 0, 0.8}));
|
Pointwise(MjNear(eps, 1e-5), vector<mjtNum>{0, 0, 0, 0, 0, 0.8}));
|
||||||
|
|
||||||
// sphere-plane
|
// sphere-plane
|
||||||
EXPECT_THAT(mj_geomDistance(model, data, 1, 0, 1.0, fromto),
|
EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 1, 0, 1.0, fromto),
|
||||||
MjNear(0.8, 1e-12, 1e-5));
|
MjNear(0.8, 1e-12, 1e-5));
|
||||||
EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5),
|
EXPECT_THAT(fromto,
|
||||||
vector<mjtNum>{0, 0, 0.8, 0, 0, 0}));
|
Pointwise(MjNear(eps, 1e-5), vector<mjtNum>{0, 0, 0.8, 0, 0, 0}));
|
||||||
|
|
||||||
// sphere-sphere
|
// sphere-sphere
|
||||||
EXPECT_THAT(mj_geomDistance(model, data, 1, 2, 1.0, fromto),
|
EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 1, 2, 1.0, fromto),
|
||||||
MjNear(0.5, 1e-12, 1e-5));
|
MjNear(0.5, 1e-12, 1e-5));
|
||||||
EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5),
|
EXPECT_THAT(fromto,
|
||||||
vector<mjtNum>{.2, 0, 1, .7, 0, 1}));
|
Pointwise(MjNear(eps, 1e-5), vector<mjtNum>{.2, 0, 1, .7, 0, 1}));
|
||||||
|
|
||||||
// sphere-sphere, flipped order
|
// sphere-sphere, flipped order
|
||||||
EXPECT_THAT(mj_geomDistance(model, data, 2, 1, 1.0, fromto),
|
EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 2, 1, 1.0, fromto),
|
||||||
MjNear(0.5, 1e-12, 1e-5));
|
MjNear(0.5, 1e-12, 1e-5));
|
||||||
EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5),
|
EXPECT_THAT(fromto,
|
||||||
vector<mjtNum>{.7, 0, 1, .2, 0, 1}));
|
Pointwise(MjNear(eps, 1e-5), vector<mjtNum>{.7, 0, 1, .2, 0, 1}));
|
||||||
|
|
||||||
// mesh-sphere (close distmax)
|
// mesh-sphere (close distmax)
|
||||||
distmax = 0.701;
|
distmax = 0.701;
|
||||||
eps = model->opt.ccd_tolerance;
|
eps = model->opt.ccd_tolerance;
|
||||||
EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto),
|
EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 3, 1, distmax, fromto),
|
||||||
MjNear(0.7, eps, eps * 100));
|
MjNear(0.7, eps, eps * 100));
|
||||||
EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps * 100),
|
EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps * 100),
|
||||||
vector<mjtNum>{0, 0, .1, 0, 0, .8}));
|
vector<mjtNum>{0, 0, .1, 0, 0, .8}));
|
||||||
|
|
||||||
// mesh-sphere (far distmax)
|
// mesh-sphere (far distmax)
|
||||||
distmax = 1.0;
|
distmax = 1.0;
|
||||||
EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto),
|
EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 3, 1, distmax, fromto),
|
||||||
MjNear(0.7, eps, eps * 100));
|
MjNear(0.7, eps, eps * 100));
|
||||||
EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps * 100),
|
EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps * 100),
|
||||||
vector<mjtNum>{0, 0, .1, 0, 0, .8}));
|
vector<mjtNum>{0, 0, .1, 0, 0, .8}));
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SupportTest, GeomDistanceFromToFlipped) {
|
TEST_F(SupportTest, GeomDistanceFromToFlipped) {
|
||||||
mjtNum distmax = 10.0;
|
mjtNum distmax = 10.0;
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model =
|
MjModelPtr model =
|
||||||
LoadModelFromString(GeomDistanceTestingModel2, error, sizeof(error));
|
LoadModelFromString(GeomDistanceTestingModel2, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
|
|
||||||
mjtNum fromto01[6];
|
mjtNum fromto01[6];
|
||||||
mjtNum fromto10[6];
|
mjtNum fromto10[6];
|
||||||
|
|
||||||
for (int flag : {0, (int)mjDSBL_NATIVECCD}) {
|
for (int flag : {0, (int)mjDSBL_NATIVECCD}) {
|
||||||
model->opt.disableflags = flag;
|
model->opt.disableflags = flag;
|
||||||
mj_geomDistance(model, data, 0, 1, distmax, fromto01);
|
mj_geomDistance(model.get(), data.get(), 0, 1, distmax, fromto01);
|
||||||
mj_geomDistance(model, data, 1, 0, distmax, fromto10);
|
mj_geomDistance(model.get(), data.get(), 1, 0, distmax, fromto10);
|
||||||
mjtNum fromto10flipped[6] = {fromto10[3], fromto10[4], fromto10[5],
|
mjtNum fromto10flipped[6] = {fromto10[3], fromto10[4], fromto10[5],
|
||||||
fromto10[0], fromto10[1], fromto10[2]};
|
fromto10[0], fromto10[1], fromto10[2]};
|
||||||
|
|
||||||
EXPECT_THAT(AsVector(fromto10flipped, 6),
|
EXPECT_THAT(AsVector(fromto10flipped, 6),
|
||||||
Pointwise(MjNear(1.0e-12, 1e-5), fromto01));
|
Pointwise(MjNear(1.0e-12, 1e-5), fromto01));
|
||||||
}
|
}
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static constexpr char kSetKeyframeTestingModel[] = R"(
|
static constexpr char kSetKeyframeTestingModel[] = R"(
|
||||||
@@ -789,40 +765,35 @@ static constexpr char kSetKeyframeTestingModel[] = R"(
|
|||||||
|
|
||||||
TEST_F(SupportTest, SetKeyframe) {
|
TEST_F(SupportTest, SetKeyframe) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model =
|
MjModelPtr model =
|
||||||
LoadModelFromString(kSetKeyframeTestingModel, error, sizeof(error));
|
LoadModelFromString(kSetKeyframeTestingModel, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
data->ctrl[0] = 1;
|
data->ctrl[0] = 1;
|
||||||
while (data->time < 1) {
|
while (data->time < 1) {
|
||||||
mj_step(model, data);
|
mj_step(model.get(), data.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_setKeyframe(model, data, 1);
|
mj_setKeyframe(model.get(), data.get(), 1);
|
||||||
EXPECT_EQ(data->time, model->key_time[1]);
|
EXPECT_EQ(data->time, model->key_time[1]);
|
||||||
EXPECT_EQ(data->ctrl[0], model->key_ctrl[model->nu * 1]);
|
EXPECT_EQ(data->ctrl[0], model->key_ctrl[model->nu * 1]);
|
||||||
EXPECT_EQ(data->qpos[0], model->key_qpos[model->nq * 1]);
|
EXPECT_EQ(data->qpos[0], model->key_qpos[model->nq * 1]);
|
||||||
EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 1]);
|
EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 1]);
|
||||||
EXPECT_EQ(data->act[0], model->key_act[model->na * 1]);
|
EXPECT_EQ(data->act[0], model->key_act[model->na * 1]);
|
||||||
|
|
||||||
mj_step(model, data);
|
mj_step(model.get(), data.get());
|
||||||
mj_setKeyframe(model, data, 0);
|
mj_setKeyframe(model.get(), data.get(), 0);
|
||||||
EXPECT_EQ(data->time, model->key_time[0]);
|
EXPECT_EQ(data->time, model->key_time[0]);
|
||||||
EXPECT_EQ(data->ctrl[0], model->key_ctrl[model->nu * 0]);
|
EXPECT_EQ(data->ctrl[0], model->key_ctrl[model->nu * 0]);
|
||||||
EXPECT_EQ(data->qpos[0], model->key_qpos[model->nq * 0]);
|
EXPECT_EQ(data->qpos[0], model->key_qpos[model->nq * 0]);
|
||||||
EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 0]);
|
EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 0]);
|
||||||
EXPECT_EQ(data->act[0], model->key_act[model->na * 0]);
|
EXPECT_EQ(data->act[0], model->key_act[model->na * 0]);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SupportTest, ContactSensorDim) {
|
TEST_F(SupportTest, ContactSensorDim) {
|
||||||
int dataSpec = 1 << mjCONDATA_FOUND |
|
int dataSpec = 1 << mjCONDATA_FOUND | 1 << mjCONDATA_FORCE |
|
||||||
1 << mjCONDATA_FORCE |
|
1 << mjCONDATA_DIST | 1 << mjCONDATA_POS |
|
||||||
1 << mjCONDATA_DIST |
|
|
||||||
1 << mjCONDATA_POS |
|
|
||||||
1 << mjCONDATA_TANGENT;
|
1 << mjCONDATA_TANGENT;
|
||||||
|
|
||||||
EXPECT_EQ(mju_condataSize(dataSpec), 1 + 3 + 1 + 3 + 3);
|
EXPECT_EQ(mju_condataSize(dataSpec), 1 + 3 + 1 + 3 + 3);
|
||||||
@@ -844,16 +815,14 @@ TEST_F(SupportTest, ReadCtrlNoDelay) {
|
|||||||
</actuator>
|
</actuator>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* model = LoadModelFromString(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
ASSERT_THAT(model, NotNull());
|
ASSERT_THAT(model.get(), NotNull());
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// no delay: should return current ctrl value
|
// no delay: should return current ctrl value
|
||||||
data->ctrl[0] = 42.0;
|
data->ctrl[0] = 42.0;
|
||||||
EXPECT_EQ(mj_readCtrl(model, data, 0, data->time, /*order=*/0), 42.0);
|
EXPECT_EQ(mj_readCtrl(model.get(), data.get(), 0, data->time, /*order=*/0),
|
||||||
|
42.0);
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SupportTest, ReadCtrlWithDelay) {
|
TEST_F(SupportTest, ReadCtrlWithDelay) {
|
||||||
@@ -872,9 +841,9 @@ TEST_F(SupportTest, ReadCtrlWithDelay) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// model should have delay configured
|
// model should have delay configured
|
||||||
// delay = 0.03 seconds, timestep = 0.01, so ndelay = ceil(0.03/0.01) = 3
|
// delay = 0.03 seconds, timestep = 0.01, so ndelay = ceil(0.03/0.01) = 3
|
||||||
@@ -884,11 +853,8 @@ TEST_F(SupportTest, ReadCtrlWithDelay) {
|
|||||||
|
|
||||||
// initially, buffer should be filled with constant value (from init)
|
// initially, buffer should be filled with constant value (from init)
|
||||||
// reading at current time should return the init value
|
// reading at current time should return the init value
|
||||||
mjtNum val = mj_readCtrl(model, data, 0, data->time, /*order=*/0);
|
mjtNum val = mj_readCtrl(model.get(), data.get(), 0, data->time, /*order=*/0);
|
||||||
EXPECT_EQ(val, data->ctrl[0]);
|
EXPECT_EQ(val, data->ctrl[0]);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SupportTest, InitCtrlDelay) {
|
TEST_F(SupportTest, InitCtrlDelay) {
|
||||||
@@ -907,35 +873,33 @@ TEST_F(SupportTest, InitCtrlDelay) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// verify nhistory
|
// verify nhistory
|
||||||
EXPECT_EQ(model->actuator_history[0], 3);
|
EXPECT_EQ(model->actuator_history[0], 3);
|
||||||
|
|
||||||
// initialize with custom times and values
|
// initialize with custom times and values
|
||||||
// buffer stores: time 0.0 -> value 1.0, time 0.01 -> value 2.0, time 0.02 -> value 3.0
|
// buffer stores: time 0.0 -> value 1.0, time 0.01 -> value 2.0, time 0.02 ->
|
||||||
|
// value 3.0
|
||||||
mjtNum times[3] = {0.0, 0.01, 0.02};
|
mjtNum times[3] = {0.0, 0.01, 0.02};
|
||||||
mjtNum values[3] = {1.0, 2.0, 3.0};
|
mjtNum values[3] = {1.0, 2.0, 3.0};
|
||||||
mj_initCtrlHistory(model, data, 0, times, values);
|
mj_initCtrlHistory(model.get(), data.get(), 0, times, values);
|
||||||
|
|
||||||
// mj_readCtrl now auto-subtracts delay: lookup_time = time - delay
|
// mj_readCtrl now auto-subtracts delay: lookup_time = time - delay
|
||||||
// delay = 0.02, so:
|
// delay = 0.02, so:
|
||||||
// time=0.04 -> lookup at 0.02 -> value 3.0
|
// time=0.04 -> lookup at 0.02 -> value 3.0
|
||||||
// time=0.03 -> lookup at 0.01 -> value 2.0
|
// time=0.03 -> lookup at 0.01 -> value 2.0
|
||||||
// time=0.02 -> lookup at 0.00 -> value 1.0
|
// time=0.02 -> lookup at 0.00 -> value 1.0
|
||||||
mjtNum val = mj_readCtrl(model, data, 0, 0.04, /*order=*/0);
|
mjtNum val = mj_readCtrl(model.get(), data.get(), 0, 0.04, /*order=*/0);
|
||||||
EXPECT_EQ(val, 3.0);
|
EXPECT_EQ(val, 3.0);
|
||||||
|
|
||||||
val = mj_readCtrl(model, data, 0, 0.03, /*order=*/0);
|
val = mj_readCtrl(model.get(), data.get(), 0, 0.03, /*order=*/0);
|
||||||
EXPECT_EQ(val, 2.0);
|
EXPECT_EQ(val, 2.0);
|
||||||
|
|
||||||
val = mj_readCtrl(model, data, 0, 0.02, /*order=*/0);
|
val = mj_readCtrl(model.get(), data.get(), 0, 0.02, /*order=*/0);
|
||||||
EXPECT_EQ(val, 1.0);
|
EXPECT_EQ(val, 1.0);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SupportTest, InitCtrlDelayNullTimes) {
|
TEST_F(SupportTest, InitCtrlDelayNullTimes) {
|
||||||
@@ -954,9 +918,9 @@ TEST_F(SupportTest, InitCtrlDelayNullTimes) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// get existing times from buffer
|
// get existing times from buffer
|
||||||
int adr = model->actuator_historyadr[0];
|
int adr = model->actuator_historyadr[0];
|
||||||
@@ -965,7 +929,7 @@ TEST_F(SupportTest, InitCtrlDelayNullTimes) {
|
|||||||
|
|
||||||
// initialize with NULL times (use existing) and new values
|
// initialize with NULL times (use existing) and new values
|
||||||
mjtNum values[3] = {10.0, 20.0, 30.0};
|
mjtNum values[3] = {10.0, 20.0, 30.0};
|
||||||
mj_initCtrlHistory(model, data, 0, nullptr, values);
|
mj_initCtrlHistory(model.get(), data.get(), 0, nullptr, values);
|
||||||
|
|
||||||
// verify times are unchanged
|
// verify times are unchanged
|
||||||
EXPECT_EQ(buf[2], existing_times[0]);
|
EXPECT_EQ(buf[2], existing_times[0]);
|
||||||
@@ -976,9 +940,6 @@ TEST_F(SupportTest, InitCtrlDelayNullTimes) {
|
|||||||
EXPECT_EQ(buf[5], 10.0);
|
EXPECT_EQ(buf[5], 10.0);
|
||||||
EXPECT_EQ(buf[6], 20.0);
|
EXPECT_EQ(buf[6], 20.0);
|
||||||
EXPECT_EQ(buf[7], 30.0);
|
EXPECT_EQ(buf[7], 30.0);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SupportTest, InitSensorDelay) {
|
TEST_F(SupportTest, InitSensorDelay) {
|
||||||
@@ -997,36 +958,35 @@ TEST_F(SupportTest, InitSensorDelay) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// verify nsample for sensor
|
// verify nsample for sensor
|
||||||
EXPECT_EQ(model->sensor_history[0], 3);
|
EXPECT_EQ(model->sensor_history[0], 3);
|
||||||
|
|
||||||
// initialize with custom times and values, phase=0
|
// initialize with custom times and values, phase=0
|
||||||
// buffer stores: time 0.0 -> value 0.5, time 0.01 -> value 0.6, time 0.02 -> value 0.7
|
// buffer stores: time 0.0 -> value 0.5, time 0.01 -> value 0.6, time 0.02 ->
|
||||||
|
// value 0.7
|
||||||
mjtNum times[3] = {0.0, 0.01, 0.02};
|
mjtNum times[3] = {0.0, 0.01, 0.02};
|
||||||
mjtNum values[3] = {0.5, 0.6, 0.7};
|
mjtNum values[3] = {0.5, 0.6, 0.7};
|
||||||
mj_initSensorHistory(model, data, 0, times, values, /*phase=*/0.0);
|
mj_initSensorHistory(model.get(), data.get(), 0, times, values,
|
||||||
|
/*phase=*/0.0);
|
||||||
|
|
||||||
// mj_readSensor now auto-subtracts delay: lookup_time = time - delay
|
// mj_readSensor now auto-subtracts delay: lookup_time = time - delay
|
||||||
// delay = 0.02, so:
|
// delay = 0.02, so:
|
||||||
// time=0.04 -> lookup at 0.02 -> value 0.7
|
// time=0.04 -> lookup at 0.02 -> value 0.7
|
||||||
// time=0.03 -> lookup at 0.01 -> value 0.6
|
// time=0.03 -> lookup at 0.01 -> value 0.6
|
||||||
mjtNum result = 0;
|
mjtNum result = 0;
|
||||||
const mjtNum* ptr = mj_readSensor(model, data, 0, 0.04, &result, /*order=*/0);
|
const mjtNum* ptr =
|
||||||
|
mj_readSensor(model.get(), data.get(), 0, 0.04, &result, /*order=*/0);
|
||||||
mjtNum val = ptr ? *ptr : result;
|
mjtNum val = ptr ? *ptr : result;
|
||||||
EXPECT_NEAR(val, 0.7, 1e-6);
|
EXPECT_NEAR(val, 0.7, 1e-6);
|
||||||
|
|
||||||
ptr = mj_readSensor(model, data, 0, 0.03, &result, /*order=*/0);
|
ptr = mj_readSensor(model.get(), data.get(), 0, 0.03, &result, /*order=*/0);
|
||||||
val = ptr ? *ptr : result;
|
val = ptr ? *ptr : result;
|
||||||
EXPECT_NEAR(val, 0.6, 1e-6);
|
EXPECT_NEAR(val, 0.6, 1e-6);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace mujoco
|
} // namespace mujoco
|
||||||
|
|
||||||
|
|||||||
@@ -16,9 +16,9 @@
|
|||||||
#include <array>
|
#include <array>
|
||||||
#include <cstddef>
|
#include <cstddef>
|
||||||
|
|
||||||
#include <mujoco/mjdata.h>
|
|
||||||
#include <gmock/gmock.h>
|
#include <gmock/gmock.h>
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
|
#include <mujoco/mjdata.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
#include "test/fixture.h"
|
#include "test/fixture.h"
|
||||||
|
|
||||||
@@ -32,8 +32,7 @@ constexpr int GetExpectedStackUsageBytes() {
|
|||||||
if constexpr (N <= 0) {
|
if constexpr (N <= 0) {
|
||||||
return prev_size;
|
return prev_size;
|
||||||
} else {
|
} else {
|
||||||
constexpr auto RoundUpToAlignment =
|
constexpr auto RoundUpToAlignment = [](int x, int alignment) {
|
||||||
[](int x, int alignment) {
|
|
||||||
return alignment * (x / alignment + ((x % alignment) ? 1 : 0));
|
return alignment * (x / alignment + ((x % alignment) ? 1 : 0));
|
||||||
};
|
};
|
||||||
constexpr int size_with_arraylist = RoundUpToAlignment(
|
constexpr int size_with_arraylist = RoundUpToAlignment(
|
||||||
@@ -49,15 +48,15 @@ class TestMjArrayList : public MujocoTest {};
|
|||||||
|
|
||||||
TEST_F(TestMjArrayList, TestMjArrayListSingleThreaded) {
|
TEST_F(TestMjArrayList, TestMjArrayListSingleThreaded) {
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString("<mujoco/>", error.data(), error.size());
|
MjModelPtr m = LoadModelFromString("<mujoco/>", error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << "Failed to load model: " << error.data();
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_markStack(d);
|
mj_markStack(d.get());
|
||||||
|
|
||||||
using DataType = int;
|
using DataType = int;
|
||||||
constexpr int kInitialCapacity = 10;
|
constexpr int kInitialCapacity = 10;
|
||||||
mjArrayList* array_list =
|
mjArrayList* array_list =
|
||||||
mju_arrayListCreate(d, sizeof(DataType), kInitialCapacity);
|
mju_arrayListCreate(d.get(), sizeof(DataType), kInitialCapacity);
|
||||||
|
|
||||||
constexpr int kNumElements = 35;
|
constexpr int kNumElements = 35;
|
||||||
for (int i = 0; i < kNumElements; ++i) {
|
for (int i = 0; i < kNumElements; ++i) {
|
||||||
@@ -78,19 +77,17 @@ TEST_F(TestMjArrayList, TestMjArrayListSingleThreaded) {
|
|||||||
EXPECT_EQ(mju_arrayListAt(array_list, kNumElements), nullptr);
|
EXPECT_EQ(mju_arrayListAt(array_list, kNumElements), nullptr);
|
||||||
EXPECT_EQ(mju_arrayListAt(array_list, 100), nullptr);
|
EXPECT_EQ(mju_arrayListAt(array_list, 100), nullptr);
|
||||||
|
|
||||||
mj_freeStack(d);
|
mj_freeStack(d.get());
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(TestMjArrayList, ZeroInitialCapacity) {
|
TEST_F(TestMjArrayList, ZeroInitialCapacity) {
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* m = LoadModelFromString("<mujoco/>", error, sizeof(error));
|
MjModelPtr m = LoadModelFromString("<mujoco/>", error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
|
ASSERT_THAT(m.get(), NotNull()) << "Failed to load model: " << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_markStack(d);
|
mj_markStack(d.get());
|
||||||
mjArrayList* array_list =
|
mjArrayList* array_list =
|
||||||
mju_arrayListCreate(d, sizeof(double), /*initial_capacity=*/0);
|
mju_arrayListCreate(d.get(), sizeof(double), /*initial_capacity=*/0);
|
||||||
EXPECT_EQ(mju_arrayListSize(array_list), 0);
|
EXPECT_EQ(mju_arrayListSize(array_list), 0);
|
||||||
|
|
||||||
for (int i = 0; i < 35; ++i) {
|
for (int i = 0; i < 35; ++i) {
|
||||||
@@ -104,9 +101,7 @@ TEST_F(TestMjArrayList, ZeroInitialCapacity) {
|
|||||||
}
|
}
|
||||||
EXPECT_EQ(mju_arrayListAt(array_list, 35), nullptr);
|
EXPECT_EQ(mju_arrayListAt(array_list, 35), nullptr);
|
||||||
|
|
||||||
mj_freeStack(d);
|
mj_freeStack(d.get());
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
@@ -14,19 +14,20 @@
|
|||||||
|
|
||||||
// Tests for engine/engine_util_solve.c.
|
// Tests for engine/engine_util_solve.c.
|
||||||
|
|
||||||
|
#include "src/engine/engine_util_misc.h"
|
||||||
|
|
||||||
#include <array>
|
#include <array>
|
||||||
#include <vector>
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <cstddef>
|
#include <cstddef>
|
||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
#include <gmock/gmock.h>
|
#include <gmock/gmock.h>
|
||||||
#include <gtest/gtest.h>
|
|
||||||
#include <gtest/gtest-spi.h>
|
#include <gtest/gtest-spi.h>
|
||||||
|
#include <gtest/gtest.h>
|
||||||
#include <mujoco/mjdata.h>
|
#include <mujoco/mjdata.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
#include "src/engine/engine_util_misc.h"
|
|
||||||
#include "test/fixture.h"
|
#include "test/fixture.h"
|
||||||
|
|
||||||
namespace mujoco {
|
namespace mujoco {
|
||||||
@@ -125,29 +126,27 @@ TEST_F(UtilMiscTest, SphereWrap) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
// measure tendon length for keyframe 0
|
// measure tendon length for keyframe 0
|
||||||
mj_resetDataKeyframe(model, data, 0);
|
mj_resetDataKeyframe(model.get(), data.get(), 0);
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
mjtNum ten_length0 = data->sensordata[0];
|
mjtNum ten_length0 = data->sensordata[0];
|
||||||
|
|
||||||
// measure tendon length for keyframe 1
|
// measure tendon length for keyframe 1
|
||||||
mj_resetDataKeyframe(model, data, 1);
|
mj_resetDataKeyframe(model.get(), data.get(), 1);
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
mjtNum ten_length1 = data->sensordata[0];
|
mjtNum ten_length1 = data->sensordata[0];
|
||||||
|
|
||||||
// difference should be small
|
// difference should be small
|
||||||
mjtNum diff = ten_length1 - ten_length0;
|
mjtNum diff = ten_length1 - ten_length0;
|
||||||
EXPECT_LT(mju_abs(diff), 1e-3);
|
EXPECT_LT(mju_abs(diff), 1e-3);
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// compute time constant as in Millard et al. (2013) https://doi.org/10.1115/1.4023390
|
// compute time constant as in Millard et al. (2013)
|
||||||
|
// https://doi.org/10.1115/1.4023390
|
||||||
mjtNum muscleDynamicsMillard(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) {
|
mjtNum muscleDynamicsMillard(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) {
|
||||||
// clamp control
|
// clamp control
|
||||||
mjtNum ctrlclamp = mju_clip(ctrl, 0, 1);
|
mjtNum ctrlclamp = mju_clip(ctrl, 0, 1);
|
||||||
@@ -196,10 +195,10 @@ TEST_F(UtilMiscTest, SmoothMuscleDynamics) {
|
|||||||
mjtNum tau_act = 0.2;
|
mjtNum tau_act = 0.2;
|
||||||
mjtNum tau_deact = 0.3;
|
mjtNum tau_deact = 0.3;
|
||||||
for (mjtNum dctrl : {0.0, 0.1, 0.2, 1.0, 1.1}) {
|
for (mjtNum dctrl : {0.0, 0.1, 0.2, 1.0, 1.1}) {
|
||||||
mjtNum lower = mju_muscleDynamicsTimescale(-dctrl,
|
mjtNum lower =
|
||||||
tau_act, tau_deact, tau_smooth);
|
mju_muscleDynamicsTimescale(-dctrl, tau_act, tau_deact, tau_smooth);
|
||||||
mjtNum upper = mju_muscleDynamicsTimescale(dctrl,
|
mjtNum upper =
|
||||||
tau_act, tau_deact, tau_smooth);
|
mju_muscleDynamicsTimescale(dctrl, tau_act, tau_deact, tau_smooth);
|
||||||
EXPECT_EQ(0.5 * (upper + lower), 0.5 * (tau_act + tau_deact));
|
EXPECT_EQ(0.5 * (upper + lower), 0.5 * (tau_act + tau_deact));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -250,11 +249,9 @@ TEST_F(UtilMiscTest, MjuSparseMap) {
|
|||||||
// Verify the map by checking values
|
// Verify the map by checking values
|
||||||
mjtNum mat_res_gathered[nnz_res];
|
mjtNum mat_res_gathered[nnz_res];
|
||||||
mju_gather(mat_res_gathered, mat_src, map, nnz_res);
|
mju_gather(mat_res_gathered, mat_src, map, nnz_res);
|
||||||
EXPECT_THAT(AsVector(mat_res_gathered, nnz_res),
|
EXPECT_THAT(AsVector(mat_res_gathered, nnz_res), ElementsAre(1, 3, 5, 6));
|
||||||
ElementsAre(1, 3, 5, 6));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(UtilMiscTest, MjuSparseLower2SymMap) {
|
TEST_F(UtilMiscTest, MjuSparseLower2SymMap) {
|
||||||
// nr = 3
|
// nr = 3
|
||||||
// src = [[1, 0, 0],
|
// src = [[1, 0, 0],
|
||||||
@@ -277,8 +274,8 @@ TEST_F(UtilMiscTest, MjuSparseLower2SymMap) {
|
|||||||
int map[res_nnz];
|
int map[res_nnz];
|
||||||
int cursor[nr];
|
int cursor[nr];
|
||||||
|
|
||||||
mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res,
|
mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, rowadr_src,
|
||||||
rowadr_src, rownnz_src, colind_src, cursor);
|
rownnz_src, colind_src, cursor);
|
||||||
|
|
||||||
// Expected map:
|
// Expected map:
|
||||||
// res(0,0) -> src(0,0) (k=0) => map[0] = 0
|
// res(0,0) -> src(0,0) (k=0) => map[0] = 0
|
||||||
@@ -323,8 +320,8 @@ TEST_F(UtilMiscTest, MjuSparseLower2SymMapPartial) {
|
|||||||
int map[res_nnz];
|
int map[res_nnz];
|
||||||
int cursor[nr];
|
int cursor[nr];
|
||||||
|
|
||||||
mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res,
|
mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, rowadr_src,
|
||||||
rowadr_src, rownnz_src, colind_src, cursor);
|
rownnz_src, colind_src, cursor);
|
||||||
|
|
||||||
// Expected map for the non-zeros in res:
|
// Expected map for the non-zeros in res:
|
||||||
// res(0,0) -> src(0,0) (k=0) => map[0] = 0
|
// res(0,0) -> src(0,0) (k=0) => map[0] = 0
|
||||||
@@ -404,7 +401,8 @@ TEST_F(InterpolationTest, mju_interpolate3D) {
|
|||||||
return x * y * z + y * z * z + x * z * z;
|
return x * y * z + y * z * z + x * z * z;
|
||||||
};
|
};
|
||||||
auto quadratic_function_3 = [](mjtNum x, mjtNum y, mjtNum z) {
|
auto quadratic_function_3 = [](mjtNum x, mjtNum y, mjtNum z) {
|
||||||
return x*y*z + y*z*z + x*z*z + y*y*z + x*x*z + x + y + z;
|
return x * y * z + y * z * z + x * z * z + y * y * z + x * x * z + x + y +
|
||||||
|
z;
|
||||||
};
|
};
|
||||||
static constexpr int order = 2;
|
static constexpr int order = 2;
|
||||||
mjtNum coeff[3 * (order + 1) * (order + 1) * (order + 1)];
|
mjtNum coeff[3 * (order + 1) * (order + 1) * (order + 1)];
|
||||||
@@ -956,11 +954,14 @@ TEST_F(HistoryTest, ReadVector_ExactMatch) {
|
|||||||
|
|
||||||
// set values: t=0->(1,2), t=1->(3,4), t=2->(5,6)
|
// set values: t=0->(1,2), t=1->(3,4), t=2->(5,6)
|
||||||
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
|
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
|
||||||
slot0[0] = 1.0; slot0[1] = 2.0;
|
slot0[0] = 1.0;
|
||||||
|
slot0[1] = 2.0;
|
||||||
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
|
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
|
||||||
slot1[0] = 3.0; slot1[1] = 4.0;
|
slot1[0] = 3.0;
|
||||||
|
slot1[1] = 4.0;
|
||||||
mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0);
|
mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0);
|
||||||
slot2[0] = 5.0; slot2[1] = 6.0;
|
slot2[0] = 5.0;
|
||||||
|
slot2[1] = 6.0;
|
||||||
|
|
||||||
mjtNum res[dim];
|
mjtNum res[dim];
|
||||||
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 1.0, 0);
|
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 1.0, 0);
|
||||||
@@ -980,11 +981,14 @@ TEST_F(HistoryTest, ReadVector_ZOH) {
|
|||||||
mju_zero(buf + 2 + n, n * dim);
|
mju_zero(buf + 2 + n, n * dim);
|
||||||
|
|
||||||
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
|
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
|
||||||
slot0[0] = 1.0; slot0[1] = 2.0;
|
slot0[0] = 1.0;
|
||||||
|
slot0[1] = 2.0;
|
||||||
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
|
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
|
||||||
slot1[0] = 3.0; slot1[1] = 4.0;
|
slot1[0] = 3.0;
|
||||||
|
slot1[1] = 4.0;
|
||||||
mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0);
|
mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0);
|
||||||
slot2[0] = 5.0; slot2[1] = 6.0;
|
slot2[0] = 5.0;
|
||||||
|
slot2[1] = 6.0;
|
||||||
|
|
||||||
mjtNum res[dim];
|
mjtNum res[dim];
|
||||||
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 0);
|
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 0);
|
||||||
@@ -1004,11 +1008,14 @@ TEST_F(HistoryTest, ReadVector_Linear) {
|
|||||||
mju_zero(buf + 2 + n, n * dim);
|
mju_zero(buf + 2 + n, n * dim);
|
||||||
|
|
||||||
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
|
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
|
||||||
slot0[0] = 1.0; slot0[1] = 2.0;
|
slot0[0] = 1.0;
|
||||||
|
slot0[1] = 2.0;
|
||||||
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
|
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
|
||||||
slot1[0] = 3.0; slot1[1] = 4.0;
|
slot1[0] = 3.0;
|
||||||
|
slot1[1] = 4.0;
|
||||||
mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0);
|
mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0);
|
||||||
slot2[0] = 5.0; slot2[1] = 6.0;
|
slot2[0] = 5.0;
|
||||||
|
slot2[1] = 6.0;
|
||||||
|
|
||||||
mjtNum res[dim];
|
mjtNum res[dim];
|
||||||
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 1);
|
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 1);
|
||||||
@@ -1135,9 +1142,11 @@ TEST_F(HistoryTest, CubicInterpolation) {
|
|||||||
// Dim 0: 0 -> 1. Spline: p(x) = 3x^2 - 2x^3
|
// Dim 0: 0 -> 1. Spline: p(x) = 3x^2 - 2x^3
|
||||||
// Dim 1: 1 -> 0. Spline: p(x) = 1 - 3x^2 + 2x^3
|
// Dim 1: 1 -> 0. Spline: p(x) = 1 - 3x^2 + 2x^3
|
||||||
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
|
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
|
||||||
slot0[0] = 0.0; slot0[1] = 1.0;
|
slot0[0] = 0.0;
|
||||||
|
slot0[1] = 1.0;
|
||||||
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
|
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
|
||||||
slot1[0] = 1.0; slot1[1] = 0.0;
|
slot1[0] = 1.0;
|
||||||
|
slot1[1] = 0.0;
|
||||||
|
|
||||||
mjtNum res[2];
|
mjtNum res[2];
|
||||||
|
|
||||||
@@ -1198,8 +1207,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
|
|||||||
// normal_axis=0, na0=1, na1=2
|
// normal_axis=0, na0=1, na1=2
|
||||||
{
|
{
|
||||||
int indices[4];
|
int indices[4];
|
||||||
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL,
|
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, NULL,
|
||||||
NULL, NULL, NULL, indices, NULL);
|
NULL, NULL, indices, NULL);
|
||||||
EXPECT_EQ(indices[0], gidx(0, 0, 0));
|
EXPECT_EQ(indices[0], gidx(0, 0, 0));
|
||||||
EXPECT_EQ(indices[1], gidx(0, 0, 1));
|
EXPECT_EQ(indices[1], gidx(0, 0, 1));
|
||||||
EXPECT_EQ(indices[2], gidx(0, 1, 0));
|
EXPECT_EQ(indices[2], gidx(0, 1, 0));
|
||||||
@@ -1209,8 +1218,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
|
|||||||
// face 1: x=max (fixed g[0]=1, varying g[1], g[2])
|
// face 1: x=max (fixed g[0]=1, varying g[1], g[2])
|
||||||
{
|
{
|
||||||
int indices[4];
|
int indices[4];
|
||||||
mju_flexGatherFaceState(order, cx, cy, cz, 1, xpos.data(), NULL, NULL,
|
mju_flexGatherFaceState(order, cx, cy, cz, 1, xpos.data(), NULL, NULL, NULL,
|
||||||
NULL, NULL, NULL, indices, NULL);
|
NULL, NULL, indices, NULL);
|
||||||
EXPECT_EQ(indices[0], gidx(1, 0, 0));
|
EXPECT_EQ(indices[0], gidx(1, 0, 0));
|
||||||
EXPECT_EQ(indices[1], gidx(1, 0, 1));
|
EXPECT_EQ(indices[1], gidx(1, 0, 1));
|
||||||
EXPECT_EQ(indices[2], gidx(1, 1, 0));
|
EXPECT_EQ(indices[2], gidx(1, 1, 0));
|
||||||
@@ -1222,8 +1231,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
|
|||||||
// loop order: l0→z, l1→x
|
// loop order: l0→z, l1→x
|
||||||
{
|
{
|
||||||
int indices[4];
|
int indices[4];
|
||||||
mju_flexGatherFaceState(order, cx, cy, cz, 2, xpos.data(), NULL, NULL,
|
mju_flexGatherFaceState(order, cx, cy, cz, 2, xpos.data(), NULL, NULL, NULL,
|
||||||
NULL, NULL, NULL, indices, NULL);
|
NULL, NULL, indices, NULL);
|
||||||
EXPECT_EQ(indices[0], gidx(0, 0, 0)); // l0=0(z=0), l1=0(x=0)
|
EXPECT_EQ(indices[0], gidx(0, 0, 0)); // l0=0(z=0), l1=0(x=0)
|
||||||
EXPECT_EQ(indices[1], gidx(1, 0, 0)); // l0=0(z=0), l1=1(x=1)
|
EXPECT_EQ(indices[1], gidx(1, 0, 0)); // l0=0(z=0), l1=1(x=1)
|
||||||
EXPECT_EQ(indices[2], gidx(0, 0, 1)); // l0=1(z=1), l1=0(x=0)
|
EXPECT_EQ(indices[2], gidx(0, 0, 1)); // l0=1(z=1), l1=0(x=0)
|
||||||
@@ -1234,8 +1243,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
|
|||||||
// normal_axis=1, na0=2(z slow), na1=0(x fast)
|
// normal_axis=1, na0=2(z slow), na1=0(x fast)
|
||||||
{
|
{
|
||||||
int indices[4];
|
int indices[4];
|
||||||
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL,
|
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, NULL,
|
||||||
NULL, NULL, NULL, indices, NULL);
|
NULL, NULL, indices, NULL);
|
||||||
EXPECT_EQ(indices[0], gidx(0, 1, 0)); // l0=0(z=0), l1=0(x=0)
|
EXPECT_EQ(indices[0], gidx(0, 1, 0)); // l0=0(z=0), l1=0(x=0)
|
||||||
EXPECT_EQ(indices[1], gidx(1, 1, 0)); // l0=0(z=0), l1=1(x=1)
|
EXPECT_EQ(indices[1], gidx(1, 1, 0)); // l0=0(z=0), l1=1(x=1)
|
||||||
EXPECT_EQ(indices[2], gidx(0, 1, 1)); // l0=1(z=1), l1=0(x=0)
|
EXPECT_EQ(indices[2], gidx(0, 1, 1)); // l0=1(z=1), l1=0(x=0)
|
||||||
@@ -1246,8 +1255,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
|
|||||||
// normal_axis=2, na0=0, na1=1
|
// normal_axis=2, na0=0, na1=1
|
||||||
{
|
{
|
||||||
int indices[4];
|
int indices[4];
|
||||||
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL,
|
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, NULL,
|
||||||
NULL, NULL, NULL, indices, NULL);
|
NULL, NULL, indices, NULL);
|
||||||
EXPECT_EQ(indices[0], gidx(0, 0, 0));
|
EXPECT_EQ(indices[0], gidx(0, 0, 0));
|
||||||
EXPECT_EQ(indices[1], gidx(0, 1, 0));
|
EXPECT_EQ(indices[1], gidx(0, 1, 0));
|
||||||
EXPECT_EQ(indices[2], gidx(1, 0, 0));
|
EXPECT_EQ(indices[2], gidx(1, 0, 0));
|
||||||
@@ -1257,8 +1266,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
|
|||||||
// face 5: z=max (fixed g[2]=1)
|
// face 5: z=max (fixed g[2]=1)
|
||||||
{
|
{
|
||||||
int indices[4];
|
int indices[4];
|
||||||
mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL,
|
mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL, NULL,
|
||||||
NULL, NULL, NULL, indices, NULL);
|
NULL, NULL, indices, NULL);
|
||||||
EXPECT_EQ(indices[0], gidx(0, 0, 1));
|
EXPECT_EQ(indices[0], gidx(0, 0, 1));
|
||||||
EXPECT_EQ(indices[1], gidx(0, 1, 1));
|
EXPECT_EQ(indices[1], gidx(0, 1, 1));
|
||||||
EXPECT_EQ(indices[2], gidx(1, 0, 1));
|
EXPECT_EQ(indices[2], gidx(1, 0, 1));
|
||||||
@@ -1299,8 +1308,8 @@ TEST_F(FaceStateTest, NodeIndicesMultiCell) {
|
|||||||
// na0=1, na1=2: g[0]=0, g[1]=0..1, g[2]=0..1
|
// na0=1, na1=2: g[0]=0, g[1]=0..1, g[2]=0..1
|
||||||
{
|
{
|
||||||
int indices[4];
|
int indices[4];
|
||||||
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL,
|
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, NULL,
|
||||||
NULL, NULL, NULL, indices, NULL);
|
NULL, NULL, indices, NULL);
|
||||||
EXPECT_EQ(indices[0], gidx(0, 0, 0));
|
EXPECT_EQ(indices[0], gidx(0, 0, 0));
|
||||||
EXPECT_EQ(indices[1], gidx(0, 0, 1));
|
EXPECT_EQ(indices[1], gidx(0, 0, 1));
|
||||||
EXPECT_EQ(indices[2], gidx(0, 1, 0));
|
EXPECT_EQ(indices[2], gidx(0, 1, 0));
|
||||||
@@ -1310,8 +1319,8 @@ TEST_F(FaceStateTest, NodeIndicesMultiCell) {
|
|||||||
// face 0 (x=0), quad 3: (q0=1, q1=1) → within_face = 1*2+1 = 3
|
// face 0 (x=0), quad 3: (q0=1, q1=1) → within_face = 1*2+1 = 3
|
||||||
{
|
{
|
||||||
int indices[4];
|
int indices[4];
|
||||||
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL,
|
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, NULL,
|
||||||
NULL, NULL, NULL, indices, NULL);
|
NULL, NULL, indices, NULL);
|
||||||
EXPECT_EQ(indices[0], gidx(0, 1, 1));
|
EXPECT_EQ(indices[0], gidx(0, 1, 1));
|
||||||
EXPECT_EQ(indices[1], gidx(0, 1, 2));
|
EXPECT_EQ(indices[1], gidx(0, 1, 2));
|
||||||
EXPECT_EQ(indices[2], gidx(0, 2, 1));
|
EXPECT_EQ(indices[2], gidx(0, 2, 1));
|
||||||
@@ -1322,8 +1331,8 @@ TEST_F(FaceStateTest, NodeIndicesMultiCell) {
|
|||||||
// g[0] = cx*order = 2
|
// g[0] = cx*order = 2
|
||||||
{
|
{
|
||||||
int indices[4];
|
int indices[4];
|
||||||
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL,
|
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, NULL,
|
||||||
NULL, NULL, NULL, indices, NULL);
|
NULL, NULL, indices, NULL);
|
||||||
EXPECT_EQ(indices[0], gidx(2, 0, 0));
|
EXPECT_EQ(indices[0], gidx(2, 0, 0));
|
||||||
EXPECT_EQ(indices[1], gidx(2, 0, 1));
|
EXPECT_EQ(indices[1], gidx(2, 0, 1));
|
||||||
EXPECT_EQ(indices[2], gidx(2, 1, 0));
|
EXPECT_EQ(indices[2], gidx(2, 1, 0));
|
||||||
@@ -1366,8 +1375,8 @@ TEST_F(FaceStateTest, NodeIndicesNonCubicGrid) {
|
|||||||
// Total flat index = 3 + 3 + 2 = 8
|
// Total flat index = 3 + 3 + 2 = 8
|
||||||
{
|
{
|
||||||
int indices[4];
|
int indices[4];
|
||||||
mju_flexGatherFaceState(order, cx, cy, cz, 8, xpos.data(), NULL, NULL,
|
mju_flexGatherFaceState(order, cx, cy, cz, 8, xpos.data(), NULL, NULL, NULL,
|
||||||
NULL, NULL, NULL, indices, NULL);
|
NULL, NULL, indices, NULL);
|
||||||
EXPECT_EQ(indices[0], gidx(0, 0, 1));
|
EXPECT_EQ(indices[0], gidx(0, 0, 1));
|
||||||
EXPECT_EQ(indices[1], gidx(1, 0, 1));
|
EXPECT_EQ(indices[1], gidx(1, 0, 1));
|
||||||
EXPECT_EQ(indices[2], gidx(0, 0, 2));
|
EXPECT_EQ(indices[2], gidx(0, 0, 2));
|
||||||
@@ -1681,8 +1690,7 @@ TEST_F(ShellTFITest, BoundaryUnmodified) {
|
|||||||
for (int i = 0; i < nx; i++) {
|
for (int i = 0; i < nx; i++) {
|
||||||
for (int j = 0; j < ny; j++) {
|
for (int j = 0; j < ny; j++) {
|
||||||
for (int k = 0; k < nz; k++) {
|
for (int k = 0; k < nz; k++) {
|
||||||
bool is_boundary = (i == 0 || i == nx-1 ||
|
bool is_boundary = (i == 0 || i == nx - 1 || j == 0 || j == ny - 1 ||
|
||||||
j == 0 || j == ny-1 ||
|
|
||||||
k == 0 || k == nz - 1);
|
k == 0 || k == nz - 1);
|
||||||
if (is_boundary) {
|
if (is_boundary) {
|
||||||
int idx = i * ny * nz + j * nz + k;
|
int idx = i * ny * nz + j * nz + k;
|
||||||
|
|||||||
@@ -35,13 +35,12 @@
|
|||||||
namespace mujoco {
|
namespace mujoco {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
|
|
||||||
using ::testing::Pointwise;
|
using ::testing::Pointwise;
|
||||||
|
|
||||||
using ::testing::ElementsAre;
|
using ::std::setw;
|
||||||
using ::std::string;
|
using ::std::string;
|
||||||
using ::std::vector;
|
using ::std::vector;
|
||||||
using ::std::setw;
|
using ::testing::ElementsAre;
|
||||||
using QCQP2Test = MujocoTest;
|
using QCQP2Test = MujocoTest;
|
||||||
|
|
||||||
TEST_F(QCQP2Test, DegenerateAMatrix) {
|
TEST_F(QCQP2Test, DegenerateAMatrix) {
|
||||||
@@ -174,10 +173,7 @@ void randomBoxQP(int n, mjtNum* H, mjtNum* g, mjtNum* lower, mjtNum* upper,
|
|||||||
TEST_F(BoxQPTest, UnboundedQP) {
|
TEST_F(BoxQPTest, UnboundedQP) {
|
||||||
// small arrays, allocate on stack
|
// small arrays, allocate on stack
|
||||||
static const int n = 2;
|
static const int n = 2;
|
||||||
mjtNum H[n*n] = {
|
mjtNum H[n * n] = {2, 0, 0, 2};
|
||||||
2, 0,
|
|
||||||
0, 2
|
|
||||||
};
|
|
||||||
mjtNum g[n] = {1, 3};
|
mjtNum g[n] = {1, 3};
|
||||||
mjtNum res[n] = {0, 0};
|
mjtNum res[n] = {0, 0};
|
||||||
mjtNum R[n * (n + 7)];
|
mjtNum R[n * (n + 7)];
|
||||||
@@ -209,10 +205,7 @@ TEST_F(BoxQPTest, UnboundedQP) {
|
|||||||
TEST_F(BoxQPTest, AsymmetricUpperIgnored) {
|
TEST_F(BoxQPTest, AsymmetricUpperIgnored) {
|
||||||
// small arrays, allocate on stack
|
// small arrays, allocate on stack
|
||||||
static const int n = 2;
|
static const int n = 2;
|
||||||
mjtNum H[n*n] = {
|
mjtNum H[n * n] = {1, -400, 0, 1};
|
||||||
1, -400,
|
|
||||||
0, 1
|
|
||||||
};
|
|
||||||
mjtNum g[n] = {1, 3};
|
mjtNum g[n] = {1, 3};
|
||||||
mjtNum res[n] = {0, 0};
|
mjtNum res[n] = {0, 0};
|
||||||
mjtNum lower[n] = {-2, -2};
|
mjtNum lower[n] = {-2, -2};
|
||||||
@@ -266,9 +259,9 @@ TEST_F(BoxQPTest, UpperTrianglePoisoned) {
|
|||||||
|
|
||||||
// solve with symmetric H to get the reference result
|
// solve with symmetric H to get the reference result
|
||||||
mju_zero(res, n);
|
mju_zero(res, n);
|
||||||
int nfree_ref = mju_boxQPoption(res, R, index, H, g, n, lower, upper,
|
int nfree_ref =
|
||||||
maxiter, mingrad, backtrack, minstep,
|
mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter, mingrad,
|
||||||
armijo, nullptr, 0);
|
backtrack, minstep, armijo, nullptr, 0);
|
||||||
ASSERT_GT(nfree_ref, -1);
|
ASSERT_GT(nfree_ref, -1);
|
||||||
|
|
||||||
// save reference
|
// save reference
|
||||||
@@ -285,9 +278,9 @@ TEST_F(BoxQPTest, UpperTrianglePoisoned) {
|
|||||||
|
|
||||||
// solve again; result must match because only lower triangle should be read
|
// solve again; result must match because only lower triangle should be read
|
||||||
mju_zero(res, n);
|
mju_zero(res, n);
|
||||||
int nfree_poisoned = mju_boxQPoption(res, R, index, H, g, n, lower, upper,
|
int nfree_poisoned =
|
||||||
maxiter, mingrad, backtrack, minstep,
|
mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter, mingrad,
|
||||||
armijo, nullptr, 0);
|
backtrack, minstep, armijo, nullptr, 0);
|
||||||
|
|
||||||
EXPECT_EQ(nfree_poisoned, nfree_ref);
|
EXPECT_EQ(nfree_poisoned, nfree_ref);
|
||||||
for (int i = 0; i < n; i++) {
|
for (int i = 0; i < n; i++) {
|
||||||
@@ -319,7 +312,8 @@ TEST_F(BoxQPTest, BoundedQP) {
|
|||||||
|
|
||||||
// use default options
|
// use default options
|
||||||
int maxiter = 100; // maximum number of iterations
|
int maxiter = 100; // maximum number of iterations
|
||||||
mjtNum mingrad = MjTol(1E-16, 1E-5); // minimum squared norm of (unclamped) gradient
|
mjtNum mingrad =
|
||||||
|
MjTol(1E-16, 1E-5); // minimum squared norm of (unclamped) gradient
|
||||||
mjtNum backtrack = 0.5; // backtrack factor for decreasing stepsize
|
mjtNum backtrack = 0.5; // backtrack factor for decreasing stepsize
|
||||||
mjtNum minstep = MjTol(1E-22, 1E-10); // minimum stepsize for linesearch
|
mjtNum minstep = MjTol(1E-22, 1E-10); // minimum stepsize for linesearch
|
||||||
mjtNum armijo = 0.1; // Armijo parameter
|
mjtNum armijo = 0.1; // Armijo parameter
|
||||||
@@ -328,9 +322,8 @@ TEST_F(BoxQPTest, BoundedQP) {
|
|||||||
static const int logsz = 10000;
|
static const int logsz = 10000;
|
||||||
char log[logsz];
|
char log[logsz];
|
||||||
|
|
||||||
int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper,
|
int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter,
|
||||||
maxiter, mingrad, backtrack,
|
mingrad, backtrack, minstep, armijo, log, logsz);
|
||||||
minstep, armijo, log, logsz);
|
|
||||||
|
|
||||||
// ADD_FAILURE() << log; // uncomment to print `log` to error log
|
// ADD_FAILURE() << log; // uncomment to print `log` to error log
|
||||||
|
|
||||||
@@ -404,9 +397,9 @@ TEST_F(BoxQPTest, BoundedQPvariations) {
|
|||||||
mjtNum armijo = 0.1;
|
mjtNum armijo = 0.1;
|
||||||
|
|
||||||
// solve box-QP with logging
|
// solve box-QP with logging
|
||||||
int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper,
|
int nfree =
|
||||||
maxiter, mingrad, backtrack,
|
mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter,
|
||||||
minstep, armijo, log, logsz);
|
mingrad, backtrack, minstep, armijo, log, logsz);
|
||||||
|
|
||||||
// check solution
|
// check solution
|
||||||
EXPECT_GT(nfree, -1) << log;
|
EXPECT_GT(nfree, -1) << log;
|
||||||
@@ -435,8 +428,8 @@ TEST_F(BoxQPTest, BoundedQPvariations) {
|
|||||||
// average of 4.5 factorizations is expected
|
// average of 4.5 factorizations is expected
|
||||||
EXPECT_LE(meanfactor, 5.0);
|
EXPECT_LE(meanfactor, 5.0);
|
||||||
|
|
||||||
std::cerr << "n=" << setw(3) << n
|
std::cerr << "n=" << setw(3) << n << ": average of " << meanfactor
|
||||||
<< ": average of " << meanfactor << " factorizations\n";
|
<< " factorizations\n";
|
||||||
}
|
}
|
||||||
mju_free(res);
|
mju_free(res);
|
||||||
mju_free(R);
|
mju_free(R);
|
||||||
@@ -492,7 +485,6 @@ void randomBanded(mjtNum* H, int nTotal, int nBand, int nDense, int seed,
|
|||||||
mju_free(sqrtH);
|
mju_free(sqrtH);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// test banded-vector diagonal values
|
// test banded-vector diagonal values
|
||||||
TEST_F(BandMatrixTest, Diagonal) {
|
TEST_F(BandMatrixTest, Diagonal) {
|
||||||
int seed = 1;
|
int seed = 1;
|
||||||
@@ -631,8 +623,8 @@ TEST_F(BandMatrixTest, Factorization) {
|
|||||||
EXPECT_EQ(rank, nTotal);
|
EXPECT_EQ(rank, nTotal);
|
||||||
|
|
||||||
// banded factorization
|
// banded factorization
|
||||||
mjtNum minDiag = mju_cholFactorBand(B, nTotal, nBand, nDense,
|
mjtNum minDiag =
|
||||||
diagadd, diagmul);
|
mju_cholFactorBand(B, nTotal, nBand, nDense, diagadd, diagmul);
|
||||||
|
|
||||||
// expect factorization to have succeeded
|
// expect factorization to have succeeded
|
||||||
EXPECT_GT(minDiag, 0);
|
EXPECT_GT(minDiag, 0);
|
||||||
@@ -736,8 +728,8 @@ TEST_F(BandMatrixTest, Solve) {
|
|||||||
using EngineUtilSolveTest = MujocoTest;
|
using EngineUtilSolveTest = MujocoTest;
|
||||||
|
|
||||||
TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) {
|
TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) {
|
||||||
mjModel* model = LoadModelFromString("<mujoco/>");
|
MjModelPtr model = LoadModelFromString("<mujoco/>");
|
||||||
mjData* d = mj_makeData(model);
|
MjDataPtr d = MakeData(model);
|
||||||
|
|
||||||
// Test matrix (upper triangular, representing symmetric matrix):
|
// Test matrix (upper triangular, representing symmetric matrix):
|
||||||
// [10 1 2 3]
|
// [10 1 2 3]
|
||||||
@@ -760,7 +752,7 @@ TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) {
|
|||||||
int LT_rownnz[4], LT_rowadr[4];
|
int LT_rownnz[4], LT_rowadr[4];
|
||||||
int nnz = mju_cholFactorSymbolic(nullptr, L_rownnz, L_rowadr, nullptr,
|
int nnz = mju_cholFactorSymbolic(nullptr, L_rownnz, L_rowadr, nullptr,
|
||||||
LT_rownnz, LT_rowadr, nullptr, H_rownnz,
|
LT_rownnz, LT_rowadr, nullptr, H_rownnz,
|
||||||
H_rowadr, H_colind, n, d);
|
H_rowadr, H_colind, n, d.get());
|
||||||
|
|
||||||
// verify counting phase outputs
|
// verify counting phase outputs
|
||||||
EXPECT_EQ(nnz, 8);
|
EXPECT_EQ(nnz, 8);
|
||||||
@@ -774,7 +766,8 @@ TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) {
|
|||||||
// phase 2: filling
|
// phase 2: filling
|
||||||
int L_colind[8], LT_colind[8], LT_pos[8];
|
int L_colind[8], LT_colind[8], LT_pos[8];
|
||||||
mju_cholFactorSymbolic(L_colind, L_rownnz, L_rowadr, LT_colind, LT_rownnz,
|
mju_cholFactorSymbolic(L_colind, L_rownnz, L_rowadr, LT_colind, LT_rownnz,
|
||||||
LT_rowadr, LT_pos, H_rownnz, H_rowadr, H_colind, n, d);
|
LT_rowadr, LT_pos, H_rownnz, H_rowadr, H_colind, n,
|
||||||
|
d.get());
|
||||||
|
|
||||||
// verify L_colind
|
// verify L_colind
|
||||||
EXPECT_THAT(AsVector(L_colind, 8), ElementsAre(0, 0, 1, 0, 2, 0, 2, 3));
|
EXPECT_THAT(AsVector(L_colind, 8), ElementsAre(0, 0, 1, 0, 2, 0, 2, 3));
|
||||||
@@ -801,9 +794,6 @@ TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) {
|
|||||||
<< "LT_pos mismatch at column " << c << ", entry " << k;
|
<< "LT_pos mismatch at column " << c << ", entry " << k;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Test for mju_cholUpdate: rank-one Cholesky update L*L' +/- x*x'
|
// Test for mju_cholUpdate: rank-one Cholesky update L*L' +/- x*x'
|
||||||
@@ -904,8 +894,8 @@ TEST_F(EngineUtilSolveTest, MjuCholUpdate) {
|
|||||||
// Test for mju_cholUpdateSparse: sparse rank-one Cholesky update L'*L +/- x*x'
|
// Test for mju_cholUpdateSparse: sparse rank-one Cholesky update L'*L +/- x*x'
|
||||||
// Uses sparse reverse Cholesky factorization and sparse rank-one update.
|
// Uses sparse reverse Cholesky factorization and sparse rank-one update.
|
||||||
TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) {
|
TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) {
|
||||||
mjModel* model = LoadModelFromString("<mujoco/>");
|
MjModelPtr model = LoadModelFromString("<mujoco/>");
|
||||||
mjData* d = mj_makeData(model);
|
MjDataPtr d = MakeData(model);
|
||||||
|
|
||||||
std::mt19937_64 rng;
|
std::mt19937_64 rng;
|
||||||
rng.seed(123);
|
rng.seed(123);
|
||||||
@@ -987,12 +977,12 @@ TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) {
|
|||||||
|
|
||||||
// sparse Cholesky factorization (reverse-order: L'*L)
|
// sparse Cholesky factorization (reverse-order: L'*L)
|
||||||
mju_cholFactorSparse(L_sparse.data(), n, 0, rownnz.data(), rowadr.data(),
|
mju_cholFactorSparse(L_sparse.data(), n, 0, rownnz.data(), rowadr.data(),
|
||||||
colind.data(), d);
|
colind.data(), d.get());
|
||||||
|
|
||||||
// apply sparse rank-one update
|
// apply sparse rank-one update
|
||||||
int rank_sparse = mju_cholUpdateSparse(
|
int rank_sparse = mju_cholUpdateSparse(
|
||||||
L_sparse.data(), x_sparse.data(), n, flg_plus, rownnz.data(),
|
L_sparse.data(), x_sparse.data(), n, flg_plus, rownnz.data(),
|
||||||
rowadr.data(), colind.data(), x_nnz, x_ind.data(), d);
|
rowadr.data(), colind.data(), x_nnz, x_ind.data(), d.get());
|
||||||
EXPECT_EQ(rank_sparse, n)
|
EXPECT_EQ(rank_sparse, n)
|
||||||
<< "Sparse update rank loss for n=" << n << ", flg_plus=" << flg_plus;
|
<< "Sparse update rank loss for n=" << n << ", flg_plus=" << flg_plus;
|
||||||
|
|
||||||
@@ -1014,16 +1004,13 @@ TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Test that mju_cholFactorSymbolic + mju_cholFactorNumeric produces identical
|
// Test that mju_cholFactorSymbolic + mju_cholFactorNumeric produces identical
|
||||||
// results to the reference implementation mju_cholFactorSparse
|
// results to the reference implementation mju_cholFactorSparse
|
||||||
TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
|
TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
|
||||||
mjModel* model = LoadModelFromString("<mujoco/>");
|
MjModelPtr model = LoadModelFromString("<mujoco/>");
|
||||||
mjData* d = mj_makeData(model);
|
MjDataPtr d = MakeData(model);
|
||||||
|
|
||||||
// test matrix with fill-in: upper triangle structure
|
// test matrix with fill-in: upper triangle structure
|
||||||
int n = 4;
|
int n = 4;
|
||||||
@@ -1044,13 +1031,13 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
|
|||||||
int LT_rownnz[4], LT_rowadr[4];
|
int LT_rownnz[4], LT_rowadr[4];
|
||||||
int nnz = mju_cholFactorSymbolic(nullptr, L_rownnz, L_rowadr, nullptr,
|
int nnz = mju_cholFactorSymbolic(nullptr, L_rownnz, L_rowadr, nullptr,
|
||||||
LT_rownnz, LT_rowadr, nullptr, HT_rownnz,
|
LT_rownnz, LT_rowadr, nullptr, HT_rownnz,
|
||||||
HT_rowadr, HT_colind, n, d);
|
HT_rowadr, HT_colind, n, d.get());
|
||||||
|
|
||||||
// filling phase: compute L_colind, LT_colind, and LT_pos
|
// filling phase: compute L_colind, LT_colind, and LT_pos
|
||||||
int L_colind[16], LT_colind[16], LT_pos[16];
|
int L_colind[16], LT_colind[16], LT_pos[16];
|
||||||
mju_cholFactorSymbolic(L_colind, L_rownnz, L_rowadr, LT_colind, LT_rownnz,
|
mju_cholFactorSymbolic(L_colind, L_rownnz, L_rowadr, LT_colind, LT_rownnz,
|
||||||
LT_rowadr, LT_pos, HT_rownnz, HT_rowadr, HT_colind, n,
|
LT_rowadr, LT_pos, HT_rownnz, HT_rowadr, HT_colind, n,
|
||||||
d);
|
d.get());
|
||||||
|
|
||||||
// verify LT structure matches what we'd get from a separate transpose
|
// verify LT structure matches what we'd get from a separate transpose
|
||||||
int LT_rownnz_ref[4], LT_rowadr_ref[4], LT_colind_ref[16];
|
int LT_rownnz_ref[4], LT_rowadr_ref[4], LT_colind_ref[16];
|
||||||
@@ -1080,7 +1067,7 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
|
|||||||
mjtNum L_new[16];
|
mjtNum L_new[16];
|
||||||
int rank_new = mju_cholFactorNumeric(
|
int rank_new = mju_cholFactorNumeric(
|
||||||
L_new, n, 1e-10, L_rownnz, L_rowadr, L_colind, LT_rownnz, LT_rowadr,
|
L_new, n, 1e-10, L_rownnz, L_rowadr, L_colind, LT_rownnz, LT_rowadr,
|
||||||
LT_colind, LT_pos, sparseH, H_rownnz, H_rowadr, H_colind, d);
|
LT_colind, LT_pos, sparseH, H_rownnz, H_rowadr, H_colind, d.get());
|
||||||
|
|
||||||
// reference implementation: copy sparse H into L_ref, then factor in-place
|
// reference implementation: copy sparse H into L_ref, then factor in-place
|
||||||
mjtNum L_ref[16];
|
mjtNum L_ref[16];
|
||||||
@@ -1099,7 +1086,7 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
|
|||||||
L_ref_rownnz[r] = lower_nnz;
|
L_ref_rownnz[r] = lower_nnz;
|
||||||
}
|
}
|
||||||
int rank_ref = mju_cholFactorSparse(L_ref, n, 1e-10, L_ref_rownnz, L_rowadr,
|
int rank_ref = mju_cholFactorSparse(L_ref, n, 1e-10, L_ref_rownnz, L_rowadr,
|
||||||
L_ref_colind, d);
|
L_ref_colind, d.get());
|
||||||
|
|
||||||
// compare results
|
// compare results
|
||||||
EXPECT_EQ(rank_new, rank_ref);
|
EXPECT_EQ(rank_new, rank_ref);
|
||||||
@@ -1110,9 +1097,6 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
|
|||||||
for (int i = 0; i < nnz; i++) {
|
for (int i = 0; i < nnz; i++) {
|
||||||
EXPECT_NEAR(L_new[i], L_ref[i], eps) << "mismatch at index " << i;
|
EXPECT_NEAR(L_new[i], L_ref[i], eps) << "mismatch at index " << i;
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ----------------------------- dense LU --------------------------------------
|
// ----------------------------- dense LU --------------------------------------
|
||||||
@@ -1123,10 +1107,7 @@ using DenseLUTest = MujocoTest;
|
|||||||
TEST_F(DenseLUTest, Identity) {
|
TEST_F(DenseLUTest, Identity) {
|
||||||
constexpr int n = 4;
|
constexpr int n = 4;
|
||||||
mjtNum A[n * n] = {
|
mjtNum A[n * n] = {
|
||||||
1, 0, 0, 0,
|
1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1,
|
||||||
0, 1, 0, 0,
|
|
||||||
0, 0, 1, 0,
|
|
||||||
0, 0, 0, 1,
|
|
||||||
};
|
};
|
||||||
int pivot[n];
|
int pivot[n];
|
||||||
mjtNum b[n] = {1, 2, 3, 4};
|
mjtNum b[n] = {1, 2, 3, 4};
|
||||||
@@ -1146,9 +1127,7 @@ TEST_F(DenseLUTest, SmallKnown) {
|
|||||||
// A = [2 1 1; 4 3 3; 8 7 9], b = [1; 1; 1]
|
// A = [2 1 1; 4 3 3; 8 7 9], b = [1; 1; 1]
|
||||||
// solution: x = [1; -1; 0] (verified: A*x = [2-1; 4-3; 8-7] = [1;1;1])
|
// solution: x = [1; -1; 0] (verified: A*x = [2-1; 4-3; 8-7] = [1;1;1])
|
||||||
mjtNum A[n * n] = {
|
mjtNum A[n * n] = {
|
||||||
2, 1, 1,
|
2, 1, 1, 4, 3, 3, 8, 7, 9,
|
||||||
4, 3, 3,
|
|
||||||
8, 7, 9,
|
|
||||||
};
|
};
|
||||||
int pivot[n];
|
int pivot[n];
|
||||||
mjtNum b[n] = {1, 1, 1};
|
mjtNum b[n] = {1, 1, 1};
|
||||||
@@ -1202,8 +1181,7 @@ TEST_F(DenseLUTest, RandomSPD) {
|
|||||||
// compare
|
// compare
|
||||||
mjtNum eps = MjTol(1e-15, 1e-7);
|
mjtNum eps = MjTol(1e-15, 1e-7);
|
||||||
EXPECT_THAT(AsVector(x_lu.data(), n),
|
EXPECT_THAT(AsVector(x_lu.data(), n),
|
||||||
Pointwise(MjNear(eps, eps),
|
Pointwise(MjNear(eps, eps), AsVector(x_chol.data(), n)));
|
||||||
AsVector(x_chol.data(), n)));
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -44,9 +44,7 @@ class MjvSceneTest : public MujocoTest {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void FreeSceneObjects() {
|
void FreeSceneObjects() { mjv_freeScene(&scn_); }
|
||||||
mjv_freeScene(&scn_);
|
|
||||||
}
|
|
||||||
|
|
||||||
mjvScene scn_;
|
mjvScene scn_;
|
||||||
mjvOption opt_;
|
mjvOption opt_;
|
||||||
@@ -80,7 +78,6 @@ TEST_F(MjvSceneTest, UpdateScene) {
|
|||||||
|
|
||||||
mj_deleteData(data_copy);
|
mj_deleteData(data_copy);
|
||||||
mj_deleteData(data);
|
mj_deleteData(data);
|
||||||
|
|
||||||
FreeSceneObjects();
|
FreeSceneObjects();
|
||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
}
|
}
|
||||||
@@ -101,7 +98,6 @@ TEST_F(MjvSceneTest, UpdateSceneGeomsExhausted) {
|
|||||||
mjv_updateScene(model, data, &opt_, &pert_, &cam_, mjCAT_ALL, &scn_);
|
mjv_updateScene(model, data, &opt_, &pert_, &cam_, mjCAT_ALL, &scn_);
|
||||||
EXPECT_EQ(scn_.status, 1);
|
EXPECT_EQ(scn_.status, 1);
|
||||||
EXPECT_EQ(scn_.ngeom, maxgeoms);
|
EXPECT_EQ(scn_.ngeom, maxgeoms);
|
||||||
|
|
||||||
mj_deleteData(data);
|
mj_deleteData(data);
|
||||||
FreeSceneObjects();
|
FreeSceneObjects();
|
||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
@@ -118,17 +114,17 @@ TEST_F(MjvSceneTest, PrincipalPointFrustumSign) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
|
|
||||||
mjModel* model = LoadModelFromString(xml);
|
MjModelPtr model = LoadModelFromString(xml);
|
||||||
ASSERT_THAT(model, NotNull());
|
ASSERT_THAT(model.get(), NotNull());
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
mj_forward(model, data);
|
mj_forward(model.get(), data.get());
|
||||||
|
|
||||||
InitSceneObjects(model);
|
InitSceneObjects(model.get());
|
||||||
|
|
||||||
// point camera at the fixed cam
|
// point camera at the fixed cam
|
||||||
cam_.type = mjCAMERA_FIXED;
|
cam_.type = mjCAMERA_FIXED;
|
||||||
cam_.fixedcamid = 0;
|
cam_.fixedcamid = 0;
|
||||||
mjv_updateCamera(model, data, &cam_, &scn_);
|
mjv_updateCamera(model.get(), data.get(), &cam_, &scn_);
|
||||||
|
|
||||||
float top = scn_.camera[0].frustum_top;
|
float top = scn_.camera[0].frustum_top;
|
||||||
float bottom = scn_.camera[0].frustum_bottom;
|
float bottom = scn_.camera[0].frustum_bottom;
|
||||||
@@ -148,9 +144,7 @@ TEST_F(MjvSceneTest, PrincipalPointFrustumSign) {
|
|||||||
EXPECT_FLOAT_EQ(top, half - offset);
|
EXPECT_FLOAT_EQ(top, half - offset);
|
||||||
EXPECT_FLOAT_EQ(bottom, -(half + offset));
|
EXPECT_FLOAT_EQ(bottom, -(half + offset));
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
FreeSceneObjects();
|
FreeSceneObjects();
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
+6
-2
@@ -157,7 +157,7 @@ const std::string GetModelPath(std::string_view path) { // NOLINT
|
|||||||
return absl::StrCat("../model/", path);
|
return absl::StrCat("../model/", path);
|
||||||
}
|
}
|
||||||
|
|
||||||
mjModel* LoadModelFromString(std::string_view xml, char* error,
|
MjModelPtr LoadModelFromString(std::string_view xml, char* error,
|
||||||
int error_size, mjVFS* vfs) {
|
int error_size, mjVFS* vfs) {
|
||||||
if (error) {
|
if (error) {
|
||||||
error[0] = '\0';
|
error[0] = '\0';
|
||||||
@@ -197,7 +197,11 @@ mjModel* LoadModelFromString(std::string_view xml, char* error,
|
|||||||
}
|
}
|
||||||
|
|
||||||
SetGlobalXmlSpec(spec);
|
SetGlobalXmlSpec(spec);
|
||||||
return model;
|
return MjModelPtr(model);
|
||||||
|
}
|
||||||
|
|
||||||
|
MjDataPtr MakeData(const MjModelPtr& model) {
|
||||||
|
return MjDataPtr(mj_makeData(model.get()));
|
||||||
}
|
}
|
||||||
|
|
||||||
static void AssertModelNotNull(mjModel* model,
|
static void AssertModelNotNull(mjModel* model,
|
||||||
|
|||||||
+19
-1
@@ -25,6 +25,7 @@
|
|||||||
#include <string>
|
#include <string>
|
||||||
#include <string_view>
|
#include <string_view>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
#include <memory>
|
||||||
|
|
||||||
#include <gmock/gmock.h>
|
#include <gmock/gmock.h>
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
@@ -39,6 +40,20 @@ MJAPI mjfLogHandler _mjPRIVATE_setTlsLogHandler(mjfLogHandler handler);
|
|||||||
|
|
||||||
namespace mujoco {
|
namespace mujoco {
|
||||||
|
|
||||||
|
struct MjModelDeleter {
|
||||||
|
void operator()(mjModel* m) const {
|
||||||
|
mj_deleteModel(m);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
using MjModelPtr = std::unique_ptr<mjModel, MjModelDeleter>;
|
||||||
|
|
||||||
|
struct MjDataDeleter {
|
||||||
|
void operator()(mjData* d) const {
|
||||||
|
mj_deleteData(d);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
using MjDataPtr = std::unique_ptr<mjData, MjDataDeleter>;
|
||||||
|
|
||||||
// Runtime scale factor for test tolerances, controlled by MJTOL_SCALE env var.
|
// Runtime scale factor for test tolerances, controlled by MJTOL_SCALE env var.
|
||||||
// Set MJTOL_SCALE=0 to run tests with zero tolerance and see actual residuals.
|
// Set MJTOL_SCALE=0 to run tests with zero tolerance and see actual residuals.
|
||||||
inline mjtNum MjTolScale() {
|
inline mjtNum MjTolScale() {
|
||||||
@@ -189,9 +204,12 @@ const std::string GetModelPath(std::string_view path);
|
|||||||
|
|
||||||
// Returns a newly-allocated mjModel, loaded from the contents of xml.
|
// Returns a newly-allocated mjModel, loaded from the contents of xml.
|
||||||
// On failure returns nullptr and populates the error array if present.
|
// On failure returns nullptr and populates the error array if present.
|
||||||
mjModel* LoadModelFromString(std::string_view xml, char* error = nullptr,
|
MjModelPtr LoadModelFromString(std::string_view xml, char* error = nullptr,
|
||||||
int error_size = 0, mjVFS* vfs = nullptr);
|
int error_size = 0, mjVFS* vfs = nullptr);
|
||||||
|
|
||||||
|
// Returns a newly-allocated mjData, initialized using model.
|
||||||
|
MjDataPtr MakeData(const MjModelPtr& model);
|
||||||
|
|
||||||
// Returns a newly-allocated mjModel, loaded from the contents in model_path.
|
// Returns a newly-allocated mjModel, loaded from the contents in model_path.
|
||||||
// On failure it asserts that model is null.
|
// On failure it asserts that model is null.
|
||||||
mjModel* LoadModelFromPath(const char* model_path);
|
mjModel* LoadModelFromPath(const char* model_path);
|
||||||
|
|||||||
@@ -65,23 +65,21 @@ TEST_F(PidTest, PGain) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
|
|
||||||
|
|
||||||
EXPECT_EQ(m->plugin_statenum[0], 0)
|
EXPECT_EQ(m->plugin_statenum[0], 0)
|
||||||
<< "Plugin should have no state variables";
|
<< "Plugin should have no state variables";
|
||||||
EXPECT_EQ(m->actuator_actnum[0], 0)
|
EXPECT_EQ(m->actuator_actnum[0], 0)
|
||||||
<< "Plugin should have no activation variables";
|
<< "Plugin should have no activation variables";
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
|
|
||||||
|
|
||||||
// apply the same ctrl to both actuators and see if the same qpos results
|
// apply the same ctrl to both actuators and see if the same qpos results
|
||||||
d->ctrl[0] = 1.0;
|
d->ctrl[0] = 1.0;
|
||||||
d->ctrl[1] = 1.0;
|
d->ctrl[1] = 1.0;
|
||||||
|
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
|
|
||||||
EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]);
|
EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]);
|
||||||
EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]);
|
EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]);
|
||||||
@@ -120,24 +118,22 @@ TEST_F(PidTest, PGainWithFilterExact) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
|
|
||||||
|
|
||||||
EXPECT_EQ(m->plugin_statenum[0], 0)
|
EXPECT_EQ(m->plugin_statenum[0], 0)
|
||||||
<< "Plugin should have no state variables";
|
<< "Plugin should have no state variables";
|
||||||
EXPECT_EQ(m->actuator_actnum[0], 1)
|
EXPECT_EQ(m->actuator_actnum[0], 1)
|
||||||
<< "Plugin should have one activation variable";
|
<< "Plugin should have one activation variable";
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
|
|
||||||
|
|
||||||
// apply the same ctrl to both actuators and see if the same qpos results
|
// apply the same ctrl to both actuators and see if the same qpos results
|
||||||
d->ctrl[0] = 1.0;
|
d->ctrl[0] = 1.0;
|
||||||
d->ctrl[1] = 1.0;
|
d->ctrl[1] = 1.0;
|
||||||
|
|
||||||
for (int i = 0; i < 2; i++) {
|
for (int i = 0; i < 2; i++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
|
|
||||||
EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]);
|
EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]);
|
||||||
EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]);
|
EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]);
|
||||||
@@ -178,27 +174,25 @@ TEST_F(PidTest, SlewMaxRate) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
|
|
||||||
|
|
||||||
// having a slew rate means that there should be one extra state variable
|
// having a slew rate means that there should be one extra state variable
|
||||||
// for the plugin.
|
// for the plugin.
|
||||||
EXPECT_EQ(m->actuator_actnum[0], 1);
|
EXPECT_EQ(m->actuator_actnum[0], 1);
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
|
|
||||||
|
|
||||||
// going from ctrl=0.0 to ctrl=1.0 immediately should be equivalent to slowly
|
// going from ctrl=0.0 to ctrl=1.0 immediately should be equivalent to slowly
|
||||||
// incrementing the setpoint
|
// incrementing the setpoint
|
||||||
d->ctrl[0] = d->ctrl[1] = 0.0;
|
d->ctrl[0] = d->ctrl[1] = 0.0;
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
|
|
||||||
mjtNum max_slew_rate = 0.75;
|
mjtNum max_slew_rate = 0.75;
|
||||||
for (int i = 0; i < 2; i++) {
|
for (int i = 0; i < 2; i++) {
|
||||||
d->ctrl[0] = 1.0;
|
d->ctrl[0] = 1.0;
|
||||||
d->ctrl[1] = d->time * max_slew_rate;
|
d->ctrl[1] = d->time * max_slew_rate;
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
|
|
||||||
EXPECT_EQ(d->actuator_force[0], d->actuator_force[1])
|
EXPECT_EQ(d->actuator_force[0], d->actuator_force[1])
|
||||||
<< "actuator_force mismatch at step " << i;
|
<< "actuator_force mismatch at step " << i;
|
||||||
@@ -246,9 +240,8 @@ TEST_F(PidTest, IntegratedVelocitySlewMaxRate) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
|
|
||||||
|
|
||||||
// having a slew rate means that there should be one extra state variable
|
// having a slew rate means that there should be one extra state variable
|
||||||
// for the plugin.
|
// for the plugin.
|
||||||
@@ -256,22 +249,20 @@ TEST_F(PidTest, IntegratedVelocitySlewMaxRate) {
|
|||||||
// The integrated-velocity controller should have one activation variable too.
|
// The integrated-velocity controller should have one activation variable too.
|
||||||
ASSERT_EQ(m->actuator_actnum[1], 1);
|
ASSERT_EQ(m->actuator_actnum[1], 1);
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
|
|
||||||
|
|
||||||
// going from ctrl=0.0 to ctrl=1.0 immediately should be equivalent to using
|
// going from ctrl=0.0 to ctrl=1.0 immediately should be equivalent to using
|
||||||
// an integrated-velocity controller, with actearly = true
|
// an integrated-velocity controller, with actearly = true
|
||||||
d->ctrl[0] = d->ctrl[1] = 0.0;
|
d->ctrl[0] = d->ctrl[1] = 0.0;
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
|
|
||||||
mjtNum max_slew_rate = 0.75;
|
mjtNum max_slew_rate = 0.75;
|
||||||
for (int i = 0; i < 2; i++) {
|
for (int i = 0; i < 2; i++) {
|
||||||
d->ctrl[0] = 1.0;
|
d->ctrl[0] = 1.0;
|
||||||
d->ctrl[1] = max_slew_rate;
|
d->ctrl[1] = max_slew_rate;
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
|
|
||||||
EXPECT_EQ(d->act[0], d->act[1])
|
EXPECT_EQ(d->act[0], d->act[1]) << "act mismatch at step " << i;
|
||||||
<< "act mismatch at step " << i;
|
|
||||||
EXPECT_EQ(d->actuator_force[0], d->actuator_force[1])
|
EXPECT_EQ(d->actuator_force[0], d->actuator_force[1])
|
||||||
<< "actuator_force mismatch at step " << i;
|
<< "actuator_force mismatch at step " << i;
|
||||||
EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1])
|
EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1])
|
||||||
@@ -308,16 +299,14 @@ TEST_F(PidTest, SlewMaxRateUsesFirstCtrl) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
|
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
|
|
||||||
|
|
||||||
// When starting with ctrl = 1.0, there shouldn't be a slew rate restriction
|
// When starting with ctrl = 1.0, there shouldn't be a slew rate restriction
|
||||||
d->ctrl[0] = 1.0;
|
d->ctrl[0] = 1.0;
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
EXPECT_EQ(d->actuator_force[0], 4.0);
|
EXPECT_EQ(d->actuator_force[0], 4.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -374,17 +363,15 @@ TEST_F(PidTest, ITerm) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
|
|
||||||
|
|
||||||
// only the actuators with an I term should have state.
|
// only the actuators with an I term should have state.
|
||||||
EXPECT_EQ(m->actuator_actnum[0], 0);
|
EXPECT_EQ(m->actuator_actnum[0], 0);
|
||||||
EXPECT_EQ(m->actuator_actnum[1], 1);
|
EXPECT_EQ(m->actuator_actnum[1], 1);
|
||||||
EXPECT_EQ(m->actuator_actnum[2], 1);
|
EXPECT_EQ(m->actuator_actnum[2], 1);
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
|
|
||||||
|
|
||||||
// when applying a constant 1.0 control for a while:
|
// when applying a constant 1.0 control for a while:
|
||||||
// - the PD controller should settle to 1.0 - m*g / kp
|
// - the PD controller should settle to 1.0 - m*g / kp
|
||||||
@@ -392,7 +379,7 @@ TEST_F(PidTest, ITerm) {
|
|||||||
// - the clamped PID controller should reach 1.0 - (m*g - imax) / kp
|
// - the clamped PID controller should reach 1.0 - (m*g - imax) / kp
|
||||||
d->ctrl[0] = d->ctrl[1] = d->ctrl[2] = 1.0;
|
d->ctrl[0] = d->ctrl[1] = d->ctrl[2] = 1.0;
|
||||||
for (int i = 0; i < 10000; i++) {
|
for (int i = 0; i < 10000; i++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
EXPECT_THAT(d->qpos[0], MjNear(1.0 - 10 / 40.0, 1e-5, 1e-5));
|
EXPECT_THAT(d->qpos[0], MjNear(1.0 - 10 / 40.0, 1e-5, 1e-5));
|
||||||
@@ -423,19 +410,17 @@ TEST_F(PidTest, FiniteDifferencing) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
|
|
||||||
|
|
||||||
// actuators with an I term and max slew rate should have 2 activation
|
// actuators with an I term and max slew rate should have 2 activation
|
||||||
// variables.
|
// variables.
|
||||||
ASSERT_EQ(m->actuator_actnum[0], 2);
|
ASSERT_EQ(m->actuator_actnum[0], 2);
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
|
|
||||||
|
|
||||||
d->ctrl[0] = 1.0;
|
d->ctrl[0] = 1.0;
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
EXPECT_NE(d->act[0], 0);
|
EXPECT_NE(d->act[0], 0);
|
||||||
mjtNum state_before_finite_differencing = d->act[0];
|
mjtNum state_before_finite_differencing = d->act[0];
|
||||||
|
|
||||||
@@ -443,8 +428,8 @@ TEST_F(PidTest, FiniteDifferencing) {
|
|||||||
std::vector<mjtNum> B((2 * m->nv + m->na) * m->nu, 0);
|
std::vector<mjtNum> B((2 * m->nv + m->na) * m->nu, 0);
|
||||||
std::vector<mjtNum> C((m->nsensordata) * (2 * m->nv + m->na), 0);
|
std::vector<mjtNum> C((m->nsensordata) * (2 * m->nv + m->na), 0);
|
||||||
std::vector<mjtNum> D((m->nsensordata) * m->nu, 0);
|
std::vector<mjtNum> D((m->nsensordata) * m->nu, 0);
|
||||||
mjd_transitionFD(m, d, /*eps=*/1e-3, /*flg_centered=*/true, A.data(),
|
mjd_transitionFD(m.get(), d.get(), /*eps=*/1e-3, /*flg_centered=*/true,
|
||||||
B.data(), C.data(), D.data());
|
A.data(), B.data(), C.data(), D.data());
|
||||||
|
|
||||||
EXPECT_EQ(d->act[0], state_before_finite_differencing)
|
EXPECT_EQ(d->act[0], state_before_finite_differencing)
|
||||||
<< "mjd_transitionFD should not change actuator plugin state.";
|
<< "mjd_transitionFD should not change actuator plugin state.";
|
||||||
@@ -481,25 +466,23 @@ TEST_F(PidTest, CtrlClamp) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
|
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
|
|
||||||
|
|
||||||
// when applying a constant 1.0 control for a while, control should be clamped
|
// when applying a constant 1.0 control for a while, control should be clamped
|
||||||
// to 0.75, and the body should reach that point.
|
// to 0.75, and the body should reach that point.
|
||||||
d->ctrl[0] = 1.0;
|
d->ctrl[0] = 1.0;
|
||||||
for (int i = 0; i < 10000; i++) {
|
for (int i = 0; i < 10000; i++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
}
|
}
|
||||||
EXPECT_THAT(d->qpos[0], MjNear(0.75, 1e-5, 1e-5));
|
EXPECT_THAT(d->qpos[0], MjNear(0.75, 1e-5, 1e-5));
|
||||||
|
|
||||||
// when applying 0, it should be clamped to 0.25
|
// when applying 0, it should be clamped to 0.25
|
||||||
d->ctrl[0] = 0.0;
|
d->ctrl[0] = 0.0;
|
||||||
for (int i = 0; i < 10000; i++) {
|
for (int i = 0; i < 10000; i++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
}
|
}
|
||||||
EXPECT_THAT(d->qpos[0], MjNear(0.25, 1e-5, 1e-5));
|
EXPECT_THAT(d->qpos[0], MjNear(0.25, 1e-5, 1e-5));
|
||||||
}
|
}
|
||||||
@@ -544,23 +527,21 @@ TEST_F(PidTest, CopyData) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
|
|
||||||
|
|
||||||
mjData* d1 = mj_makeData(m);
|
MjDataPtr d1 = MakeData(m);
|
||||||
absl::Cleanup d1_deleter = [d1] { mj_deleteData(d1); };
|
|
||||||
|
|
||||||
d1->ctrl[0] = d1->ctrl[1] = -1.0;
|
d1->ctrl[0] = d1->ctrl[1] = -1.0;
|
||||||
|
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
mj_step(m, d1);
|
mj_step(m.get(), d1.get());
|
||||||
}
|
}
|
||||||
mjData* d2 = mj_copyData(nullptr, m, d1);
|
mjData* d2 = mj_copyData(nullptr, m.get(), d1.get());
|
||||||
absl::Cleanup d2_deleter = [d2] { mj_deleteData(d2); };
|
absl::Cleanup d2_deleter = [d2] { mj_deleteData(d2); };
|
||||||
|
|
||||||
mj_step(m, d1);
|
mj_step(m.get(), d1.get());
|
||||||
mj_step(m, d2);
|
mj_step(m.get(), d2);
|
||||||
|
|
||||||
EXPECT_EQ(d1->qpos[0], d2->qpos[0]);
|
EXPECT_EQ(d1->qpos[0], d2->qpos[0]);
|
||||||
EXPECT_EQ(d1->qpos[1], d2->qpos[1]);
|
EXPECT_EQ(d1->qpos[1], d2->qpos[1]);
|
||||||
@@ -599,17 +580,15 @@ TEST_F(PidTest, MultipleActuatorsSamePlugin) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
|
|
||||||
|
|
||||||
// having a slew rate means that there should be one extra state variable
|
// having a slew rate means that there should be one extra state variable
|
||||||
// for the plugin.
|
// for the plugin.
|
||||||
EXPECT_EQ(m->actuator_actnum[0], 1);
|
EXPECT_EQ(m->actuator_actnum[0], 1);
|
||||||
EXPECT_EQ(m->actuator_actnum[1], 1);
|
EXPECT_EQ(m->actuator_actnum[1], 1);
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
|
|
||||||
|
|
||||||
// Set different ctrls for the two actuators, and check that they're
|
// Set different ctrls for the two actuators, and check that they're
|
||||||
// independent.
|
// independent.
|
||||||
@@ -617,7 +596,7 @@ TEST_F(PidTest, MultipleActuatorsSamePlugin) {
|
|||||||
d->ctrl[1] = -1.0;
|
d->ctrl[1] = -1.0;
|
||||||
|
|
||||||
for (int i = 0; i < 2; i++) {
|
for (int i = 0; i < 2; i++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
|
|
||||||
EXPECT_EQ(d->actuator_force[0], -d->actuator_force[1])
|
EXPECT_EQ(d->actuator_force[0], -d->actuator_force[1])
|
||||||
<< "actuator_force mismatch at step " << i;
|
<< "actuator_force mismatch at step " << i;
|
||||||
@@ -649,8 +628,8 @@ TEST_F(PidTest, InvalidClamp) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
|
||||||
EXPECT_THAT(m, IsNull());
|
EXPECT_THAT(m.get(), IsNull());
|
||||||
|
|
||||||
EXPECT_THAT(std::string_view(error), HasSubstr("plugin"));
|
EXPECT_THAT(std::string_view(error), HasSubstr("plugin"));
|
||||||
EXPECT_THAT(std::string_view(error), HasSubstr("imax"));
|
EXPECT_THAT(std::string_view(error), HasSubstr("imax"));
|
||||||
@@ -680,8 +659,8 @@ TEST_F(PidTest, InvalidSlew) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, IsNull());
|
ASSERT_THAT(m.get(), IsNull());
|
||||||
|
|
||||||
EXPECT_THAT(std::string_view(error), HasSubstr("plugin"));
|
EXPECT_THAT(std::string_view(error), HasSubstr("plugin"));
|
||||||
EXPECT_THAT(std::string_view(error), HasSubstr("slewmax"));
|
EXPECT_THAT(std::string_view(error), HasSubstr("slewmax"));
|
||||||
@@ -717,24 +696,25 @@ TEST_F(PidTest, WrongActdim) {
|
|||||||
{
|
{
|
||||||
std::string no_actdim =
|
std::string no_actdim =
|
||||||
absl::StrReplaceAll(kBaseXml, {{"PLACEHOLDER", ""}});
|
absl::StrReplaceAll(kBaseXml, {{"PLACEHOLDER", ""}});
|
||||||
mjModel* m = LoadModelFromString(no_actdim, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(no_actdim, error, sizeof(error));
|
||||||
EXPECT_THAT(m, IsNull());
|
EXPECT_THAT(m.get(), IsNull());
|
||||||
EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"1\""));
|
EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"1\""));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
std::string big_actdim =
|
std::string big_actdim =
|
||||||
absl::StrReplaceAll(kBaseXml, {{"PLACEHOLDER", "actdim=\"2\""}});
|
absl::StrReplaceAll(kBaseXml, {{"PLACEHOLDER", "actdim=\"2\""}});
|
||||||
mjModel* m = LoadModelFromString(big_actdim, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(big_actdim, error, sizeof(error));
|
||||||
EXPECT_THAT(m, IsNull());
|
EXPECT_THAT(m.get(), IsNull());
|
||||||
EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"1\""));
|
EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"1\""));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
std::string dyntype_integrator = absl::StrReplaceAll(
|
std::string dyntype_integrator = absl::StrReplaceAll(
|
||||||
kBaseXml, {{"PLACEHOLDER", "dyntype=\"integrator\" actdim=\"1\""}});
|
kBaseXml, {{"PLACEHOLDER", "dyntype=\"integrator\" actdim=\"1\""}});
|
||||||
mjModel* m = LoadModelFromString(dyntype_integrator, error, sizeof(error));
|
MjModelPtr m =
|
||||||
EXPECT_THAT(m, IsNull());
|
LoadModelFromString(dyntype_integrator, error, sizeof(error));
|
||||||
|
EXPECT_THAT(m.get(), IsNull());
|
||||||
EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"2\""));
|
EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"2\""));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -769,9 +749,8 @@ TEST_F(PidTest, Keyframe) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, NotNull()) << error;
|
ASSERT_THAT(m.get(), NotNull()) << error;
|
||||||
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
|
|
||||||
|
|
||||||
// having a slew rate means that there should be one extra state variable
|
// having a slew rate means that there should be one extra state variable
|
||||||
// for the plugin.
|
// for the plugin.
|
||||||
@@ -780,10 +759,9 @@ TEST_F(PidTest, Keyframe) {
|
|||||||
ASSERT_EQ(m->nkey, 1);
|
ASSERT_EQ(m->nkey, 1);
|
||||||
EXPECT_EQ(m->key_act[0], 1.0);
|
EXPECT_EQ(m->key_act[0], 1.0);
|
||||||
|
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
|
|
||||||
|
|
||||||
mj_resetDataKeyframe(m, d, 0);
|
mj_resetDataKeyframe(m.get(), d.get(), 0);
|
||||||
EXPECT_EQ(d->act[0], 1.0);
|
EXPECT_EQ(d->act[0], 1.0);
|
||||||
}
|
}
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
@@ -28,7 +28,6 @@ namespace {
|
|||||||
|
|
||||||
using ElasticityTest = MujocoTest;
|
using ElasticityTest = MujocoTest;
|
||||||
|
|
||||||
|
|
||||||
// -------------------------------- cable -----------------------------------
|
// -------------------------------- cable -----------------------------------
|
||||||
TEST_F(ElasticityTest, CantileverIntoCircle) {
|
TEST_F(ElasticityTest, CantileverIntoCircle) {
|
||||||
#ifdef mjUSESINGLE
|
#ifdef mjUSESINGLE
|
||||||
@@ -66,25 +65,24 @@ TEST_F(ElasticityTest, CantileverIntoCircle) {
|
|||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
|
|
||||||
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
|
|
||||||
// see Oliver Weeger, Sai-Kit Yeung, Martin L. Dunn, "Isogeometric collocation methods for Cosserat rods and rod
|
// see Oliver Weeger, Sai-Kit Yeung, Martin L. Dunn, "Isogeometric collocation
|
||||||
// structures", section 7.1 (DOI: j.cma.2016.05.009), the torque for achieving an angle phi is phi * E * Iy.
|
// methods for Cosserat rods and rod structures", section 7.1 (DOI:
|
||||||
|
// j.cma.2016.05.009), the torque for achieving an angle phi is phi * E * Iy.
|
||||||
mjtNum Iy = mjPI * pow(0.005, 4) / 4;
|
mjtNum Iy = mjPI * pow(0.005, 4) / 4;
|
||||||
mjtNum torque = 2 * mjPI * 1e9 * Iy;
|
mjtNum torque = 2 * mjPI * 1e9 * Iy;
|
||||||
for (int i = 0; i < 1300; i++) {
|
for (int i = 0; i < 1300; i++) {
|
||||||
if (i < 300) {
|
if (i < 300) {
|
||||||
d->ctrl[0] += torque / 300;
|
d->ctrl[0] += torque / 300;
|
||||||
}
|
}
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
}
|
}
|
||||||
EXPECT_NEAR(d->sensordata[0], 0, std::numeric_limits<float>::epsilon());
|
EXPECT_NEAR(d->sensordata[0], 0, std::numeric_limits<float>::epsilon());
|
||||||
EXPECT_NEAR(d->sensordata[1], 0, std::numeric_limits<float>::epsilon());
|
EXPECT_NEAR(d->sensordata[1], 0, std::numeric_limits<float>::epsilon());
|
||||||
EXPECT_NEAR(d->sensordata[2], 1, std::numeric_limits<float>::epsilon());
|
EXPECT_NEAR(d->sensordata[2], 1, std::numeric_limits<float>::epsilon());
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ElasticityTest, InvalidTxtAttribute) {
|
TEST_F(ElasticityTest, InvalidTxtAttribute) {
|
||||||
@@ -106,8 +104,8 @@ TEST_F(ElasticityTest, InvalidTxtAttribute) {
|
|||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
|
|
||||||
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::IsNull());
|
ASSERT_THAT(m.get(), testing::IsNull());
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ElasticityTest, InvalidMixedAttribute) {
|
TEST_F(ElasticityTest, InvalidMixedAttribute) {
|
||||||
@@ -129,8 +127,8 @@ TEST_F(ElasticityTest, InvalidMixedAttribute) {
|
|||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
|
|
||||||
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::IsNull());
|
ASSERT_THAT(m.get(), testing::IsNull());
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ElasticityTest, ValidAttributes) {
|
TEST_F(ElasticityTest, ValidAttributes) {
|
||||||
@@ -152,9 +150,8 @@ TEST_F(ElasticityTest, ValidAttributes) {
|
|||||||
|
|
||||||
char error[1024] = {0};
|
char error[1024] = {0};
|
||||||
|
|
||||||
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error;
|
ASSERT_THAT(m.get(), testing::NotNull()) << error;
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
+128
-143
@@ -24,12 +24,12 @@
|
|||||||
#include <string>
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
#include <absl/strings/str_format.h>
|
|
||||||
#include <gmock/gmock.h>
|
#include <gmock/gmock.h>
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <mujoco/mujoco.h>
|
#include <absl/strings/str_format.h>
|
||||||
#include <mujoco/mjspec.h>
|
|
||||||
#include <mujoco/mjplugin.h>
|
#include <mujoco/mjplugin.h>
|
||||||
|
#include <mujoco/mjspec.h>
|
||||||
|
#include <mujoco/mujoco.h>
|
||||||
#include "src/xml/xml_api.h"
|
#include "src/xml/xml_api.h"
|
||||||
#include "test/compare_model.h"
|
#include "test/compare_model.h"
|
||||||
#include "test/fixture.h"
|
#include "test/fixture.h"
|
||||||
@@ -228,7 +228,6 @@ TEST_F(MujocoTest, OriginSpecInvariantToAttachment) {
|
|||||||
mjs_findElement(parent_spec, mjOBJ_BODY, "parent_body");
|
mjs_findElement(parent_spec, mjOBJ_BODY, "parent_body");
|
||||||
EXPECT_EQ(mjs_getOriginSpec(parent_spec_body), parent_spec);
|
EXPECT_EQ(mjs_getOriginSpec(parent_spec_body), parent_spec);
|
||||||
|
|
||||||
|
|
||||||
mj_deleteSpec(child_spec);
|
mj_deleteSpec(child_spec);
|
||||||
mj_deleteSpec(parent_spec);
|
mj_deleteSpec(parent_spec);
|
||||||
}
|
}
|
||||||
@@ -404,10 +403,9 @@ TEST_F(MujocoTest, SetToDCMotorNullable) {
|
|||||||
double motorconst[2] = {0.05, 0.05};
|
double motorconst[2] = {0.05, 0.05};
|
||||||
double resistance = 2.0;
|
double resistance = 2.0;
|
||||||
|
|
||||||
const char* err = mjs_setToDCMotor(actuator, motorconst, resistance,
|
const char* err =
|
||||||
nullptr, nullptr, nullptr,
|
mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, nullptr,
|
||||||
nullptr, nullptr, nullptr,
|
nullptr, nullptr, nullptr, nullptr, nullptr, 0);
|
||||||
nullptr, 0);
|
|
||||||
EXPECT_STREQ(err, "");
|
EXPECT_STREQ(err, "");
|
||||||
EXPECT_EQ(actuator->gainprm[0], 2.0);
|
EXPECT_EQ(actuator->gainprm[0], 2.0);
|
||||||
EXPECT_EQ(actuator->gainprm[1], 0.05);
|
EXPECT_EQ(actuator->gainprm[1], 0.05);
|
||||||
@@ -427,10 +425,9 @@ TEST_F(MujocoTest, SetToDCMotorDeriveKe) {
|
|||||||
double resistance = 2.0;
|
double resistance = 2.0;
|
||||||
double nominal[3] = {12.0, 0, 100.0}; // vn=12, omega0=100
|
double nominal[3] = {12.0, 0, 100.0}; // vn=12, omega0=100
|
||||||
|
|
||||||
const char* err = mjs_setToDCMotor(actuator, nullptr, resistance,
|
const char* err =
|
||||||
nominal, nullptr, nullptr,
|
mjs_setToDCMotor(actuator, nullptr, resistance, nominal, nullptr, nullptr,
|
||||||
nullptr, nullptr, nullptr,
|
nullptr, nullptr, nullptr, nullptr, 0);
|
||||||
nullptr, 0);
|
|
||||||
EXPECT_STREQ(err, "");
|
EXPECT_STREQ(err, "");
|
||||||
EXPECT_EQ(actuator->gainprm[0], 2.0);
|
EXPECT_EQ(actuator->gainprm[0], 2.0);
|
||||||
EXPECT_NEAR(actuator->gainprm[1], 0.12, 1e-5);
|
EXPECT_NEAR(actuator->gainprm[1], 0.12, 1e-5);
|
||||||
@@ -447,10 +444,9 @@ TEST_F(MujocoTest, SetToDCMotorFull) {
|
|||||||
double saturation[3] = {1.0, 2.0, 3.0};
|
double saturation[3] = {1.0, 2.0, 3.0};
|
||||||
double controller[6] = {10.0, 20.0, 30.0, 40.0, 50.0, 60.0};
|
double controller[6] = {10.0, 20.0, 30.0, 40.0, 50.0, 60.0};
|
||||||
|
|
||||||
const char* err = mjs_setToDCMotor(actuator, motorconst, resistance,
|
const char* err =
|
||||||
nullptr, saturation, nullptr,
|
mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, saturation,
|
||||||
nullptr, controller, nullptr,
|
nullptr, nullptr, controller, nullptr, nullptr, 0);
|
||||||
nullptr, 0);
|
|
||||||
EXPECT_STREQ(err, "");
|
EXPECT_STREQ(err, "");
|
||||||
EXPECT_EQ(actuator->gainprm[0], 2.0); // resistance
|
EXPECT_EQ(actuator->gainprm[0], 2.0); // resistance
|
||||||
EXPECT_EQ(actuator->gainprm[1], 0.05); // K
|
EXPECT_EQ(actuator->gainprm[1], 0.05); // K
|
||||||
@@ -473,10 +469,9 @@ TEST_F(MujocoTest, SetToDCMotorLuGre) {
|
|||||||
double resistance = 2.0;
|
double resistance = 2.0;
|
||||||
double lugre[5] = {100.0, 1.0, 0.5, 0.7, 10.0};
|
double lugre[5] = {100.0, 1.0, 0.5, 0.7, 10.0};
|
||||||
|
|
||||||
const char* err = mjs_setToDCMotor(actuator, motorconst, resistance,
|
const char* err =
|
||||||
nullptr, nullptr, nullptr,
|
mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, nullptr,
|
||||||
nullptr, nullptr, nullptr,
|
nullptr, nullptr, nullptr, nullptr, lugre, 0);
|
||||||
lugre, 0);
|
|
||||||
EXPECT_STREQ(err, "");
|
EXPECT_STREQ(err, "");
|
||||||
EXPECT_EQ(actuator->dynprm[5], 100.0); // stiffness
|
EXPECT_EQ(actuator->dynprm[5], 100.0); // stiffness
|
||||||
EXPECT_EQ(actuator->dynprm[6], 1.0); // damping
|
EXPECT_EQ(actuator->dynprm[6], 1.0); // damping
|
||||||
@@ -764,7 +759,6 @@ TEST_F(MujocoTest, ModifyShellInertiaFails) {
|
|||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(MujocoTest, RecompileEdit) {
|
TEST_F(MujocoTest, RecompileEdit) {
|
||||||
static constexpr char xml[] = R"(
|
static constexpr char xml[] = R"(
|
||||||
<mujoco>
|
<mujoco>
|
||||||
@@ -837,43 +831,40 @@ TEST_F(MujocoTest, RecompileCompareObjCache) {
|
|||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
|
|
||||||
// load model once
|
// load model once
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
|
MjModelPtr m =
|
||||||
|
LoadModelFromString(xml, error.data(), error.size(), vfs.get());
|
||||||
EXPECT_EQ(m->mesh_vert[0], -0.5);
|
EXPECT_EQ(m->mesh_vert[0], -0.5);
|
||||||
mj_deleteModel(m);
|
|
||||||
|
|
||||||
// update cube.obj, load again
|
// update cube.obj, load again
|
||||||
mj_deleteFileVFS(vfs.get(), "cube.obj");
|
mj_deleteFileVFS(vfs.get(), "cube.obj");
|
||||||
mj_addBufferVFS(vfs.get(), "cube.obj", cube2, sizeof(cube2));
|
mj_addBufferVFS(vfs.get(), "cube.obj", cube2, sizeof(cube2));
|
||||||
m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
|
m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
|
||||||
EXPECT_EQ(m->mesh_vert[0], -1);
|
EXPECT_EQ(m->mesh_vert[0], -1);
|
||||||
mj_deleteModel(m);
|
|
||||||
|
|
||||||
mj_deleteVFS(vfs.get());
|
mj_deleteVFS(vfs.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
// tiny RGB 2 x 3 PNG file
|
// tiny RGB 2 x 3 PNG file
|
||||||
static constexpr uint8_t tex1[] = {
|
static constexpr uint8_t tex1[] = {
|
||||||
0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00, 0x0d,
|
0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00,
|
||||||
0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x02,
|
0x0d, 0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
|
||||||
0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d, 0x00, 0x00, 0x00,
|
0x00, 0x02, 0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d,
|
||||||
0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63, 0x78, 0xc1, 0xc0, 0xc0,
|
0x00, 0x00, 0x00, 0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63,
|
||||||
0xc0, 0xf0, 0xbf, 0xb8, 0xb8, 0x98, 0x81, 0xe1, 0x3f, 0xc3, 0xff, 0xff,
|
0x78, 0xc1, 0xc0, 0xc0, 0xc0, 0xf0, 0xbf, 0xb8, 0xb8, 0x98, 0x81,
|
||||||
0xff, 0xc5, 0xc4, 0xc4, 0x00, 0x46, 0xd7, 0x07, 0x7f, 0xd2, 0x52, 0xa1,
|
0xe1, 0x3f, 0xc3, 0xff, 0xff, 0xff, 0xc5, 0xc4, 0xc4, 0x00, 0x46,
|
||||||
0x41, 0x00, 0x00, 0x00, 0x00, 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60,
|
0xd7, 0x07, 0x7f, 0xd2, 0x52, 0xa1, 0x41, 0x00, 0x00, 0x00, 0x00,
|
||||||
0x82
|
0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60, 0x82};
|
||||||
};
|
|
||||||
|
|
||||||
// previous PNG file, but rotated by 180 degrees
|
// previous PNG file, but rotated by 180 degrees
|
||||||
static constexpr uint8_t tex2[] = {
|
static constexpr uint8_t tex2[] = {
|
||||||
0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00, 0x0d,
|
0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00,
|
||||||
0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x02,
|
0x0d, 0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
|
||||||
0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d, 0x00, 0x00, 0x00,
|
0x00, 0x02, 0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d,
|
||||||
0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63, 0x10, 0x13, 0x13, 0xfb,
|
0x00, 0x00, 0x00, 0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63,
|
||||||
0xff, 0xff, 0x3f, 0xc3, 0x7f, 0x06, 0x96, 0xd8, 0xd8, 0x58, 0x46, 0x46,
|
0x10, 0x13, 0x13, 0xfb, 0xff, 0xff, 0x3f, 0xc3, 0x7f, 0x06, 0x96,
|
||||||
0x86, 0x17, 0x0c, 0x0c, 0x00, 0x49, 0x22, 0x06, 0x44, 0xe4, 0x91, 0xb8,
|
0xd8, 0xd8, 0x58, 0x46, 0x46, 0x86, 0x17, 0x0c, 0x0c, 0x00, 0x49,
|
||||||
0x83, 0x00, 0x00, 0x00, 0x00, 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60,
|
0x22, 0x06, 0x44, 0xe4, 0x91, 0xb8, 0x83, 0x00, 0x00, 0x00, 0x00,
|
||||||
0x82
|
0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60, 0x82};
|
||||||
};
|
|
||||||
|
|
||||||
TEST_F(MujocoTest, RecompileComparePngCache) {
|
TEST_F(MujocoTest, RecompileComparePngCache) {
|
||||||
static constexpr char xml[] = R"(
|
static constexpr char xml[] = R"(
|
||||||
@@ -896,10 +887,10 @@ TEST_F(MujocoTest, RecompileComparePngCache) {
|
|||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
|
|
||||||
// load model once
|
// load model once
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
|
MjModelPtr m =
|
||||||
|
LoadModelFromString(xml, error.data(), error.size(), vfs.get());
|
||||||
EXPECT_EQ(m->ntexdata, 18); // w x h x rgb = 3 x 2 x 3
|
EXPECT_EQ(m->ntexdata, 18); // w x h x rgb = 3 x 2 x 3
|
||||||
mjtByte byte = m->tex_data[0];
|
mjtByte byte = m->tex_data[0];
|
||||||
mj_deleteModel(m);
|
|
||||||
|
|
||||||
// update tex.png, load again
|
// update tex.png, load again
|
||||||
mj_deleteFileVFS(vfs.get(), "tex.png");
|
mj_deleteFileVFS(vfs.get(), "tex.png");
|
||||||
@@ -907,7 +898,6 @@ TEST_F(MujocoTest, RecompileComparePngCache) {
|
|||||||
m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
|
m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
|
||||||
EXPECT_NE(m->tex_data[0], byte);
|
EXPECT_NE(m->tex_data[0], byte);
|
||||||
EXPECT_EQ(m->tex_data[15], byte); // first pixel is now last pixel
|
EXPECT_EQ(m->tex_data[15], byte); // first pixel is now last pixel
|
||||||
mj_deleteModel(m);
|
|
||||||
|
|
||||||
mj_deleteVFS(vfs.get());
|
mj_deleteVFS(vfs.get());
|
||||||
}
|
}
|
||||||
@@ -937,14 +927,13 @@ TEST_F(MujocoTest, DisableCache) {
|
|||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
|
|
||||||
// load model once
|
// load model once
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
|
MjModelPtr m =
|
||||||
|
LoadModelFromString(xml, error.data(), error.size(), vfs.get());
|
||||||
|
|
||||||
EXPECT_EQ(mj_getCacheSize(cache), 0);
|
EXPECT_EQ(mj_getCacheSize(cache), 0);
|
||||||
EXPECT_EQ(m->ntexdata, 18); // w x h x rgb = 3 x 2 x 3
|
EXPECT_EQ(m->ntexdata, 18); // w x h x rgb = 3 x 2 x 3
|
||||||
|
|
||||||
mj_setCacheCapacity(cache, capacity);
|
mj_setCacheCapacity(cache, capacity);
|
||||||
|
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteVFS(vfs.get());
|
mj_deleteVFS(vfs.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1226,16 +1215,16 @@ TEST_F(MujocoTest, AttachSame) {
|
|||||||
EXPECT_THAT(m_attached->body_parentid[4], 0);
|
EXPECT_THAT(m_attached->body_parentid[4], 0);
|
||||||
|
|
||||||
// compare with expected XML
|
// compare with expected XML
|
||||||
mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
|
MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
|
||||||
EXPECT_THAT(m_expected, NotNull()) << er.data();
|
EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
|
||||||
EXPECT_LE(CompareModel(m_attached, m_expected, field), tol)
|
EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol)
|
||||||
<< "Expected and attached models are different!\n"
|
<< "Expected and attached models are different!\n"
|
||||||
<< "Different field: " << field << '\n';;
|
<< "Different field: " << field << '\n';
|
||||||
|
;
|
||||||
|
|
||||||
// destroy everything
|
// destroy everything
|
||||||
mj_deleteSpec(parent);
|
mj_deleteSpec(parent);
|
||||||
mj_deleteModel(m_attached);
|
mj_deleteModel(m_attached);
|
||||||
mj_deleteModel(m_expected);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, AttachSpatialTendonWithoutSidesite) {
|
TEST_F(MujocoTest, AttachSpatialTendonWithoutSidesite) {
|
||||||
@@ -1283,13 +1272,15 @@ TEST_F(MujocoTest, AttachSpatialTendonWithoutSidesite) {
|
|||||||
mjsSite* attach_site = mjs_addSite(parent_body, 0);
|
mjsSite* attach_site = mjs_addSite(parent_body, 0);
|
||||||
mjs_setName(attach_site->element, "attach_site");
|
mjs_setName(attach_site->element, "attach_site");
|
||||||
|
|
||||||
mjs_attach(attach_site->element,
|
mjs_attach(attach_site->element, mjs_findBody(child, "child_body")->element,
|
||||||
mjs_findBody(child, "child_body")->element, "", "_child");
|
"", "_child");
|
||||||
|
|
||||||
EXPECT_THAT(mjs_findElement(parent, mjOBJ_TENDON,
|
EXPECT_THAT(
|
||||||
"tendon_with_sidesite_child"), NotNull());
|
mjs_findElement(parent, mjOBJ_TENDON, "tendon_with_sidesite_child"),
|
||||||
EXPECT_THAT(mjs_findElement(parent, mjOBJ_TENDON,
|
NotNull());
|
||||||
"tendon_without_sidesite_child"), NotNull());
|
EXPECT_THAT(
|
||||||
|
mjs_findElement(parent, mjOBJ_TENDON, "tendon_without_sidesite_child"),
|
||||||
|
NotNull());
|
||||||
|
|
||||||
mjModel* model = mj_compile(parent, nullptr);
|
mjModel* model = mj_compile(parent, nullptr);
|
||||||
ASSERT_THAT(model, NotNull()) << mjs_getError(parent);
|
ASSERT_THAT(model, NotNull()) << mjs_getError(parent);
|
||||||
@@ -1335,11 +1326,9 @@ TEST_F(MujocoTest, AttachSpatialTendonGitHubIssue3119) {
|
|||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
std::array<char, 1000> er;
|
std::array<char, 1000> er;
|
||||||
mjSpec* parent_spec =
|
mjSpec* parent_spec = mj_parseXMLString(parent_xml, 0, er.data(), er.size());
|
||||||
mj_parseXMLString(parent_xml, 0, er.data(), er.size());
|
|
||||||
ASSERT_THAT(parent_spec, NotNull()) << er.data();
|
ASSERT_THAT(parent_spec, NotNull()) << er.data();
|
||||||
mjSpec* child_spec =
|
mjSpec* child_spec = mj_parseXMLString(child_xml, 0, er.data(), er.size());
|
||||||
mj_parseXMLString(child_xml, 0, er.data(), er.size());
|
|
||||||
ASSERT_THAT(child_spec, NotNull()) << er.data();
|
ASSERT_THAT(child_spec, NotNull()) << er.data();
|
||||||
|
|
||||||
mjsBody* parent_body = mjs_findBody(parent_spec, "parent_body");
|
mjsBody* parent_body = mjs_findBody(parent_spec, "parent_body");
|
||||||
@@ -1348,13 +1337,14 @@ TEST_F(MujocoTest, AttachSpatialTendonGitHubIssue3119) {
|
|||||||
mjs_setName(attach_site->element, "attach_site");
|
mjs_setName(attach_site->element, "attach_site");
|
||||||
|
|
||||||
mjs_attach(attach_site->element,
|
mjs_attach(attach_site->element,
|
||||||
mjs_findBody(child_spec, "child_body")->element,
|
mjs_findBody(child_spec, "child_body")->element, "", "_child");
|
||||||
"", "_child");
|
|
||||||
|
|
||||||
|
EXPECT_THAT(
|
||||||
|
mjs_findElement(parent_spec, mjOBJ_TENDON, "tendon_with_sidesite_child"),
|
||||||
|
NotNull());
|
||||||
EXPECT_THAT(mjs_findElement(parent_spec, mjOBJ_TENDON,
|
EXPECT_THAT(mjs_findElement(parent_spec, mjOBJ_TENDON,
|
||||||
"tendon_with_sidesite_child"), NotNull());
|
"tendon_without_sidesite_child"),
|
||||||
EXPECT_THAT(mjs_findElement(parent_spec, mjOBJ_TENDON,
|
NotNull());
|
||||||
"tendon_without_sidesite_child"), NotNull());
|
|
||||||
|
|
||||||
mjModel* model = mj_compile(parent_spec, nullptr);
|
mjModel* model = mj_compile(parent_spec, nullptr);
|
||||||
ASSERT_THAT(model, NotNull()) << mjs_getError(parent_spec);
|
ASSERT_THAT(model, NotNull()) << mjs_getError(parent_spec);
|
||||||
@@ -1498,17 +1488,17 @@ TEST_F(MujocoTest, AttachDifferent) {
|
|||||||
EXPECT_THAT(mjs_findDefault(parent, "attached-cylinder-1"), NotNull());
|
EXPECT_THAT(mjs_findDefault(parent, "attached-cylinder-1"), NotNull());
|
||||||
|
|
||||||
// compare with expected XML
|
// compare with expected XML
|
||||||
mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
|
MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
|
||||||
EXPECT_THAT(m_expected, NotNull()) << er.data();
|
EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
|
||||||
EXPECT_LE(CompareModel(m_attached, m_expected, field), tol)
|
EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol)
|
||||||
<< "Expected and attached models are different!\n"
|
<< "Expected and attached models are different!\n"
|
||||||
<< "Different field: " << field << '\n';;
|
<< "Different field: " << field << '\n';
|
||||||
|
;
|
||||||
|
|
||||||
// destroy everything
|
// destroy everything
|
||||||
mj_deleteSpec(parent);
|
mj_deleteSpec(parent);
|
||||||
mj_deleteSpec(child);
|
mj_deleteSpec(child);
|
||||||
mj_deleteModel(m_attached);
|
mj_deleteModel(m_attached);
|
||||||
mj_deleteModel(m_expected);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, AttachFrame) {
|
TEST_F(MujocoTest, AttachFrame) {
|
||||||
@@ -1633,17 +1623,17 @@ TEST_F(MujocoTest, AttachFrame) {
|
|||||||
EXPECT_THAT(m_attached->body_parentid[2], 1);
|
EXPECT_THAT(m_attached->body_parentid[2], 1);
|
||||||
|
|
||||||
// compare with expected XML
|
// compare with expected XML
|
||||||
mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
|
MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
|
||||||
EXPECT_THAT(m_expected, NotNull()) << er.data();
|
EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
|
||||||
EXPECT_LE(CompareModel(m_attached, m_expected, field), tol)
|
EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol)
|
||||||
<< "Expected and attached models are different!\n"
|
<< "Expected and attached models are different!\n"
|
||||||
<< "Different field: " << field << '\n';;
|
<< "Different field: " << field << '\n';
|
||||||
|
;
|
||||||
|
|
||||||
// destroy everything
|
// destroy everything
|
||||||
mj_deleteSpec(parent);
|
mj_deleteSpec(parent);
|
||||||
mj_deleteSpec(child);
|
mj_deleteSpec(child);
|
||||||
mj_deleteModel(m_attached);
|
mj_deleteModel(m_attached);
|
||||||
mj_deleteModel(m_expected);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, AttachCompiled) {
|
TEST_F(MujocoTest, AttachCompiled) {
|
||||||
@@ -1691,8 +1681,8 @@ TEST_F(MujocoTest, AttachCompiled) {
|
|||||||
|
|
||||||
// check that attached model can be compiled
|
// check that attached model can be compiled
|
||||||
mjModel* m_attached = mj_compile(parent, 0);
|
mjModel* m_attached = mj_compile(parent, 0);
|
||||||
EXPECT_THAT(m_attached, NotNull()) <<
|
EXPECT_THAT(m_attached, NotNull())
|
||||||
"Failed to compile attached model" << mjs_getError(parent);
|
<< "Failed to compile attached model" << mjs_getError(parent);
|
||||||
|
|
||||||
// destroy everything
|
// destroy everything
|
||||||
mj_deleteSpec(parent);
|
mj_deleteSpec(parent);
|
||||||
@@ -1766,16 +1756,15 @@ void TestDetachBody(bool compile) {
|
|||||||
EXPECT_THAT(m_detached, NotNull());
|
EXPECT_THAT(m_detached, NotNull());
|
||||||
|
|
||||||
// compare with expected XML
|
// compare with expected XML
|
||||||
mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
|
MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
|
||||||
EXPECT_THAT(m_expected, NotNull()) << er.data();
|
EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
|
||||||
EXPECT_LE(CompareModel(m_detached, m_expected, field), tol)
|
EXPECT_LE(CompareModel(m_detached, m_expected.get(), field), tol)
|
||||||
<< "Expected and attached models are different!\n"
|
<< "Expected and attached models are different!\n"
|
||||||
<< "Different field: " << field << '\n';
|
<< "Different field: " << field << '\n';
|
||||||
|
|
||||||
// destroy everything
|
// destroy everything
|
||||||
mj_deleteSpec(child);
|
mj_deleteSpec(child);
|
||||||
mj_deleteModel(m_detached);
|
mj_deleteModel(m_detached);
|
||||||
mj_deleteModel(m_expected);
|
|
||||||
if (m_child) mj_deleteModel(m_child);
|
if (m_child) mj_deleteModel(m_child);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1836,16 +1825,15 @@ TEST_F(MujocoTest, AttachToSite) {
|
|||||||
|
|
||||||
mjModel* model = mj_compile(parent, 0);
|
mjModel* model = mj_compile(parent, 0);
|
||||||
EXPECT_THAT(model, NotNull());
|
EXPECT_THAT(model, NotNull());
|
||||||
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
|
MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
|
||||||
EXPECT_THAT(expected, NotNull()) << er.data();
|
EXPECT_THAT(expected.get(), NotNull()) << er.data();
|
||||||
EXPECT_LE(CompareModel(model, expected, field), tol)
|
EXPECT_LE(CompareModel(model, expected.get(), field), tol)
|
||||||
<< "Expected and attached models are different!\n"
|
<< "Expected and attached models are different!\n"
|
||||||
<< "Different field: " << field << '\n';
|
<< "Different field: " << field << '\n';
|
||||||
|
|
||||||
mj_deleteSpec(parent);
|
mj_deleteSpec(parent);
|
||||||
mj_deleteSpec(child);
|
mj_deleteSpec(child);
|
||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
mj_deleteModel(expected);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, AttachFrameToSite) {
|
TEST_F(MujocoTest, AttachFrameToSite) {
|
||||||
@@ -1902,16 +1890,15 @@ TEST_F(MujocoTest, AttachFrameToSite) {
|
|||||||
|
|
||||||
mjModel* model = mj_compile(parent, 0);
|
mjModel* model = mj_compile(parent, 0);
|
||||||
EXPECT_THAT(model, NotNull());
|
EXPECT_THAT(model, NotNull());
|
||||||
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
|
MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
|
||||||
EXPECT_THAT(expected, NotNull()) << er.data();
|
EXPECT_THAT(expected.get(), NotNull()) << er.data();
|
||||||
EXPECT_LE(CompareModel(model, expected, field), tol)
|
EXPECT_LE(CompareModel(model, expected.get(), field), tol)
|
||||||
<< "Expected and attached models are different!\n"
|
<< "Expected and attached models are different!\n"
|
||||||
<< "Different field: " << field << '\n';
|
<< "Different field: " << field << '\n';
|
||||||
|
|
||||||
mj_deleteSpec(parent);
|
mj_deleteSpec(parent);
|
||||||
mj_deleteSpec(child);
|
mj_deleteSpec(child);
|
||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
mj_deleteModel(expected);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, BodyToFrame) {
|
TEST_F(MujocoTest, BodyToFrame) {
|
||||||
@@ -1991,9 +1978,9 @@ TEST_F(MujocoTest, BodyToFrame) {
|
|||||||
// compile and compare
|
// compile and compare
|
||||||
mjModel* model2 = mj_compile(parent, 0);
|
mjModel* model2 = mj_compile(parent, 0);
|
||||||
EXPECT_THAT(model2, NotNull());
|
EXPECT_THAT(model2, NotNull());
|
||||||
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
|
MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
|
||||||
EXPECT_THAT(expected, NotNull()) << er.data();
|
EXPECT_THAT(expected.get(), NotNull()) << er.data();
|
||||||
EXPECT_LE(CompareModel(model2, expected, field), tol)
|
EXPECT_LE(CompareModel(model2, expected.get(), field), tol)
|
||||||
<< "Expected and attached models are different!\n"
|
<< "Expected and attached models are different!\n"
|
||||||
<< "Different field: " << field << '\n';
|
<< "Different field: " << field << '\n';
|
||||||
|
|
||||||
@@ -2002,7 +1989,6 @@ TEST_F(MujocoTest, BodyToFrame) {
|
|||||||
mj_deleteSpec(child2);
|
mj_deleteSpec(child2);
|
||||||
mj_deleteModel(model1);
|
mj_deleteModel(model1);
|
||||||
mj_deleteModel(model2);
|
mj_deleteModel(model2);
|
||||||
mj_deleteModel(expected);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, BodyToFrameWithInertial) {
|
TEST_F(MujocoTest, BodyToFrameWithInertial) {
|
||||||
@@ -2100,9 +2086,9 @@ TEST_F(MujocoTest, AttachSpecToSite) {
|
|||||||
// compile and compare
|
// compile and compare
|
||||||
mjModel* model = mj_compile(parent, 0);
|
mjModel* model = mj_compile(parent, 0);
|
||||||
EXPECT_THAT(model, NotNull());
|
EXPECT_THAT(model, NotNull());
|
||||||
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
|
MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
|
||||||
EXPECT_THAT(expected, NotNull()) << er.data();
|
EXPECT_THAT(expected.get(), NotNull()) << er.data();
|
||||||
EXPECT_LE(CompareModel(model, expected, field), tol)
|
EXPECT_LE(CompareModel(model, expected.get(), field), tol)
|
||||||
<< "Expected and attached models are different!\n"
|
<< "Expected and attached models are different!\n"
|
||||||
<< "Different field: " << field << '\n';
|
<< "Different field: " << field << '\n';
|
||||||
|
|
||||||
@@ -2113,7 +2099,6 @@ TEST_F(MujocoTest, AttachSpecToSite) {
|
|||||||
mj_deleteSpec(parent);
|
mj_deleteSpec(parent);
|
||||||
mj_deleteSpec(child);
|
mj_deleteSpec(child);
|
||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
mj_deleteModel(expected);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, AttachSpecToBody) {
|
TEST_F(MujocoTest, AttachSpecToBody) {
|
||||||
@@ -2181,9 +2166,9 @@ TEST_F(MujocoTest, AttachSpecToBody) {
|
|||||||
// compile and compare
|
// compile and compare
|
||||||
mjModel* model = mj_compile(parent, 0);
|
mjModel* model = mj_compile(parent, 0);
|
||||||
EXPECT_THAT(model, NotNull());
|
EXPECT_THAT(model, NotNull());
|
||||||
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
|
MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
|
||||||
EXPECT_THAT(expected, NotNull()) << er.data();
|
EXPECT_THAT(expected.get(), NotNull()) << er.data();
|
||||||
EXPECT_LE(CompareModel(model, expected, field), tol)
|
EXPECT_LE(CompareModel(model, expected.get(), field), tol)
|
||||||
<< "Expected and attached models are different!\n"
|
<< "Expected and attached models are different!\n"
|
||||||
<< "Different field: " << field << '\n';
|
<< "Different field: " << field << '\n';
|
||||||
|
|
||||||
@@ -2194,7 +2179,6 @@ TEST_F(MujocoTest, AttachSpecToBody) {
|
|||||||
mj_deleteSpec(parent);
|
mj_deleteSpec(parent);
|
||||||
mj_deleteSpec(child);
|
mj_deleteSpec(child);
|
||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
mj_deleteModel(expected);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, PreserveState) {
|
TEST_F(MujocoTest, PreserveState) {
|
||||||
@@ -2246,31 +2230,32 @@ TEST_F(MujocoTest, PreserveState) {
|
|||||||
// compile models
|
// compile models
|
||||||
mjModel* model = mj_compile(spec, 0);
|
mjModel* model = mj_compile(spec, 0);
|
||||||
EXPECT_THAT(model, NotNull());
|
EXPECT_THAT(model, NotNull());
|
||||||
mjModel* m_expected = LoadModelFromString(xml_expected, er.data(), er.size());
|
MjModelPtr m_expected =
|
||||||
EXPECT_THAT(m_expected, NotNull());
|
LoadModelFromString(xml_expected, er.data(), er.size());
|
||||||
|
EXPECT_THAT(m_expected.get(), NotNull());
|
||||||
|
|
||||||
// create data
|
// create data
|
||||||
mjData* data = mj_makeData(model);
|
mjData* data = mj_makeData(model);
|
||||||
EXPECT_THAT(data, NotNull());
|
EXPECT_THAT(data, NotNull());
|
||||||
mjData* d_expected = mj_makeData(m_expected);
|
MjDataPtr d_expected = MakeData(m_expected);
|
||||||
EXPECT_THAT(d_expected, NotNull());
|
EXPECT_THAT(d_expected.get(), NotNull());
|
||||||
|
|
||||||
// set ctrl
|
// set ctrl
|
||||||
data->ctrl[0] = 1;
|
data->ctrl[0] = 1;
|
||||||
data->ctrl[1] = 2;
|
data->ctrl[1] = 2;
|
||||||
d_expected->ctrl[0] = 2;
|
d_expected.get()->ctrl[0] = 2;
|
||||||
|
|
||||||
// set mocap
|
// set mocap
|
||||||
data->mocap_pos[3] = 1;
|
data->mocap_pos[3] = 1;
|
||||||
data->mocap_quat[4] = 0;
|
data->mocap_quat[4] = 0;
|
||||||
data->mocap_quat[5] = 1;
|
data->mocap_quat[5] = 1;
|
||||||
d_expected->mocap_pos[0] = 1;
|
d_expected.get()->mocap_pos[0] = 1;
|
||||||
d_expected->mocap_quat[0] = 0;
|
d_expected.get()->mocap_quat[0] = 0;
|
||||||
d_expected->mocap_quat[1] = 1;
|
d_expected.get()->mocap_quat[1] = 1;
|
||||||
|
|
||||||
// step models
|
// step models
|
||||||
mj_step(model, data);
|
mj_step(model, data);
|
||||||
mj_step(m_expected, d_expected);
|
mj_step(m_expected.get(), d_expected.get());
|
||||||
EXPECT_THAT(data->time, model->opt.timestep);
|
EXPECT_THAT(data->time, model->opt.timestep);
|
||||||
|
|
||||||
// detach subtree
|
// detach subtree
|
||||||
@@ -2298,7 +2283,7 @@ TEST_F(MujocoTest, PreserveState) {
|
|||||||
joint->axis[0] = 0;
|
joint->axis[0] = 0;
|
||||||
joint->axis[1] = 0;
|
joint->axis[1] = 0;
|
||||||
joint->axis[2] = 1;
|
joint->axis[2] = 1;
|
||||||
joint->ref = d_expected->qpos[m_expected->nq-1];
|
joint->ref = d_expected.get()->qpos[m_expected->nq - 1];
|
||||||
|
|
||||||
// compile new model
|
// compile new model
|
||||||
mj_recompile(spec, 0, model, data);
|
mj_recompile(spec, 0, model, data);
|
||||||
@@ -2308,13 +2293,13 @@ TEST_F(MujocoTest, PreserveState) {
|
|||||||
// compare qpos
|
// compare qpos
|
||||||
EXPECT_EQ(model->nq, m_expected->nq);
|
EXPECT_EQ(model->nq, m_expected->nq);
|
||||||
for (int i = 0; i < model->nq; ++i) {
|
for (int i = 0; i < model->nq; ++i) {
|
||||||
EXPECT_EQ(data->qpos[i], d_expected->qpos[i]) << i;
|
EXPECT_EQ(data->qpos[i], d_expected.get()->qpos[i]) << i;
|
||||||
}
|
}
|
||||||
|
|
||||||
// compare qvel
|
// compare qvel
|
||||||
EXPECT_EQ(model->nv, m_expected->nv);
|
EXPECT_EQ(model->nv, m_expected->nv);
|
||||||
for (int i = 0; i < model->nv - 1; ++i) {
|
for (int i = 0; i < model->nv - 1; ++i) {
|
||||||
EXPECT_EQ(data->qvel[i], d_expected->qvel[i]) << i;
|
EXPECT_EQ(data->qvel[i], d_expected.get()->qvel[i]) << i;
|
||||||
}
|
}
|
||||||
|
|
||||||
// second body was added after stepping so qvel should be zero
|
// second body was added after stepping so qvel should be zero
|
||||||
@@ -2323,17 +2308,21 @@ TEST_F(MujocoTest, PreserveState) {
|
|||||||
// compare act
|
// compare act
|
||||||
EXPECT_EQ(model->na, m_expected->na);
|
EXPECT_EQ(model->na, m_expected->na);
|
||||||
for (int i = 0; i < model->na; ++i) {
|
for (int i = 0; i < model->na; ++i) {
|
||||||
EXPECT_EQ(data->act[i], d_expected->act[i]) << i;
|
EXPECT_EQ(data->act[i], d_expected.get()->act[i]) << i;
|
||||||
}
|
}
|
||||||
|
|
||||||
// compare mocap
|
// compare mocap
|
||||||
EXPECT_EQ(model->nmocap, m_expected->nmocap);
|
EXPECT_EQ(model->nmocap, m_expected->nmocap);
|
||||||
for (int i = 0; i < model->nmocap; ++i) {
|
for (int i = 0; i < model->nmocap; ++i) {
|
||||||
for (int j = 0; j < 3; ++j) {
|
for (int j = 0; j < 3; ++j) {
|
||||||
EXPECT_EQ(data->mocap_pos[3*i+j], d_expected->mocap_pos[3*i+j]) << i;
|
EXPECT_EQ(data->mocap_pos[3 * i + j],
|
||||||
|
d_expected.get()->mocap_pos[3 * i + j])
|
||||||
|
<< i;
|
||||||
}
|
}
|
||||||
for (int j = 0; j < 4; ++j) {
|
for (int j = 0; j < 4; ++j) {
|
||||||
EXPECT_EQ(data->mocap_quat[4*i+j], d_expected->mocap_quat[4*i+j]) << i;
|
EXPECT_EQ(data->mocap_quat[4 * i + j],
|
||||||
|
d_expected.get()->mocap_quat[4 * i + j])
|
||||||
|
<< i;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -2342,10 +2331,8 @@ TEST_F(MujocoTest, PreserveState) {
|
|||||||
mj_recompile(spec, 0, model, nullptr);
|
mj_recompile(spec, 0, model, nullptr);
|
||||||
|
|
||||||
// destroy everything
|
// destroy everything
|
||||||
mj_deleteData(d_expected);
|
|
||||||
mj_deleteSpec(spec);
|
mj_deleteSpec(spec);
|
||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
mj_deleteModel(m_expected);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, RecompileAttach) {
|
TEST_F(MujocoTest, RecompileAttach) {
|
||||||
@@ -2445,15 +2432,15 @@ TEST_F(MujocoTest, AttachMocap) {
|
|||||||
mjModel* model = mj_compile(spec, 0);
|
mjModel* model = mj_compile(spec, 0);
|
||||||
EXPECT_THAT(model, NotNull());
|
EXPECT_THAT(model, NotNull());
|
||||||
|
|
||||||
mjModel* m_expected = LoadModelFromString(xml_expected, er.data(), er.size());
|
MjModelPtr m_expected =
|
||||||
EXPECT_THAT(m_expected, NotNull()) << er.data();
|
LoadModelFromString(xml_expected, er.data(), er.size());
|
||||||
EXPECT_LE(CompareModel(model, m_expected, field), tol)
|
EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
|
||||||
|
EXPECT_LE(CompareModel(model, m_expected.get(), field), tol)
|
||||||
<< "Expected and attached models are different!\n"
|
<< "Expected and attached models are different!\n"
|
||||||
<< "Different field: " << field << '\n';
|
<< "Different field: " << field << '\n';
|
||||||
|
|
||||||
mj_deleteSpec(spec);
|
mj_deleteSpec(spec);
|
||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
mj_deleteModel(m_expected);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, ReplicateKeyframe) {
|
TEST_F(MujocoTest, ReplicateKeyframe) {
|
||||||
@@ -2475,8 +2462,8 @@ TEST_F(MujocoTest, ReplicateKeyframe) {
|
|||||||
|
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, testing::NotNull()) << error.data();
|
EXPECT_THAT(m.get(), testing::NotNull()) << error.data();
|
||||||
EXPECT_THAT(m->ngeom, 1);
|
EXPECT_THAT(m->ngeom, 1);
|
||||||
EXPECT_THAT(m->nbody, 2);
|
EXPECT_THAT(m->nbody, 2);
|
||||||
|
|
||||||
@@ -2485,10 +2472,8 @@ TEST_F(MujocoTest, ReplicateKeyframe) {
|
|||||||
EXPECT_THAT(m->nq, 1);
|
EXPECT_THAT(m->nq, 1);
|
||||||
EXPECT_THAT(m->key_qpos[0], 1);
|
EXPECT_THAT(m->key_qpos[0], 1);
|
||||||
EXPECT_THAT(m->key_qpos[1], 0);
|
EXPECT_THAT(m->key_qpos[1], 0);
|
||||||
EXPECT_STREQ(mj_id2name(m, mjOBJ_KEY, 0), "keyframe0");
|
EXPECT_STREQ(mj_id2name(m.get(), mjOBJ_KEY, 0), "keyframe0");
|
||||||
EXPECT_STREQ(mj_id2name(m, mjOBJ_KEY, 1), "keyframe");
|
EXPECT_STREQ(mj_id2name(m.get(), mjOBJ_KEY, 1), "keyframe");
|
||||||
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, AttachUnnamedAssets) {
|
TEST_F(MujocoTest, AttachUnnamedAssets) {
|
||||||
@@ -2690,17 +2675,17 @@ void AttachNestedKeyframe(bool compile) {
|
|||||||
std::string field = "";
|
std::string field = "";
|
||||||
mjModel* m_attached = mj_compile(parent, 0);
|
mjModel* m_attached = mj_compile(parent, 0);
|
||||||
EXPECT_THAT(m_attached, NotNull());
|
EXPECT_THAT(m_attached, NotNull());
|
||||||
mjModel* m_expected = LoadModelFromString(
|
MjModelPtr m_expected = LoadModelFromString(
|
||||||
compile ? expected_xml : expected_xml_uncompiled, er.data(), er.size());
|
compile ? expected_xml : expected_xml_uncompiled, er.data(), er.size());
|
||||||
EXPECT_THAT(m_expected, NotNull()) << er.data();
|
EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
|
||||||
EXPECT_LE(CompareModel(m_attached, m_expected, field), tol)
|
EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol)
|
||||||
<< "Expected and attached models are different!\n"
|
<< "Expected and attached models are different!\n"
|
||||||
<< "Different field: " << field << '\n';;
|
<< "Different field: " << field << '\n';
|
||||||
|
;
|
||||||
|
|
||||||
mj_deleteSpec(parent);
|
mj_deleteSpec(parent);
|
||||||
mj_deleteSpec(child);
|
mj_deleteSpec(child);
|
||||||
mj_deleteSpec(gchild);
|
mj_deleteSpec(gchild);
|
||||||
mj_deleteModel(m_expected);
|
|
||||||
mj_deleteModel(m_attached);
|
mj_deleteModel(m_attached);
|
||||||
mj_deleteModel(m_child);
|
mj_deleteModel(m_child);
|
||||||
}
|
}
|
||||||
@@ -2820,16 +2805,15 @@ TEST_F(MujocoTest, ResizeParentKeyframe) {
|
|||||||
|
|
||||||
mjtNum tol = 0;
|
mjtNum tol = 0;
|
||||||
std::string field = "";
|
std::string field = "";
|
||||||
mjModel* expected = LoadModelFromString(xml_expected, er.data(), er.size());
|
MjModelPtr expected = LoadModelFromString(xml_expected, er.data(), er.size());
|
||||||
EXPECT_THAT(expected, NotNull()) << er.data();
|
EXPECT_THAT(expected.get(), NotNull()) << er.data();
|
||||||
EXPECT_LE(CompareModel(model, expected, field), tol)
|
EXPECT_LE(CompareModel(model, expected.get(), field), tol)
|
||||||
<< "Expected and attached models are different!\n"
|
<< "Expected and attached models are different!\n"
|
||||||
<< "Different field: " << field << '\n';
|
<< "Different field: " << field << '\n';
|
||||||
|
|
||||||
mj_deleteSpec(parent);
|
mj_deleteSpec(parent);
|
||||||
mj_deleteSpec(child);
|
mj_deleteSpec(child);
|
||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
mj_deleteModel(expected);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, KeyframeSizeError) {
|
TEST_F(MujocoTest, KeyframeSizeError) {
|
||||||
@@ -2854,7 +2838,9 @@ TEST_F(MujocoTest, KeyframeSizeError) {
|
|||||||
std::array<char, 1000> er;
|
std::array<char, 1000> er;
|
||||||
mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
|
mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
|
||||||
EXPECT_THAT(spec, IsNull());
|
EXPECT_THAT(spec, IsNull());
|
||||||
EXPECT_THAT(er.data(), HasSubstr(
|
EXPECT_THAT(
|
||||||
|
er.data(),
|
||||||
|
HasSubstr(
|
||||||
"Keyframe 'invalid_qpos' has invalid qpos size, got 2, should be 1"));
|
"Keyframe 'invalid_qpos' has invalid qpos size, got 2, should be 1"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -2899,8 +2885,7 @@ TEST_F(MujocoTest, DifferentUnitsAllowed) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjSpec* gchild =
|
mjSpec* gchild = mj_parseXMLString(gchild_xml, 0, error.data(), error.size());
|
||||||
mj_parseXMLString(gchild_xml, 0, error.data(), error.size());
|
|
||||||
mjSpec* child = mj_parseXMLString(child_xml, 0, error.data(), error.size());
|
mjSpec* child = mj_parseXMLString(child_xml, 0, error.data(), error.size());
|
||||||
mjSpec* spec = mj_parseXMLString(parent_xml, 0, error.data(), error.size());
|
mjSpec* spec = mj_parseXMLString(parent_xml, 0, error.data(), error.size());
|
||||||
ASSERT_THAT(spec, NotNull()) << error.data();
|
ASSERT_THAT(spec, NotNull()) << error.data();
|
||||||
|
|||||||
@@ -48,9 +48,8 @@ TEST_F(UserCompositeTest, ShapeCanBeOmitted) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, NotNull()) << error.data();
|
EXPECT_THAT(m.get(), NotNull()) << error.data();
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserCompositeTest, InvalidShape) {
|
TEST_F(UserCompositeTest, InvalidShape) {
|
||||||
@@ -64,8 +63,8 @@ TEST_F(UserCompositeTest, InvalidShape) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull()) << error.data();
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(),
|
EXPECT_THAT(error.data(),
|
||||||
HasSubstr("The curve array contains an invalid shape"));
|
HasSubstr("The curve array contains an invalid shape"));
|
||||||
}
|
}
|
||||||
|
|||||||
+152
-198
@@ -24,19 +24,15 @@
|
|||||||
#include <mujoco/mujoco.h>
|
#include <mujoco/mujoco.h>
|
||||||
#include "test/fixture.h"
|
#include "test/fixture.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
namespace mujoco {
|
namespace mujoco {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
|
using ::testing::HasSubstr;
|
||||||
using ::testing::IsNull;
|
using ::testing::IsNull;
|
||||||
using ::testing::NotNull;
|
using ::testing::NotNull;
|
||||||
using ::testing::HasSubstr;
|
|
||||||
using ::testing::Pointwise;
|
using ::testing::Pointwise;
|
||||||
using UserFlexTest = MujocoTest;
|
using UserFlexTest = MujocoTest;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
TEST_F(UserFlexTest, ParentMustHaveName) {
|
TEST_F(UserFlexTest, ParentMustHaveName) {
|
||||||
static constexpr char xml[] = R"(
|
static constexpr char xml[] = R"(
|
||||||
<mujoco>
|
<mujoco>
|
||||||
@@ -46,8 +42,8 @@ TEST_F(UserFlexTest, ParentMustHaveName) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull());
|
EXPECT_THAT(m.get(), IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("required attribute missing: 'name'"));
|
EXPECT_THAT(error.data(), HasSubstr("required attribute missing: 'name'"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -60,8 +56,8 @@ TEST_F(UserFlexTest, InvalidDim) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull());
|
EXPECT_THAT(m.get(), IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("Invalid dim, must be between 1 and 3"));
|
EXPECT_THAT(error.data(), HasSubstr("Invalid dim, must be between 1 and 3"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -74,8 +70,8 @@ TEST_F(UserFlexTest, CountTooSmall) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull());
|
EXPECT_THAT(m.get(), IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("Count too small"));
|
EXPECT_THAT(error.data(), HasSubstr("Count too small"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -95,8 +91,8 @@ TEST_F(UserFlexTest, CellnumZeroInterpolated) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull());
|
EXPECT_THAT(m.get(), IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("cellcount cannot be 0"));
|
EXPECT_THAT(error.data(), HasSubstr("cellcount cannot be 0"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -109,8 +105,8 @@ TEST_F(UserFlexTest, SpacingGreaterThanGeometry) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull()) << error.data();
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(),
|
EXPECT_THAT(error.data(),
|
||||||
HasSubstr("Spacing must be larger than geometry size"));
|
HasSubstr("Spacing must be larger than geometry size"));
|
||||||
}
|
}
|
||||||
@@ -124,8 +120,8 @@ TEST_F(UserFlexTest, ScaleMinValue) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull()) << error.data();
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(), HasSubstr("Scale must be larger than mjMINVAL"));
|
EXPECT_THAT(error.data(), HasSubstr("Scale must be larger than mjMINVAL"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -138,8 +134,8 @@ TEST_F(UserFlexTest, MassMinValue) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull()) << error.data();
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(),
|
EXPECT_THAT(error.data(),
|
||||||
HasSubstr("Mass and inertiabox must be larger than mjMINVAL"));
|
HasSubstr("Mass and inertiabox must be larger than mjMINVAL"));
|
||||||
}
|
}
|
||||||
@@ -153,8 +149,8 @@ TEST_F(UserFlexTest, PointSizeNotMultipleOf3) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull()) << error.data();
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(), HasSubstr("Point size must be a multiple of 3"));
|
EXPECT_THAT(error.data(), HasSubstr("Point size must be a multiple of 3"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -167,8 +163,8 @@ TEST_F(UserFlexTest, ElementSize) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull()) << error.data();
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(),
|
EXPECT_THAT(error.data(),
|
||||||
HasSubstr("Element size must be a multiple of dim+1"));
|
HasSubstr("Element size must be a multiple of dim+1"));
|
||||||
}
|
}
|
||||||
@@ -182,8 +178,8 @@ TEST_F(UserFlexTest, PointAndElementNotInDirect) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull()) << error.data();
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(), HasSubstr("Point and element required"));
|
EXPECT_THAT(error.data(), HasSubstr("Point and element required"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -196,8 +192,8 @@ TEST_F(UserFlexTest, UnknownFlexCompType) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull()) << error.data();
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(), HasSubstr("invalid keyword: 'unknown'"));
|
EXPECT_THAT(error.data(), HasSubstr("invalid keyword: 'unknown'"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -212,8 +208,8 @@ TEST_F(UserFlexTest, InvalidPinid) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull()) << error.data();
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(),
|
EXPECT_THAT(error.data(),
|
||||||
HasSubstr("element 1 has point id 1, number of points is 1"));
|
HasSubstr("element 1 has point id 1, number of points is 1"));
|
||||||
}
|
}
|
||||||
@@ -227,8 +223,8 @@ TEST_F(UserFlexTest, MeshFileMissing) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(m, IsNull()) << error.data();
|
EXPECT_THAT(m.get(), IsNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(), HasSubstr("File is required"));
|
EXPECT_THAT(error.data(), HasSubstr("File is required"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -249,8 +245,8 @@ TEST_F(UserFlexTest, CreateBVHSuccess) {
|
|||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m, NotNull()) << error.data();
|
||||||
mjData* d = mj_makeData(m);
|
mjData* d = mj_makeData(m);
|
||||||
mj_step(m, d);
|
mj_step(m, d);
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, RigidFlex) {
|
TEST_F(UserFlexTest, RigidFlex) {
|
||||||
@@ -261,8 +257,8 @@ TEST_F(UserFlexTest, RigidFlex) {
|
|||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m, NotNull()) << error.data();
|
||||||
mjData* d = mj_makeData(m);
|
mjData* d = mj_makeData(m);
|
||||||
mj_step(m, d);
|
mj_step(m, d);
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, FlexNotCollide) {
|
TEST_F(UserFlexTest, FlexNotCollide) {
|
||||||
@@ -277,17 +273,16 @@ TEST_F(UserFlexTest, FlexNotCollide) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, BoundingBoxCoordinates) {
|
TEST_F(UserFlexTest, BoundingBoxCoordinates) {
|
||||||
#ifdef mjUSESINGLE
|
#ifdef mjUSESINGLE
|
||||||
GTEST_SKIP() << "Float32 rounding in bounding box centering gives ~3e-8 error";
|
GTEST_SKIP()
|
||||||
|
<< "Float32 rounding in bounding box centering gives ~3e-8 error";
|
||||||
#endif
|
#endif
|
||||||
static constexpr char xml[] = R"(
|
static constexpr char xml[] = R"(
|
||||||
<mujoco>
|
<mujoco>
|
||||||
@@ -298,11 +293,11 @@ TEST_F(UserFlexTest, BoundingBoxCoordinates) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_kinematics(m, d);
|
mj_kinematics(m.get(), d.get());
|
||||||
mj_flex(m, d);
|
mj_flex(m.get(), d.get());
|
||||||
|
|
||||||
EXPECT_EQ(m->nflexvert, 5 * 5 * 5);
|
EXPECT_EQ(m->nflexvert, 5 * 5 * 5);
|
||||||
EXPECT_EQ(m->nflexelem, 4 * 4 * 4 * 6);
|
EXPECT_EQ(m->nflexelem, 4 * 4 * 4 * 6);
|
||||||
@@ -323,9 +318,6 @@ TEST_F(UserFlexTest, BoundingBoxCoordinates) {
|
|||||||
EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 3], 1);
|
EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 3], 1);
|
||||||
EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 2], 1);
|
EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 2], 1);
|
||||||
EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 1], 1);
|
EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 1], 1);
|
||||||
|
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) {
|
TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) {
|
||||||
@@ -339,8 +331,8 @@ TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m1 = LoadModelFromString(xml_selfcoll, error.data(), error.size());
|
MjModelPtr m1 = LoadModelFromString(xml_selfcoll, error.data(), error.size());
|
||||||
EXPECT_THAT(m1, IsNull()) << error.data();
|
EXPECT_THAT(m1.get(), IsNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(),
|
EXPECT_THAT(error.data(),
|
||||||
HasSubstr("trilinear interpolation cannot do self-collision"));
|
HasSubstr("trilinear interpolation cannot do self-collision"));
|
||||||
static constexpr char xml_internal[] = R"(
|
static constexpr char xml_internal[] = R"(
|
||||||
@@ -352,8 +344,8 @@ TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m2 = LoadModelFromString(xml_internal, error.data(), error.size());
|
MjModelPtr m2 = LoadModelFromString(xml_internal, error.data(), error.size());
|
||||||
EXPECT_THAT(m2, IsNull()) << error.data();
|
EXPECT_THAT(m2.get(), IsNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(),
|
EXPECT_THAT(error.data(),
|
||||||
HasSubstr("trilinear interpolation cannot do internal"));
|
HasSubstr("trilinear interpolation cannot do internal"));
|
||||||
}
|
}
|
||||||
@@ -370,10 +362,11 @@ TEST_F(UserFlexTest, TrilinearInterpolation) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m1 = LoadModelFromString(xml_trilinear, error.data(), error.size());
|
MjModelPtr m1 =
|
||||||
ASSERT_THAT(m1, NotNull()) << error.data();
|
LoadModelFromString(xml_trilinear, error.data(), error.size());
|
||||||
mjData* d1 = mj_makeData(m1);
|
ASSERT_THAT(m1.get(), NotNull()) << error.data();
|
||||||
mj_step(m1, d1);
|
MjDataPtr d1 = MakeData(m1);
|
||||||
|
mj_step(m1.get(), d1.get());
|
||||||
|
|
||||||
static constexpr char xml_linear[] = R"(
|
static constexpr char xml_linear[] = R"(
|
||||||
<mujoco>
|
<mujoco>
|
||||||
@@ -385,10 +378,10 @@ TEST_F(UserFlexTest, TrilinearInterpolation) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m2 = LoadModelFromString(xml_linear, error.data(), error.size());
|
MjModelPtr m2 = LoadModelFromString(xml_linear, error.data(), error.size());
|
||||||
ASSERT_THAT(m2, NotNull()) << error.data();
|
ASSERT_THAT(m2.get(), NotNull()) << error.data();
|
||||||
mjData* d2 = mj_makeData(m2);
|
MjDataPtr d2 = MakeData(m2);
|
||||||
mj_step(m2, d2);
|
mj_step(m2.get(), d2.get());
|
||||||
|
|
||||||
EXPECT_EQ(m1->nflexvert, m2->nflexvert);
|
EXPECT_EQ(m1->nflexvert, m2->nflexvert);
|
||||||
for (int i = 0; i < 3 * m1->nflexvert; ++i) {
|
for (int i = 0; i < 3 * m1->nflexvert; ++i) {
|
||||||
@@ -443,11 +436,6 @@ TEST_F(UserFlexTest, TrilinearInterpolation) {
|
|||||||
EXPECT_EQ(d1->efc_diagA[i], d2->efc_diagA[i]);
|
EXPECT_EQ(d1->efc_diagA[i], d2->efc_diagA[i]);
|
||||||
EXPECT_EQ(d1->efc_D[i], d2->efc_D[i]);
|
EXPECT_EQ(d1->efc_D[i], d2->efc_D[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteModel(m1);
|
|
||||||
mj_deleteModel(m2);
|
|
||||||
mj_deleteData(d1);
|
|
||||||
mj_deleteData(d2);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, StiffnessMatrix) {
|
TEST_F(UserFlexTest, StiffnessMatrix) {
|
||||||
@@ -466,8 +454,8 @@ TEST_F(UserFlexTest, StiffnessMatrix) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
EXPECT_NE(m->flex_stiffness[m->flex_stiffnessadr[0]], 0);
|
EXPECT_NE(m->flex_stiffness[m->flex_stiffnessadr[0]], 0);
|
||||||
EXPECT_EQ(m->nflexnode, 8);
|
EXPECT_EQ(m->nflexnode, 8);
|
||||||
|
|
||||||
@@ -480,8 +468,6 @@ TEST_F(UserFlexTest, StiffnessMatrix) {
|
|||||||
mju_mulMatVec(res, m->flex_stiffness + m->flex_stiffnessadr[0], ones,
|
mju_mulMatVec(res, m->flex_stiffness + m->flex_stiffnessadr[0], ones,
|
||||||
3 * m->nflexnode, 3 * m->nflexnode);
|
3 * m->nflexnode, 3 * m->nflexnode);
|
||||||
EXPECT_THAT(res, Pointwise(MjNear(1e-8, 1e-4), zeros));
|
EXPECT_THAT(res, Pointwise(MjNear(1e-8, 1e-4), zeros));
|
||||||
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, StiffnessCacheDiffersByGeometry) {
|
TEST_F(UserFlexTest, StiffnessCacheDiffersByGeometry) {
|
||||||
@@ -510,19 +496,18 @@ TEST_F(UserFlexTest, StiffnessCacheDiffersByGeometry) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
|
|
||||||
mjModel* m_small = LoadModelFromString(xml_small, error.data(), error.size());
|
MjModelPtr m_small =
|
||||||
ASSERT_THAT(m_small, NotNull()) << error.data();
|
LoadModelFromString(xml_small, error.data(), error.size());
|
||||||
|
ASSERT_THAT(m_small.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
mjModel* m_large = LoadModelFromString(xml_large, error.data(), error.size());
|
MjModelPtr m_large =
|
||||||
ASSERT_THAT(m_large, NotNull()) << error.data();
|
LoadModelFromString(xml_large, error.data(), error.size());
|
||||||
|
ASSERT_THAT(m_large.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
// Same number of nodes but different stiffness due to different geometry
|
// Same number of nodes but different stiffness due to different geometry
|
||||||
EXPECT_EQ(m_small->nflexnode, m_large->nflexnode);
|
EXPECT_EQ(m_small->nflexnode, m_large->nflexnode);
|
||||||
EXPECT_NE(m_small->flex_stiffness[m_small->flex_stiffnessadr[0]],
|
EXPECT_NE(m_small->flex_stiffness[m_small->flex_stiffnessadr[0]],
|
||||||
m_large->flex_stiffness[m_large->flex_stiffnessadr[0]]);
|
m_large->flex_stiffness[m_large->flex_stiffnessadr[0]]);
|
||||||
|
|
||||||
mj_deleteModel(m_small);
|
|
||||||
mj_deleteModel(m_large);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadTexture) {
|
TEST_F(UserFlexTest, LoadTexture) {
|
||||||
@@ -552,8 +537,8 @@ TEST_F(UserFlexTest, LoadMSHBinary_41_Success) {
|
|||||||
EXPECT_EQ(m->nflexelem, 24);
|
EXPECT_EQ(m->nflexelem, 24);
|
||||||
EXPECT_EQ(m->flex_dim[0], 3);
|
EXPECT_EQ(m->flex_dim[0], 3);
|
||||||
mj_step(m, d);
|
mj_step(m, d);
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHBinary_22_Success) {
|
TEST_F(UserFlexTest, LoadMSHBinary_22_Success) {
|
||||||
@@ -567,8 +552,8 @@ TEST_F(UserFlexTest, LoadMSHBinary_22_Success) {
|
|||||||
EXPECT_EQ(m->nflexelem, 24);
|
EXPECT_EQ(m->nflexelem, 24);
|
||||||
EXPECT_EQ(m->flex_dim[0], 3);
|
EXPECT_EQ(m->flex_dim[0], 3);
|
||||||
mj_step(m, d);
|
mj_step(m, d);
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHSurfaceBinary_41_Success) {
|
TEST_F(UserFlexTest, LoadMSHSurfaceBinary_41_Success) {
|
||||||
@@ -595,8 +580,8 @@ TEST_F(UserFlexTest, LoadMSHSurfaceBinary_41_Success) {
|
|||||||
EXPECT_EQ(m->flex_elem[2], 3 - 1);
|
EXPECT_EQ(m->flex_elem[2], 3 - 1);
|
||||||
|
|
||||||
mj_step(m, d);
|
mj_step(m, d);
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHSurfaceBinary_22_Success) {
|
TEST_F(UserFlexTest, LoadMSHSurfaceBinary_22_Success) {
|
||||||
@@ -623,8 +608,8 @@ TEST_F(UserFlexTest, LoadMSHSurfaceBinary_22_Success) {
|
|||||||
EXPECT_EQ(m->flex_elem[2], 3 - 1);
|
EXPECT_EQ(m->flex_elem[2], 3 - 1);
|
||||||
|
|
||||||
mj_step(m, d);
|
mj_step(m, d);
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHBinaryFTETWILD_22_Success) {
|
TEST_F(UserFlexTest, LoadMSHBinaryFTETWILD_22_Success) {
|
||||||
@@ -638,8 +623,8 @@ TEST_F(UserFlexTest, LoadMSHBinaryFTETWILD_22_Success) {
|
|||||||
EXPECT_EQ(m->nflexelem, 1073);
|
EXPECT_EQ(m->nflexelem, 1073);
|
||||||
EXPECT_EQ(m->flex_dim[0], 3);
|
EXPECT_EQ(m->flex_dim[0], 3);
|
||||||
mj_step(m, d);
|
mj_step(m, d);
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHASCII_41_Success) {
|
TEST_F(UserFlexTest, LoadMSHASCII_41_Success) {
|
||||||
@@ -653,8 +638,8 @@ TEST_F(UserFlexTest, LoadMSHASCII_41_Success) {
|
|||||||
EXPECT_EQ(m->nflexelem, 24);
|
EXPECT_EQ(m->nflexelem, 24);
|
||||||
EXPECT_EQ(m->flex_dim[0], 3);
|
EXPECT_EQ(m->flex_dim[0], 3);
|
||||||
mj_step(m, d);
|
mj_step(m, d);
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHASCII_22_Success) {
|
TEST_F(UserFlexTest, LoadMSHASCII_22_Success) {
|
||||||
@@ -668,8 +653,8 @@ TEST_F(UserFlexTest, LoadMSHASCII_22_Success) {
|
|||||||
EXPECT_EQ(m->nflexelem, 24);
|
EXPECT_EQ(m->nflexelem, 24);
|
||||||
EXPECT_EQ(m->flex_dim[0], 3);
|
EXPECT_EQ(m->flex_dim[0], 3);
|
||||||
mj_step(m, d);
|
mj_step(m, d);
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHSurfaceASCII_41_Success) {
|
TEST_F(UserFlexTest, LoadMSHSurfaceASCII_41_Success) {
|
||||||
@@ -696,8 +681,8 @@ TEST_F(UserFlexTest, LoadMSHSurfaceASCII_41_Success) {
|
|||||||
EXPECT_EQ(m->flex_elem[2], 3 - 1);
|
EXPECT_EQ(m->flex_elem[2], 3 - 1);
|
||||||
|
|
||||||
mj_step(m, d);
|
mj_step(m, d);
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHSurfaceASCII_22_Success) {
|
TEST_F(UserFlexTest, LoadMSHSurfaceASCII_22_Success) {
|
||||||
@@ -724,8 +709,8 @@ TEST_F(UserFlexTest, LoadMSHSurfaceASCII_22_Success) {
|
|||||||
EXPECT_EQ(m->flex_elem[2], 3 - 1);
|
EXPECT_EQ(m->flex_elem[2], 3 - 1);
|
||||||
|
|
||||||
mj_step(m, d);
|
mj_step(m, d);
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHASCIIFTETWILD_22_Success) {
|
TEST_F(UserFlexTest, LoadMSHASCIIFTETWILD_22_Success) {
|
||||||
@@ -739,69 +724,68 @@ TEST_F(UserFlexTest, LoadMSHASCIIFTETWILD_22_Success) {
|
|||||||
EXPECT_EQ(m->nflexelem, 1070);
|
EXPECT_EQ(m->nflexelem, 1070);
|
||||||
EXPECT_EQ(m->flex_dim[0], 3);
|
EXPECT_EQ(m->flex_dim[0], 3);
|
||||||
mj_step(m, d);
|
mj_step(m, d);
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
mj_deleteData(d);
|
||||||
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingNodeHeader_Fail) {
|
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingNodeHeader_Fail) {
|
||||||
const std::string xml_path =
|
const std::string xml_path = GetTestDataFilePath(
|
||||||
GetTestDataFilePath(
|
|
||||||
"user/testdata/malformed_cube_41_ascii_missing_node_header.xml");
|
"user/testdata/malformed_cube_41_ascii_missing_node_header.xml");
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||||
EXPECT_THAT(error.data(), HasSubstr(
|
EXPECT_THAT(error.data(),
|
||||||
"XML Error: Error: All nodes must be in single block"));
|
HasSubstr("XML Error: Error: All nodes must be in single block"));
|
||||||
mj_deleteModel(m);
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingNodeIndex_Fail) {
|
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingNodeIndex_Fail) {
|
||||||
const std::string xml_path =
|
const std::string xml_path = GetTestDataFilePath(
|
||||||
GetTestDataFilePath(
|
|
||||||
"user/testdata/malformed_cube_41_ascii_missing_node_index.xml");
|
"user/testdata/malformed_cube_41_ascii_missing_node_index.xml");
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||||
EXPECT_THAT(error.data(), HasSubstr(
|
EXPECT_THAT(error.data(),
|
||||||
"XML Error: Error: Node tags must be sequential"));
|
HasSubstr("XML Error: Error: Node tags must be sequential"));
|
||||||
mj_deleteModel(m);
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingElementHeader_Fail) {
|
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingElementHeader_Fail) {
|
||||||
const std::string xml_path =
|
const std::string xml_path = GetTestDataFilePath(
|
||||||
GetTestDataFilePath(
|
|
||||||
"user/testdata/malformed_cube_41_ascii_missing_element_header.xml");
|
"user/testdata/malformed_cube_41_ascii_missing_element_header.xml");
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||||
EXPECT_THAT(error.data(), HasSubstr(
|
EXPECT_THAT(
|
||||||
"XML Error: Error: All elements must be in single block"));
|
error.data(),
|
||||||
|
HasSubstr("XML Error: Error: All elements must be in single block"));
|
||||||
mj_deleteModel(m);
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingElement_Fail) {
|
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingElement_Fail) {
|
||||||
const std::string xml_path =
|
const std::string xml_path = GetTestDataFilePath(
|
||||||
GetTestDataFilePath(
|
|
||||||
"user/testdata/malformed_cube_41_ascii_missing_element.xml");
|
"user/testdata/malformed_cube_41_ascii_missing_element.xml");
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||||
EXPECT_THAT(error.data(), HasSubstr(
|
EXPECT_THAT(error.data(),
|
||||||
"XML Error: Error: Error reading Elements"));
|
HasSubstr("XML Error: Error: Error reading Elements"));
|
||||||
mj_deleteModel(m);
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest,
|
TEST_F(UserFlexTest,
|
||||||
LoadMSHASCII_41_MismatchBetweenMaxNodesAndNodesInBlock_Fail) {
|
LoadMSHASCII_41_MismatchBetweenMaxNodesAndNodesInBlock_Fail) {
|
||||||
const std::string xml_path =
|
const std::string xml_path = GetTestDataFilePath(
|
||||||
GetTestDataFilePath(
|
"user/testdata/"
|
||||||
"user/testdata/malformed_cube_41_ascii_mismatch_between_max_nodes_and_nodes_in_block.xml");
|
"malformed_cube_41_ascii_mismatch_between_max_nodes_and_nodes_in_block."
|
||||||
|
"xml");
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||||
EXPECT_THAT(error.data(), HasSubstr(
|
EXPECT_THAT(
|
||||||
"XML Error: Error: Maximum number of nodes must be equal to number of nodes in a block\nElement 'flexcomp', line 22\n"));
|
error.data(),
|
||||||
|
HasSubstr("XML Error: Error: Maximum number of nodes must be equal to "
|
||||||
|
"number of nodes in a block\nElement 'flexcomp', line 22\n"));
|
||||||
mj_deleteModel(m);
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumNodes_Fail) {
|
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumNodes_Fail) {
|
||||||
const std::string xml_path =
|
const std::string xml_path = GetTestDataFilePath(
|
||||||
GetTestDataFilePath(
|
|
||||||
"user/testdata/malformed_cube_22_ascii_missing_num_nodes.xml");
|
"user/testdata/malformed_cube_22_ascii_missing_num_nodes.xml");
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||||
@@ -812,19 +796,17 @@ TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumNodes_Fail) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNode_Fail) {
|
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNode_Fail) {
|
||||||
const std::string xml_path =
|
const std::string xml_path = GetTestDataFilePath(
|
||||||
GetTestDataFilePath(
|
|
||||||
"user/testdata/malformed_cube_22_ascii_missing_node.xml");
|
"user/testdata/malformed_cube_22_ascii_missing_node.xml");
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||||
EXPECT_THAT(error.data(), HasSubstr(
|
EXPECT_THAT(error.data(),
|
||||||
"XML Error: Error: Error reading node tags"));
|
HasSubstr("XML Error: Error: Error reading node tags"));
|
||||||
mj_deleteModel(m);
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumElements_Fail) {
|
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumElements_Fail) {
|
||||||
const std::string xml_path =
|
const std::string xml_path = GetTestDataFilePath(
|
||||||
GetTestDataFilePath(
|
|
||||||
"user/testdata/malformed_shark_22_ascii_missing_num_elements.xml");
|
"user/testdata/malformed_shark_22_ascii_missing_num_elements.xml");
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||||
@@ -835,19 +817,17 @@ TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumElements_Fail) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingElement_Fail) {
|
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingElement_Fail) {
|
||||||
const std::string xml_path =
|
const std::string xml_path = GetTestDataFilePath(
|
||||||
GetTestDataFilePath(
|
|
||||||
"user/testdata/malformed_cube_22_ascii_missing_element.xml");
|
"user/testdata/malformed_cube_22_ascii_missing_element.xml");
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||||
EXPECT_THAT(error.data(), HasSubstr(
|
EXPECT_THAT(error.data(),
|
||||||
"XML Error: Error: Error reading Elements"));
|
HasSubstr("XML Error: Error: Error reading Elements"));
|
||||||
mj_deleteModel(m);
|
mj_deleteModel(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, LoadMSHASCII_dim_missing_in_xml) {
|
TEST_F(UserFlexTest, LoadMSHASCII_dim_missing_in_xml) {
|
||||||
const std::string xml_path =
|
const std::string xml_path = GetTestDataFilePath(
|
||||||
GetTestDataFilePath(
|
|
||||||
"user/testdata/cube_22_ascii_vol_gmshApp_missing_dim.xml");
|
"user/testdata/cube_22_ascii_vol_gmshApp_missing_dim.xml");
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||||
@@ -871,9 +851,8 @@ TEST_F(UserFlexTest, TrilinearUnusedVertices_Crash) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error.data();
|
ASSERT_THAT(m.get(), testing::NotNull()) << error.data();
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, MeshNodePinning) {
|
TEST_F(UserFlexTest, MeshNodePinning) {
|
||||||
@@ -891,8 +870,8 @@ TEST_F(UserFlexTest, MeshNodePinning) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, testing::NotNull()) << error.data();
|
ASSERT_THAT(m.get(), testing::NotNull()) << error.data();
|
||||||
|
|
||||||
// Verify that node 0 (corner) is pinned
|
// Verify that node 0 (corner) is pinned
|
||||||
// Trilinear 3D has 8 nodes.
|
// Trilinear 3D has 8 nodes.
|
||||||
@@ -901,14 +880,11 @@ TEST_F(UserFlexTest, MeshNodePinning) {
|
|||||||
// m->nbody should reflect this. Flex bodies are added to the model.
|
// m->nbody should reflect this. Flex bodies are added to the model.
|
||||||
// Model has 1 world body + flex bodies.
|
// Model has 1 world body + flex bodies.
|
||||||
EXPECT_EQ(m->nbody, 1 + 7); // 1 world + 7 flex nodes (1 pinned)
|
EXPECT_EQ(m->nbody, 1 + 7); // 1 world + 7 flex nodes (1 pinned)
|
||||||
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, FlexcompMeshLoadsFromVFS) {
|
TEST_F(UserFlexTest, FlexcompMeshLoadsFromVFS) {
|
||||||
// read cube.stl from testdata into a buffer
|
// read cube.stl from testdata into a buffer
|
||||||
const std::string stl_path =
|
const std::string stl_path = GetTestDataFilePath("user/testdata/cube.stl");
|
||||||
GetTestDataFilePath("user/testdata/cube.stl");
|
|
||||||
FILE* f = fopen(stl_path.c_str(), "rb");
|
FILE* f = fopen(stl_path.c_str(), "rb");
|
||||||
ASSERT_THAT(f, NotNull()) << "Could not open " << stl_path;
|
ASSERT_THAT(f, NotNull()) << "Could not open " << stl_path;
|
||||||
fseek(f, 0, SEEK_END);
|
fseek(f, 0, SEEK_END);
|
||||||
@@ -938,12 +914,10 @@ TEST_F(UserFlexTest, FlexcompMeshLoadsFromVFS) {
|
|||||||
|
|
||||||
// cleanup
|
// cleanup
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size(), &vfs);
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size(), &vfs);
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteVFS(&vfs);
|
mj_deleteVFS(&vfs);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -986,22 +960,20 @@ TEST_F(UserFlexTest, FlexAttachConstraintPreserved) {
|
|||||||
|
|
||||||
// First verify the standalone flex model has constraints
|
// First verify the standalone flex model has constraints
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m_standalone =
|
MjModelPtr m_standalone =
|
||||||
LoadModelFromString(flex_xml, error.data(), error.size(), vfs.get());
|
LoadModelFromString(flex_xml, error.data(), error.size(), vfs.get());
|
||||||
ASSERT_THAT(m_standalone, NotNull()) << error.data();
|
ASSERT_THAT(m_standalone.get(), NotNull()) << error.data();
|
||||||
mjData* d_standalone = mj_makeData(m_standalone);
|
MjDataPtr d_standalone = MakeData(m_standalone);
|
||||||
mj_forward(m_standalone, d_standalone);
|
mj_forward(m_standalone.get(), d_standalone.get());
|
||||||
int standalone_neq = m_standalone->neq;
|
int standalone_neq = m_standalone->neq;
|
||||||
EXPECT_GT(standalone_neq, 0) << "Standalone flex should have constraints";
|
EXPECT_GT(standalone_neq, 0) << "Standalone flex should have constraints";
|
||||||
mj_deleteData(d_standalone);
|
|
||||||
mj_deleteModel(m_standalone);
|
|
||||||
|
|
||||||
// Now load the parent model which attaches the flex
|
// Now load the parent model which attaches the flex
|
||||||
mjModel* m_attached =
|
MjModelPtr m_attached =
|
||||||
LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get());
|
LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get());
|
||||||
ASSERT_THAT(m_attached, NotNull()) << error.data();
|
ASSERT_THAT(m_attached.get(), NotNull()) << error.data();
|
||||||
mjData* d_attached = mj_makeData(m_attached);
|
MjDataPtr d_attached = MakeData(m_attached);
|
||||||
mj_forward(m_attached, d_attached);
|
mj_forward(m_attached.get(), d_attached.get());
|
||||||
|
|
||||||
// THE BUG: flex constraints disappear when attached
|
// THE BUG: flex constraints disappear when attached
|
||||||
EXPECT_GT(m_attached->neq, 0)
|
EXPECT_GT(m_attached->neq, 0)
|
||||||
@@ -1009,8 +981,6 @@ TEST_F(UserFlexTest, FlexAttachConstraintPreserved) {
|
|||||||
EXPECT_EQ(m_attached->neq, standalone_neq)
|
EXPECT_EQ(m_attached->neq, standalone_neq)
|
||||||
<< "Attached flex should have same number of constraints as standalone";
|
<< "Attached flex should have same number of constraints as standalone";
|
||||||
|
|
||||||
mj_deleteData(d_attached);
|
|
||||||
mj_deleteModel(m_attached);
|
|
||||||
mj_deleteVFS(vfs.get());
|
mj_deleteVFS(vfs.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1024,11 +994,10 @@ TEST_F(UserFlexTest, FlexNoConstraintsWarning) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(),
|
EXPECT_THAT(error.data(),
|
||||||
HasSubstr("no equality constraints or passive forces"));
|
HasSubstr("no equality constraints or passive forces"));
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, EmptyCellNodePinning) {
|
TEST_F(UserFlexTest, EmptyCellNodePinning) {
|
||||||
@@ -1046,8 +1015,8 @@ TEST_F(UserFlexTest, EmptyCellNodePinning) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
// A 2x2x2 grid with trilinear order has (2+1)^3 = 27 node positions.
|
// A 2x2x2 grid with trilinear order has (2+1)^3 = 27 node positions.
|
||||||
int nadr = m->flex_nodeadr[0];
|
int nadr = m->flex_nodeadr[0];
|
||||||
@@ -1065,20 +1034,16 @@ TEST_F(UserFlexTest, EmptyCellNodePinning) {
|
|||||||
EXPECT_EQ(pinned, 0);
|
EXPECT_EQ(pinned, 0);
|
||||||
|
|
||||||
// Verify simulation works
|
// Verify simulation works
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
for (int i = 0; i < 10; i++) {
|
for (int i = 0; i < 10; i++) {
|
||||||
mj_step(m, d);
|
mj_step(m.get(), d.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteData(d);
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, EmptyCellNodePinningMesh) {
|
TEST_F(UserFlexTest, EmptyCellNodePinningMesh) {
|
||||||
// Load bunny_multicell.xml which has a 3x3x3 grid.
|
// Load bunny_multicell.xml which has a 3x3x3 grid.
|
||||||
// The bunny mesh only occupies some cells, so many nodes should be pinned.
|
// The bunny mesh only occupies some cells, so many nodes should be pinned.
|
||||||
const std::string xml_path =
|
const std::string xml_path = GetModelPath("flex/bunny_multicell.xml");
|
||||||
GetModelPath("flex/bunny_multicell.xml");
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m, NotNull()) << error.data();
|
||||||
@@ -1152,8 +1117,8 @@ TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
// 3x1x1 quadratic grid: (3*2+1) * (1*2+1) * (1*2+1) = 7*3*3 = 63 nodes
|
// 3x1x1 quadratic grid: (3*2+1) * (1*2+1) * (1*2+1) = 7*3*3 = 63 nodes
|
||||||
int nadr = m->flex_nodeadr[0];
|
int nadr = m->flex_nodeadr[0];
|
||||||
@@ -1161,7 +1126,7 @@ TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) {
|
|||||||
EXPECT_EQ(nnode, 63);
|
EXPECT_EQ(nnode, 63);
|
||||||
|
|
||||||
// Count pinned nodes: pinned nodes are assigned to the parent body.
|
// Count pinned nodes: pinned nodes are assigned to the parent body.
|
||||||
int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent");
|
int parent_bid = mj_name2id(m.get(), mjOBJ_BODY, "parent");
|
||||||
ASSERT_GT(parent_bid, 0);
|
ASSERT_GT(parent_bid, 0);
|
||||||
int pinned = 0;
|
int pinned = 0;
|
||||||
for (int n = nadr; n < nadr + nnode; n++) {
|
for (int n = nadr; n < nadr + nnode; n++) {
|
||||||
@@ -1175,8 +1140,7 @@ TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) {
|
|||||||
// That's 4 * 3 * 3 = 36 nodes.
|
// That's 4 * 3 * 3 = 36 nodes.
|
||||||
EXPECT_EQ(pinned, 36);
|
EXPECT_EQ(pinned, 36);
|
||||||
|
|
||||||
mj_deleteData(mj_makeData(m));
|
mj_deleteData(mj_makeData(m.get()));
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, EmptyCellDetectsElements) {
|
TEST_F(UserFlexTest, EmptyCellDetectsElements) {
|
||||||
@@ -1208,8 +1172,8 @@ TEST_F(UserFlexTest, EmptyCellDetectsElements) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
// 6x6x6 trilinear grid: (6+1)^3 = 343 nodes
|
// 6x6x6 trilinear grid: (6+1)^3 = 343 nodes
|
||||||
int nadr = m->flex_nodeadr[0];
|
int nadr = m->flex_nodeadr[0];
|
||||||
@@ -1217,7 +1181,7 @@ TEST_F(UserFlexTest, EmptyCellDetectsElements) {
|
|||||||
ASSERT_EQ(nnode, 343);
|
ASSERT_EQ(nnode, 343);
|
||||||
|
|
||||||
// Count pinned nodes: those assigned to the parent body.
|
// Count pinned nodes: those assigned to the parent body.
|
||||||
int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent");
|
int parent_bid = mj_name2id(m.get(), mjOBJ_BODY, "parent");
|
||||||
ASSERT_GT(parent_bid, 0);
|
ASSERT_GT(parent_bid, 0);
|
||||||
int pinned = 0;
|
int pinned = 0;
|
||||||
for (int n = nadr; n < nadr + nnode; n++) {
|
for (int n = nadr; n < nadr + nnode; n++) {
|
||||||
@@ -1233,8 +1197,7 @@ TEST_F(UserFlexTest, EmptyCellDetectsElements) {
|
|||||||
// No cells are empty → 0 nodes pinned.
|
// No cells are empty → 0 nodes pinned.
|
||||||
EXPECT_EQ(pinned, 0);
|
EXPECT_EQ(pinned, 0);
|
||||||
|
|
||||||
mj_deleteData(mj_makeData(m));
|
mj_deleteData(mj_makeData(m.get()));
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, TotalMassTrilinear) {
|
TEST_F(UserFlexTest, TotalMassTrilinear) {
|
||||||
@@ -1249,8 +1212,8 @@ TEST_F(UserFlexTest, TotalMassTrilinear) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
double total_mass = 0;
|
double total_mass = 0;
|
||||||
for (int i = 1; i < m->nbody; ++i) {
|
for (int i = 1; i < m->nbody; ++i) {
|
||||||
@@ -1258,7 +1221,6 @@ TEST_F(UserFlexTest, TotalMassTrilinear) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
EXPECT_NEAR(total_mass, 1.5, 1e-5);
|
EXPECT_NEAR(total_mass, 1.5, 1e-5);
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, TotalMassQuadratic) {
|
TEST_F(UserFlexTest, TotalMassQuadratic) {
|
||||||
@@ -1273,8 +1235,8 @@ TEST_F(UserFlexTest, TotalMassQuadratic) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
double total_mass = 0;
|
double total_mass = 0;
|
||||||
for (int i = 1; i < m->nbody; ++i) {
|
for (int i = 1; i < m->nbody; ++i) {
|
||||||
@@ -1282,7 +1244,6 @@ TEST_F(UserFlexTest, TotalMassQuadratic) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
EXPECT_NEAR(total_mass, 2.0, 1e-5);
|
EXPECT_NEAR(total_mass, 2.0, 1e-5);
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, Dof2d) {
|
TEST_F(UserFlexTest, Dof2d) {
|
||||||
@@ -1298,7 +1259,8 @@ TEST_F(UserFlexTest, Dof2d) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
|
|
||||||
// same model with dof="full" for comparison: 9 vertices, 3 DOFs each -> nv = 27
|
// same model with dof="full" for comparison: 9 vertices, 3 DOFs each -> nv =
|
||||||
|
// 27
|
||||||
static constexpr char xml_full[] = R"(
|
static constexpr char xml_full[] = R"(
|
||||||
<mujoco>
|
<mujoco>
|
||||||
<worldbody>
|
<worldbody>
|
||||||
@@ -1313,14 +1275,14 @@ TEST_F(UserFlexTest, Dof2d) {
|
|||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
|
|
||||||
// load 2d model
|
// load 2d model
|
||||||
mjModel* m_2d = LoadModelFromString(xml_2d, error.data(), error.size());
|
MjModelPtr m_2d = LoadModelFromString(xml_2d, error.data(), error.size());
|
||||||
ASSERT_THAT(m_2d, NotNull()) << error.data();
|
ASSERT_THAT(m_2d.get(), NotNull()) << error.data();
|
||||||
mjData* d_2d = mj_makeData(m_2d);
|
MjDataPtr d_2d = MakeData(m_2d);
|
||||||
|
|
||||||
// load full model
|
// load full model
|
||||||
mjModel* m_full = LoadModelFromString(xml_full, error.data(), error.size());
|
MjModelPtr m_full = LoadModelFromString(xml_full, error.data(), error.size());
|
||||||
ASSERT_THAT(m_full, NotNull()) << error.data();
|
ASSERT_THAT(m_full.get(), NotNull()) << error.data();
|
||||||
mjData* d_full = mj_makeData(m_full);
|
MjDataPtr d_full = MakeData(m_full);
|
||||||
|
|
||||||
// verify DOF counts
|
// verify DOF counts
|
||||||
EXPECT_EQ(m_2d->nv, 18); // 9 vertices * 2 DOFs
|
EXPECT_EQ(m_2d->nv, 18); // 9 vertices * 2 DOFs
|
||||||
@@ -1340,14 +1302,9 @@ TEST_F(UserFlexTest, Dof2d) {
|
|||||||
|
|
||||||
// simulate a few steps to make sure nothing crashes
|
// simulate a few steps to make sure nothing crashes
|
||||||
for (int i = 0; i < 10; i++) {
|
for (int i = 0; i < 10; i++) {
|
||||||
mj_step(m_2d, d_2d);
|
mj_step(m_2d.get(), d_2d.get());
|
||||||
mj_step(m_full, d_full);
|
mj_step(m_full.get(), d_full.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteModel(m_2d);
|
|
||||||
mj_deleteModel(m_full);
|
|
||||||
mj_deleteData(d_2d);
|
|
||||||
mj_deleteData(d_full);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, Vert0RotationInvariant) {
|
TEST_F(UserFlexTest, Vert0RotationInvariant) {
|
||||||
@@ -1380,11 +1337,12 @@ TEST_F(UserFlexTest, Vert0RotationInvariant) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m1 = LoadModelFromString(xml_unrotated, error.data(), error.size());
|
MjModelPtr m1 =
|
||||||
ASSERT_THAT(m1, NotNull()) << error.data();
|
LoadModelFromString(xml_unrotated, error.data(), error.size());
|
||||||
|
ASSERT_THAT(m1.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
mjModel* m2 = LoadModelFromString(xml_rotated, error.data(), error.size());
|
MjModelPtr m2 = LoadModelFromString(xml_rotated, error.data(), error.size());
|
||||||
ASSERT_THAT(m2, NotNull()) << error.data();
|
ASSERT_THAT(m2.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
// same number of vertices
|
// same number of vertices
|
||||||
ASSERT_EQ(m1->nflexvert, m2->nflexvert);
|
ASSERT_EQ(m1->nflexvert, m2->nflexvert);
|
||||||
@@ -1394,9 +1352,6 @@ TEST_F(UserFlexTest, Vert0RotationInvariant) {
|
|||||||
EXPECT_NEAR(m1->flex_vert0[i], m2->flex_vert0[i], 1e-10)
|
EXPECT_NEAR(m1->flex_vert0[i], m2->flex_vert0[i], 1e-10)
|
||||||
<< "vert0 mismatch at index " << i;
|
<< "vert0 mismatch at index " << i;
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteModel(m1);
|
|
||||||
mj_deleteModel(m2);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserFlexTest, Load1DFlexFromOBJ) {
|
TEST_F(UserFlexTest, Load1DFlexFromOBJ) {
|
||||||
@@ -1413,4 +1368,3 @@ TEST_F(UserFlexTest, Load1DFlexFromOBJ) {
|
|||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace mujoco
|
} // namespace mujoco
|
||||||
|
|
||||||
|
|||||||
+155
-204
@@ -37,16 +37,12 @@ namespace {
|
|||||||
|
|
||||||
using MjCMeshTest = MujocoTest;
|
using MjCMeshTest = MujocoTest;
|
||||||
|
|
||||||
static const char* const kMeshPath =
|
static const char* const kMeshPath = "user/testdata/mesh.xml";
|
||||||
"user/testdata/mesh.xml";
|
|
||||||
static const char* const kDuplicateVerticesPath =
|
static const char* const kDuplicateVerticesPath =
|
||||||
"user/testdata/duplicate_vertices.xml";
|
"user/testdata/duplicate_vertices.xml";
|
||||||
static const char* const kCubePath =
|
static const char* const kCubePath = "user/testdata/cube.xml";
|
||||||
"user/testdata/cube.xml";
|
static const char* const kCubeCompletePath = "user/testdata/cube_complete.obj";
|
||||||
static const char* const kCubeCompletePath =
|
static const char* const kTorusPath = "user/testdata/torus.xml";
|
||||||
"user/testdata/cube_complete.obj";
|
|
||||||
static const char* const kTorusPath =
|
|
||||||
"user/testdata/torus.xml";
|
|
||||||
static const char* const kTorusMaxhullVertPath =
|
static const char* const kTorusMaxhullVertPath =
|
||||||
"user/testdata/torus_maxhullvert.xml";
|
"user/testdata/torus_maxhullvert.xml";
|
||||||
static const char* const kTorusDefaultMaxhullVertPath =
|
static const char* const kTorusDefaultMaxhullVertPath =
|
||||||
@@ -57,18 +53,14 @@ static const char* const kConvexInertiaPath =
|
|||||||
"user/testdata/inertia_convex.xml";
|
"user/testdata/inertia_convex.xml";
|
||||||
static const char* const kConcaveInertiaPath =
|
static const char* const kConcaveInertiaPath =
|
||||||
"user/testdata/inertia_concave.xml";
|
"user/testdata/inertia_concave.xml";
|
||||||
static const char* const kShellInertiaPath =
|
static const char* const kShellInertiaPath = "user/testdata/inertia_shell.xml";
|
||||||
"user/testdata/inertia_shell.xml";
|
static const char* const kTorusQuadsPath = "user/testdata/torus_quads.xml";
|
||||||
static const char* const kTorusQuadsPath =
|
|
||||||
"user/testdata/torus_quads.xml";
|
|
||||||
static const char* const kTexturedTorusPath =
|
static const char* const kTexturedTorusPath =
|
||||||
"user/testdata/textured_torus.xml";
|
"user/testdata/textured_torus.xml";
|
||||||
static const char* const kDuplicateOBJPath =
|
static const char* const kDuplicateOBJPath = "user/testdata/duplicate.xml";
|
||||||
"user/testdata/duplicate.xml";
|
|
||||||
static const char* const kMalformedFaceOBJPath =
|
static const char* const kMalformedFaceOBJPath =
|
||||||
"user/testdata/malformed_face.xml";
|
"user/testdata/malformed_face.xml";
|
||||||
static const char* const kCubeSkinPath =
|
static const char* const kCubeSkinPath = "user/testdata/cube_skin.xml";
|
||||||
"user/testdata/cube_skin.xml";
|
|
||||||
|
|
||||||
static const char* const kNoDecoderForMeshErrorMsh =
|
static const char* const kNoDecoderForMeshErrorMsh =
|
||||||
"no decoder found for mesh";
|
"no decoder found for mesh";
|
||||||
@@ -94,13 +86,8 @@ TEST_F(MjCMeshTest, UnknownMeshFormat) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
|
|
||||||
std::vector<std::string> invalid_names = {
|
std::vector<std::string> invalid_names = {"noextension", "anobj", "f",
|
||||||
"noextension",
|
"mesh.exe", "file%s"};
|
||||||
"anobj",
|
|
||||||
"f",
|
|
||||||
"mesh.exe",
|
|
||||||
"file%s"
|
|
||||||
};
|
|
||||||
mjVFS vfs;
|
mjVFS vfs;
|
||||||
mj_defaultVFS(&vfs);
|
mj_defaultVFS(&vfs);
|
||||||
|
|
||||||
@@ -108,9 +95,9 @@ TEST_F(MjCMeshTest, UnknownMeshFormat) {
|
|||||||
mj_addBufferVFS(&vfs, name.c_str(), nullptr, 0);
|
mj_addBufferVFS(&vfs, name.c_str(), nullptr, 0);
|
||||||
std::string xml = absl::StrFormat(xml_format, name);
|
std::string xml = absl::StrFormat(xml_format, name);
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model =
|
MjModelPtr model =
|
||||||
LoadModelFromString(xml.c_str(), error.data(), error.size(), &vfs);
|
LoadModelFromString(xml.c_str(), error.data(), error.size(), &vfs);
|
||||||
ASSERT_THAT(model, testing::IsNull())
|
ASSERT_THAT(model.get(), testing::IsNull())
|
||||||
<< "Should fail to load a mesh named: " << name;
|
<< "Should fail to load a mesh named: " << name;
|
||||||
EXPECT_THAT(error.data(), HasSubstr(kNoDecoderForMeshErrorMsh));
|
EXPECT_THAT(error.data(), HasSubstr(kNoDecoderForMeshErrorMsh));
|
||||||
EXPECT_THAT(error.data(), HasSubstr(name));
|
EXPECT_THAT(error.data(), HasSubstr(name));
|
||||||
@@ -141,8 +128,8 @@ TEST_F(MjCMeshTest, LoadMSHWithVFS) {
|
|||||||
mj_defaultVFS(vfs.get());
|
mj_defaultVFS(vfs.get());
|
||||||
|
|
||||||
// should fallback to OS filesystem
|
// should fallback to OS filesystem
|
||||||
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
||||||
mj_deleteVFS(vfs.get());
|
mj_deleteVFS(vfs.get());
|
||||||
}
|
}
|
||||||
@@ -167,8 +154,8 @@ TEST_F(MjCMeshTest, LoadOBJWithVFS) {
|
|||||||
mj_defaultVFS(vfs.get());
|
mj_defaultVFS(vfs.get());
|
||||||
|
|
||||||
// should fallback to OS filesystem
|
// should fallback to OS filesystem
|
||||||
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
||||||
mj_deleteVFS(vfs.get());
|
mj_deleteVFS(vfs.get());
|
||||||
}
|
}
|
||||||
@@ -193,8 +180,8 @@ TEST_F(MjCMeshTest, LoadSTLWithVFS) {
|
|||||||
mj_defaultVFS(vfs.get());
|
mj_defaultVFS(vfs.get());
|
||||||
|
|
||||||
// should fallback to OS filesystem
|
// should fallback to OS filesystem
|
||||||
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
||||||
mj_deleteVFS(vfs.get());
|
mj_deleteVFS(vfs.get());
|
||||||
}
|
}
|
||||||
@@ -221,8 +208,8 @@ TEST_F(MjCMeshTest, LoadMSHWithContentType) {
|
|||||||
mj_defaultVFS(vfs.get());
|
mj_defaultVFS(vfs.get());
|
||||||
|
|
||||||
// should try opening the file (not found obviously)
|
// should try opening the file (not found obviously)
|
||||||
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
||||||
mj_deleteVFS(vfs.get());
|
mj_deleteVFS(vfs.get());
|
||||||
}
|
}
|
||||||
@@ -247,8 +234,8 @@ TEST_F(MjCMeshTest, LoadOBJWithContentType) {
|
|||||||
mj_defaultVFS(vfs.get());
|
mj_defaultVFS(vfs.get());
|
||||||
|
|
||||||
// should try opening the file (not found obviously)
|
// should try opening the file (not found obviously)
|
||||||
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
||||||
mj_deleteVFS(vfs.get());
|
mj_deleteVFS(vfs.get());
|
||||||
}
|
}
|
||||||
@@ -273,8 +260,8 @@ TEST_F(MjCMeshTest, LoadSTLWithContentType) {
|
|||||||
mj_defaultVFS(vfs.get());
|
mj_defaultVFS(vfs.get());
|
||||||
|
|
||||||
// should try opening the file (not found obviously)
|
// should try opening the file (not found obviously)
|
||||||
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
||||||
mj_deleteVFS(vfs.get());
|
mj_deleteVFS(vfs.get());
|
||||||
}
|
}
|
||||||
@@ -300,8 +287,8 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeError) {
|
|||||||
mj_addBufferVFS(&vfs, "some_file", nullptr, 0);
|
mj_addBufferVFS(&vfs, "some_file", nullptr, 0);
|
||||||
|
|
||||||
// should error with unknown file type
|
// should error with unknown file type
|
||||||
mjModel* model = LoadModelFromString(xml, error, error_sz, &vfs);
|
MjModelPtr model = LoadModelFromString(xml, error, error_sz, &vfs);
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error, HasSubstr(kNoDecoderForMeshErrorMsh));
|
EXPECT_THAT(error, HasSubstr(kNoDecoderForMeshErrorMsh));
|
||||||
mj_deleteVFS(&vfs);
|
mj_deleteVFS(&vfs);
|
||||||
}
|
}
|
||||||
@@ -327,8 +314,8 @@ TEST_F(MjCMeshTest, LoadMSHWithInvalidContentType) {
|
|||||||
mj_addBufferVFS(&vfs, "some_file", nullptr, 0);
|
mj_addBufferVFS(&vfs, "some_file", nullptr, 0);
|
||||||
|
|
||||||
// should error with unknown file type
|
// should error with unknown file type
|
||||||
mjModel* model = LoadModelFromString(xml, error, error_sz, &vfs);
|
MjModelPtr model = LoadModelFromString(xml, error, error_sz, &vfs);
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error, HasSubstr(kNoDecoderForMeshErrorMsh));
|
EXPECT_THAT(error, HasSubstr(kNoDecoderForMeshErrorMsh));
|
||||||
mj_deleteVFS(&vfs);
|
mj_deleteVFS(&vfs);
|
||||||
}
|
}
|
||||||
@@ -353,8 +340,8 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeParam) {
|
|||||||
mj_defaultVFS(vfs.get());
|
mj_defaultVFS(vfs.get());
|
||||||
|
|
||||||
// should try opening the file (not found obviously)
|
// should try opening the file (not found obviously)
|
||||||
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
EXPECT_THAT(error, HasSubstr("Error opening file"));
|
||||||
mj_deleteVFS(vfs.get());
|
mj_deleteVFS(vfs.get());
|
||||||
}
|
}
|
||||||
@@ -438,8 +425,7 @@ TEST_F(MjCMeshTest, SaveMeshOnce) {
|
|||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
|
||||||
// Confirm the mesh file is loaded and stored in paths
|
// Confirm the mesh file is loaded and stored in paths
|
||||||
EXPECT_EQ(
|
EXPECT_EQ(std::string(&model->paths[model->mesh_pathadr[0]]), "cube.obj");
|
||||||
std::string(&model->paths[model->mesh_pathadr[0]]), "cube.obj");
|
|
||||||
std::string saved_xml = SaveAndReadXml(model);
|
std::string saved_xml = SaveAndReadXml(model);
|
||||||
EXPECT_THAT(saved_xml, Not(testing::HasSubstr("vertex")));
|
EXPECT_THAT(saved_xml, Not(testing::HasSubstr("vertex")));
|
||||||
mj_deleteModel(model);
|
mj_deleteModel(model);
|
||||||
@@ -457,9 +443,8 @@ TEST_F(MjCMeshTest, TinyMeshLoads) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ------------- test max hull vert -------------------------------------------
|
// ------------- test max hull vert -------------------------------------------
|
||||||
@@ -498,9 +483,8 @@ TEST_F(MjCMeshTest, FaceNormalAutogenerated) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ------------- test inertia -------------------------------------------------
|
// ------------- test inertia -------------------------------------------------
|
||||||
@@ -521,9 +505,8 @@ TEST_F(MjCMeshTest, SmallInertiaLoads) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, TinyInertiaFails) {
|
TEST_F(MjCMeshTest, TinyInertiaFails) {
|
||||||
@@ -586,9 +569,11 @@ TEST_F(MjCMeshTest, FlippedFaceFailsExactInertia) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, testing::IsNull());
|
EXPECT_THAT(model.get(), testing::IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr(
|
EXPECT_THAT(
|
||||||
|
error.data(),
|
||||||
|
HasSubstr(
|
||||||
"Error: faces of mesh 'example_mesh' have inconsistent orientation. "
|
"Error: faces of mesh 'example_mesh' have inconsistent orientation. "
|
||||||
"Please check the faces containing the vertices 1 and 2."));
|
"Please check the faces containing the vertices 1 and 2."));
|
||||||
}
|
}
|
||||||
@@ -622,10 +607,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedWorld) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
CheckTetrahedronWasRescaled(model);
|
CheckTetrahedronWasRescaled(model.get());
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, FlippedFaceAllowedNoMass) {
|
TEST_F(MjCMeshTest, FlippedFaceAllowedNoMass) {
|
||||||
@@ -644,10 +628,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNoMass) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
CheckTetrahedronWasRescaled(model);
|
CheckTetrahedronWasRescaled(model.get());
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, FlippedFaceAllowedInertial) {
|
TEST_F(MjCMeshTest, FlippedFaceAllowedInertial) {
|
||||||
@@ -667,10 +650,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedInertial) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
CheckTetrahedronWasRescaled(model);
|
CheckTetrahedronWasRescaled(model.get());
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) {
|
TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) {
|
||||||
@@ -689,10 +671,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
CheckTetrahedronWasRescaled(model);
|
CheckTetrahedronWasRescaled(model.get());
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, AreaTooSmall) {
|
TEST_F(MjCMeshTest, AreaTooSmall) {
|
||||||
@@ -711,8 +692,8 @@ TEST_F(MjCMeshTest, AreaTooSmall) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small"));
|
EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -732,10 +713,9 @@ TEST_F(MjCMeshTest, VolumeTooSmall) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, testing::IsNull());
|
EXPECT_THAT(model.get(), testing::IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small"));
|
EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small"));
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, VisualVolumeTooSmall) {
|
TEST_F(MjCMeshTest, VisualVolumeTooSmall) {
|
||||||
@@ -759,10 +739,9 @@ TEST_F(MjCMeshTest, VisualVolumeTooSmall) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, testing::IsNull());
|
EXPECT_THAT(model.get(), testing::IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small"));
|
EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small"));
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, VisualVolumeSmallAllowedShell) {
|
TEST_F(MjCMeshTest, VisualVolumeSmallAllowedShell) {
|
||||||
@@ -786,12 +765,11 @@ TEST_F(MjCMeshTest, VisualVolumeSmallAllowedShell) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
EXPECT_LE(mju_abs(model->geom_size[0]), 1);
|
EXPECT_LE(mju_abs(model->geom_size[0]), 1);
|
||||||
EXPECT_LE(mju_abs(model->geom_size[1]), 1);
|
EXPECT_LE(mju_abs(model->geom_size[1]), 1);
|
||||||
EXPECT_LE(mju_abs(model->geom_size[2]), 1);
|
EXPECT_LE(mju_abs(model->geom_size[2]), 1);
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
|
TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
|
||||||
@@ -810,12 +788,11 @@ TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
EXPECT_LE(mju_abs(model->geom_size[0]), 1);
|
EXPECT_LE(mju_abs(model->geom_size[0]), 1);
|
||||||
EXPECT_LE(mju_abs(model->geom_size[1]), 1);
|
EXPECT_LE(mju_abs(model->geom_size[1]), 1);
|
||||||
EXPECT_LE(mju_abs(model->geom_size[2]), 1);
|
EXPECT_LE(mju_abs(model->geom_size[2]), 1);
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) {
|
TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) {
|
||||||
@@ -829,8 +806,8 @@ TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, testing::IsNull());
|
EXPECT_THAT(model.get(), testing::IsNull());
|
||||||
EXPECT_THAT(error.data(),
|
EXPECT_THAT(error.data(),
|
||||||
HasSubstr("2d elasticity requires positive thickness"));
|
HasSubstr("2d elasticity requires positive thickness"));
|
||||||
}
|
}
|
||||||
@@ -847,9 +824,8 @@ TEST_F(MjCMeshTest, InterpolatedFlexSupportsBendElasticityWithWarning) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, testing::NotNull()) << error.data();
|
EXPECT_THAT(model.get(), testing::NotNull()) << error.data();
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, InterpolatedFlexSupportsBothElasticityWithWarning) {
|
TEST_F(MjCMeshTest, InterpolatedFlexSupportsBothElasticityWithWarning) {
|
||||||
@@ -864,9 +840,8 @@ TEST_F(MjCMeshTest, InterpolatedFlexSupportsBothElasticityWithWarning) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, testing::NotNull()) << error.data();
|
EXPECT_THAT(model.get(), testing::NotNull()) << error.data();
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) {
|
TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) {
|
||||||
@@ -880,8 +855,8 @@ TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, testing::IsNull());
|
EXPECT_THAT(model.get(), testing::IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("2d elasticity requires 2d flex"));
|
EXPECT_THAT(error.data(), HasSubstr("2d elasticity requires 2d flex"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -919,12 +894,13 @@ TEST_F(MjCMeshTest, VolumeNegativeThrowsError) {
|
|||||||
|
|
||||||
std::string xml_str = xml;
|
std::string xml_str = xml;
|
||||||
absl::StrReplaceAll({{"MESH_DEFINITIONS", mesh_definitions},
|
absl::StrReplaceAll({{"MESH_DEFINITIONS", mesh_definitions},
|
||||||
{"GEOM_DEFINITIONS", geom_definitions}}, &xml_str);
|
{"GEOM_DEFINITIONS", geom_definitions}},
|
||||||
|
&xml_str);
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml_str.c_str(),
|
MjModelPtr model =
|
||||||
error.data(), error.size());
|
LoadModelFromString(xml_str.c_str(), error.data(), error.size());
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("mesh volume is negative"));
|
EXPECT_THAT(error.data(), HasSubstr("mesh volume is negative"));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -970,8 +946,8 @@ TEST_F(MjCMeshTest, ExactConcaveInertia) {
|
|||||||
mjtNum d_cube = .5 - model->body_ipos[5];
|
mjtNum d_cube = .5 - model->body_ipos[5];
|
||||||
mjtNum d_hole = .55 - model->body_ipos[5];
|
mjtNum d_hole = .55 - model->body_ipos[5];
|
||||||
mjtNum I1 = I_cube - m_hole * (.8 * .8 + .8 * .8) / 12;
|
mjtNum I1 = I_cube - m_hole * (.8 * .8 + .8 * .8) / 12;
|
||||||
mjtNum I2 = I_cube - m_hole*(.8*.8 + .9*.9)/12
|
mjtNum I2 = I_cube - m_hole * (.8 * .8 + .9 * .9) / 12 +
|
||||||
+ m_cube*d_cube*d_cube - m_hole*d_hole*d_hole;
|
m_cube * d_cube * d_cube - m_hole * d_hole * d_hole;
|
||||||
EXPECT_NEAR(model->body_mass[1], m_concave_cube, kMaxAbsErr);
|
EXPECT_NEAR(model->body_mass[1], m_concave_cube, kMaxAbsErr);
|
||||||
EXPECT_NEAR(model->body_mass[2], m_concave_cube, kMaxAbsErr);
|
EXPECT_NEAR(model->body_mass[2], m_concave_cube, kMaxAbsErr);
|
||||||
EXPECT_NEAR(model->body_mass[3], m_concave_cube, kMaxAbsErr);
|
EXPECT_NEAR(model->body_mass[3], m_concave_cube, kMaxAbsErr);
|
||||||
@@ -1026,8 +1002,8 @@ TEST_F(MjCMeshTest, MeshPosQuat) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
// loading the mesh results in an offset of the geom's pos and quat due to the
|
// loading the mesh results in an offset of the geom's pos and quat due to the
|
||||||
// fact that the geom's center is not the volumetric center of the mesh. To
|
// fact that the geom's center is not the volumetric center of the mesh. To
|
||||||
// recover the geom's originally specified pose, the offset used is stored in
|
// recover the geom's originally specified pose, the offset used is stored in
|
||||||
@@ -1035,16 +1011,15 @@ TEST_F(MjCMeshTest, MeshPosQuat) {
|
|||||||
// and orientation, first invert the specified mesh_pos and mesh_quat
|
// and orientation, first invert the specified mesh_pos and mesh_quat
|
||||||
mjtNum inverse_mesh_pos[3];
|
mjtNum inverse_mesh_pos[3];
|
||||||
mjtNum inverse_mesh_quat[4];
|
mjtNum inverse_mesh_quat[4];
|
||||||
mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
|
mju_negPose(inverse_mesh_pos, inverse_mesh_quat, &model->mesh_pos[0],
|
||||||
&model->mesh_pos[0], &model->mesh_quat[0]);
|
&model->mesh_quat[0]);
|
||||||
|
|
||||||
// apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and
|
// apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and
|
||||||
// quat. It should match the originally specified values
|
// quat. It should match the originally specified values
|
||||||
mjtNum recovered_pos[3];
|
mjtNum recovered_pos[3];
|
||||||
mjtNum recovered_quat[4];
|
mjtNum recovered_quat[4];
|
||||||
mju_mulPose(recovered_pos, recovered_quat,
|
mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[0],
|
||||||
&model->geom_pos[0], &model->geom_quat[0],
|
&model->geom_quat[0], inverse_mesh_pos, inverse_mesh_quat);
|
||||||
inverse_mesh_pos, inverse_mesh_quat);
|
|
||||||
EXPECT_NEAR(recovered_pos[0], 0, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_pos[0], 0, MjTol(1e-12, 1e-6));
|
||||||
EXPECT_NEAR(recovered_pos[1], 0, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_pos[1], 0, MjTol(1e-12, 1e-6));
|
||||||
EXPECT_NEAR(recovered_pos[2], 0, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_pos[2], 0, MjTol(1e-12, 1e-6));
|
||||||
@@ -1055,11 +1030,10 @@ TEST_F(MjCMeshTest, MeshPosQuat) {
|
|||||||
EXPECT_NEAR(recovered_quat[3], 0, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_quat[3], 0, MjTol(1e-12, 1e-6));
|
||||||
|
|
||||||
// same test on the other geom
|
// same test on the other geom
|
||||||
mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
|
mju_negPose(inverse_mesh_pos, inverse_mesh_quat, &model->mesh_pos[0],
|
||||||
&model->mesh_pos[0], &model->mesh_quat[0]);
|
&model->mesh_quat[0]);
|
||||||
mju_mulPose(recovered_pos, recovered_quat,
|
mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[3],
|
||||||
&model->geom_pos[3], &model->geom_quat[4],
|
&model->geom_quat[4], inverse_mesh_pos, inverse_mesh_quat);
|
||||||
inverse_mesh_pos, inverse_mesh_quat);
|
|
||||||
EXPECT_NEAR(recovered_pos[0], 1, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_pos[0], 1, MjTol(1e-12, 1e-6));
|
||||||
EXPECT_NEAR(recovered_pos[1], 2, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_pos[1], 2, MjTol(1e-12, 1e-6));
|
||||||
EXPECT_NEAR(recovered_pos[2], 3, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_pos[2], 3, MjTol(1e-12, 1e-6));
|
||||||
@@ -1068,8 +1042,6 @@ TEST_F(MjCMeshTest, MeshPosQuat) {
|
|||||||
EXPECT_NEAR(recovered_quat[1], 0.5, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_quat[1], 0.5, MjTol(1e-12, 1e-6));
|
||||||
EXPECT_NEAR(recovered_quat[2], 0.5, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_quat[2], 0.5, MjTol(1e-12, 1e-6));
|
||||||
EXPECT_NEAR(recovered_quat[3], 0.5, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_quat[3], 0.5, MjTol(1e-12, 1e-6));
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
|
TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
|
||||||
@@ -1085,8 +1057,8 @@ TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
// loading the mesh results in an offset of the geom's pos and quat due to the
|
// loading the mesh results in an offset of the geom's pos and quat due to the
|
||||||
// fact that the geom's center is not the volumetric center of the mesh. To
|
// fact that the geom's center is not the volumetric center of the mesh. To
|
||||||
// recover the geom's originally specified pose, the offset used is stored in
|
// recover the geom's originally specified pose, the offset used is stored in
|
||||||
@@ -1094,16 +1066,15 @@ TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
|
|||||||
// and orientation, first invert the specified mesh_pos and mesh_quat
|
// and orientation, first invert the specified mesh_pos and mesh_quat
|
||||||
mjtNum inverse_mesh_pos[3];
|
mjtNum inverse_mesh_pos[3];
|
||||||
mjtNum inverse_mesh_quat[4];
|
mjtNum inverse_mesh_quat[4];
|
||||||
mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
|
mju_negPose(inverse_mesh_pos, inverse_mesh_quat, &model->mesh_pos[0],
|
||||||
&model->mesh_pos[0], &model->mesh_quat[0]);
|
&model->mesh_quat[0]);
|
||||||
|
|
||||||
// apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and
|
// apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and
|
||||||
// quat. It should match the originally specified values
|
// quat. It should match the originally specified values
|
||||||
mjtNum recovered_pos[3];
|
mjtNum recovered_pos[3];
|
||||||
mjtNum recovered_quat[4];
|
mjtNum recovered_quat[4];
|
||||||
mju_mulPose(recovered_pos, recovered_quat,
|
mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[0],
|
||||||
&model->geom_pos[0], &model->geom_quat[0],
|
&model->geom_quat[0], inverse_mesh_pos, inverse_mesh_quat);
|
||||||
inverse_mesh_pos, inverse_mesh_quat);
|
|
||||||
EXPECT_NEAR(recovered_pos[0], 0, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_pos[0], 0, MjTol(1e-12, 1e-6));
|
||||||
EXPECT_NEAR(recovered_pos[1], 0, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_pos[1], 0, MjTol(1e-12, 1e-6));
|
||||||
EXPECT_NEAR(recovered_pos[2], 0, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_pos[2], 0, MjTol(1e-12, 1e-6));
|
||||||
@@ -1114,11 +1085,10 @@ TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
|
|||||||
EXPECT_NEAR(recovered_quat[3], 0, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_quat[3], 0, MjTol(1e-12, 1e-6));
|
||||||
|
|
||||||
// same test on the other geom
|
// same test on the other geom
|
||||||
mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
|
mju_negPose(inverse_mesh_pos, inverse_mesh_quat, &model->mesh_pos[0],
|
||||||
&model->mesh_pos[0], &model->mesh_quat[0]);
|
&model->mesh_quat[0]);
|
||||||
mju_mulPose(recovered_pos, recovered_quat,
|
mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[3],
|
||||||
&model->geom_pos[3], &model->geom_quat[4],
|
&model->geom_quat[4], inverse_mesh_pos, inverse_mesh_quat);
|
||||||
inverse_mesh_pos, inverse_mesh_quat);
|
|
||||||
EXPECT_NEAR(recovered_pos[0], 1, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_pos[0], 1, MjTol(1e-12, 1e-6));
|
||||||
EXPECT_NEAR(recovered_pos[1], 2, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_pos[1], 2, MjTol(1e-12, 1e-6));
|
||||||
EXPECT_NEAR(recovered_pos[2], 3, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_pos[2], 3, MjTol(1e-12, 1e-6));
|
||||||
@@ -1127,7 +1097,6 @@ TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
|
|||||||
EXPECT_NEAR(recovered_quat[1], 0.5, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_quat[1], 0.5, MjTol(1e-12, 1e-6));
|
||||||
EXPECT_NEAR(recovered_quat[2], 0.5, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_quat[2], 0.5, MjTol(1e-12, 1e-6));
|
||||||
EXPECT_NEAR(recovered_quat[3], 0.5, MjTol(1e-12, 1e-6));
|
EXPECT_NEAR(recovered_quat[3], 0.5, MjTol(1e-12, 1e-6));
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, MeshScale) {
|
TEST_F(MjCMeshTest, MeshScale) {
|
||||||
@@ -1144,12 +1113,11 @@ TEST_F(MjCMeshTest, MeshScale) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1));
|
EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1));
|
||||||
EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1));
|
EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1));
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, NegativeScaleUserMeshCompiles) {
|
TEST_F(MjCMeshTest, NegativeScaleUserMeshCompiles) {
|
||||||
@@ -1166,9 +1134,8 @@ TEST_F(MjCMeshTest, NegativeScaleUserMeshCompiles) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, NegativeScaleUserMeshMatchesPositiveScale) {
|
TEST_F(MjCMeshTest, NegativeScaleUserMeshMatchesPositiveScale) {
|
||||||
@@ -1199,11 +1166,11 @@ TEST_F(MjCMeshTest, NegativeScaleUserMeshMatchesPositiveScale) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* pos_model = LoadModelFromString(pos_xml, error, sizeof(error));
|
MjModelPtr pos_model = LoadModelFromString(pos_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(pos_model, NotNull()) << error;
|
ASSERT_THAT(pos_model.get(), NotNull()) << error;
|
||||||
|
|
||||||
mjModel* neg_model = LoadModelFromString(neg_xml, error, sizeof(error));
|
MjModelPtr neg_model = LoadModelFromString(neg_xml, error, sizeof(error));
|
||||||
ASSERT_THAT(neg_model, NotNull()) << error;
|
ASSERT_THAT(neg_model.get(), NotNull()) << error;
|
||||||
|
|
||||||
ASSERT_EQ(pos_model->nmeshface, neg_model->nmeshface);
|
ASSERT_EQ(pos_model->nmeshface, neg_model->nmeshface);
|
||||||
|
|
||||||
@@ -1212,9 +1179,6 @@ TEST_F(MjCMeshTest, NegativeScaleUserMeshMatchesPositiveScale) {
|
|||||||
EXPECT_EQ(pos_model->mesh_face[3 * i + 1], neg_model->mesh_face[3 * i + 2]);
|
EXPECT_EQ(pos_model->mesh_face[3 * i + 1], neg_model->mesh_face[3 * i + 2]);
|
||||||
EXPECT_EQ(pos_model->mesh_face[3 * i + 2], neg_model->mesh_face[3 * i + 1]);
|
EXPECT_EQ(pos_model->mesh_face[3 * i + 2], neg_model->mesh_face[3 * i + 1]);
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteModel(pos_model);
|
|
||||||
mj_deleteModel(neg_model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, ShellInertiaTest) {
|
TEST_F(MjCMeshTest, ShellInertiaTest) {
|
||||||
@@ -1231,12 +1195,11 @@ TEST_F(MjCMeshTest, ShellInertiaTest) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
|
|
||||||
EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1));
|
EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1));
|
||||||
EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1));
|
EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1));
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ----------------------------- texcoord -------------------------------------
|
// ----------------------------- texcoord -------------------------------------
|
||||||
@@ -1251,9 +1214,8 @@ TEST_F(MjCMeshTest, CreateFaceTexCoord) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, UseFaceTexCoord) {
|
TEST_F(MjCMeshTest, UseFaceTexCoord) {
|
||||||
@@ -1267,8 +1229,8 @@ TEST_F(MjCMeshTest, UseFaceTexCoord) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, NotNull()) << error.data();
|
EXPECT_THAT(model.get(), NotNull()) << error.data();
|
||||||
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[0]], .0);
|
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[0]], .0);
|
||||||
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[1]], .2);
|
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[1]], .2);
|
||||||
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[2]], .1);
|
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[2]], .1);
|
||||||
@@ -1281,7 +1243,6 @@ TEST_F(MjCMeshTest, UseFaceTexCoord) {
|
|||||||
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[9]], .1);
|
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[9]], .1);
|
||||||
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[10]], .2);
|
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[10]], .2);
|
||||||
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[11]], .3);
|
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[11]], .3);
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, MissingTexCoord) {
|
TEST_F(MjCMeshTest, MissingTexCoord) {
|
||||||
@@ -1294,8 +1255,8 @@ TEST_F(MjCMeshTest, MissingTexCoord) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, testing::IsNull());
|
EXPECT_THAT(model.get(), testing::IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("texcoord must be 2*nv"));
|
EXPECT_THAT(error.data(), HasSubstr("texcoord must be 2*nv"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1312,10 +1273,9 @@ TEST_F(MjCMeshTest, NaNConvexHullDisallowed) {
|
|||||||
MockWarningHandler warning_handler;
|
MockWarningHandler warning_handler;
|
||||||
warning_handler.ExpectWarnings();
|
warning_handler.ExpectWarnings();
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, testing::IsNull());
|
EXPECT_THAT(model.get(), testing::IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("vertex coordinate 0 is not finite"));
|
EXPECT_THAT(error.data(), HasSubstr("vertex coordinate 0 is not finite"));
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, InvalidIndexInFace) {
|
TEST_F(MjCMeshTest, InvalidIndexInFace) {
|
||||||
@@ -1333,10 +1293,9 @@ TEST_F(MjCMeshTest, InvalidIndexInFace) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, IsNull());
|
ASSERT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error, HasSubstr("in face 0, vertex index 6 does not exist"));
|
EXPECT_THAT(error, HasSubstr("in face 0, vertex index 6 does not exist"));
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, QhullCache) {
|
TEST_F(MjCMeshTest, QhullCache) {
|
||||||
@@ -1367,17 +1326,16 @@ TEST_F(MjCMeshTest, QhullCache) {
|
|||||||
mj_addFileVFS(&vfs, "", GetTestDataFilePath(kCubeCompletePath).c_str());
|
mj_addFileVFS(&vfs, "", GetTestDataFilePath(kCubeCompletePath).c_str());
|
||||||
|
|
||||||
std::array<char, 1000> error;
|
std::array<char, 1000> error;
|
||||||
mjModel* model = LoadModelFromString(xml1, error.data(), error.size(), &vfs);
|
MjModelPtr model =
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
LoadModelFromString(xml1, error.data(), error.size(), &vfs);
|
||||||
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
EXPECT_THAT(model->mesh_graphadr[0], -1);
|
EXPECT_THAT(model->mesh_graphadr[0], -1);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
|
|
||||||
model = LoadModelFromString(xml2, error.data(), error.size(), &vfs);
|
model = LoadModelFromString(xml2, error.data(), error.size(), &vfs);
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
EXPECT_GT(model->mesh_graphadr[0], -1);
|
EXPECT_GT(model->mesh_graphadr[0], -1);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
mj_deleteVFS(&vfs);
|
mj_deleteVFS(&vfs);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1395,8 +1353,8 @@ TEST_F(MjCMeshTest, ColocatedMeshError) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("colocated"));
|
EXPECT_THAT(error.data(), HasSubstr("colocated"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1414,8 +1372,8 @@ TEST_F(MjCMeshTest, CollinearMeshError) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("collinear"));
|
EXPECT_THAT(error.data(), HasSubstr("collinear"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1433,8 +1391,8 @@ TEST_F(MjCMeshTest, CoplanarMeshError) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("coplanar"));
|
EXPECT_THAT(error.data(), HasSubstr("coplanar"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1478,8 +1436,7 @@ bool AreAabbsAdjacent(const mjtNum* aabb1, const mjtNum* aabb2) {
|
|||||||
const mjtNum half_size1 = aabb1[dim + 3];
|
const mjtNum half_size1 = aabb1[dim + 3];
|
||||||
const mjtNum center2 = aabb2[dim];
|
const mjtNum center2 = aabb2[dim];
|
||||||
const mjtNum half_size2 = aabb2[dim + 3];
|
const mjtNum half_size2 = aabb2[dim + 3];
|
||||||
const mjtNum gap =
|
const mjtNum gap = std::abs(center1 - center2) - (half_size1 + half_size2);
|
||||||
std::abs(center1 - center2) - (half_size1 + half_size2);
|
|
||||||
if (std::abs(gap) < kEps) {
|
if (std::abs(gap) < kEps) {
|
||||||
touching_dims++;
|
touching_dims++;
|
||||||
} else if (gap < -kEps) {
|
} else if (gap < -kEps) {
|
||||||
@@ -1533,15 +1490,15 @@ TEST_F(MjCMeshTest, OctreeIsBalanced) {
|
|||||||
const int level2 = model->oct_depth[octree_adr + node2_idx];
|
const int level2 = model->oct_depth[octree_adr + node2_idx];
|
||||||
if (std::abs(level1 - level2) > 1) {
|
if (std::abs(level1 - level2) > 1) {
|
||||||
if (unbalanced_pairs < 10) {
|
if (unbalanced_pairs < 10) {
|
||||||
ADD_FAILURE()
|
ADD_FAILURE() << "Nodes " << node1_idx << " (level " << level1
|
||||||
<< "Nodes " << node1_idx << " (level " << level1 << ") and "
|
<< ") and " << node2_idx << " (level " << level2
|
||||||
<< node2_idx << " (level " << level2 << ") are not balanced."
|
<< ") are not balanced."
|
||||||
<< "\nAABB1: center=(" << aabb1[0] << ", " << aabb1[1] << ", "
|
<< "\nAABB1: center=(" << aabb1[0] << ", " << aabb1[1]
|
||||||
<< aabb1[2] << "), half_size=(" << aabb1[3] << ", "
|
<< ", " << aabb1[2] << "), half_size=(" << aabb1[3]
|
||||||
<< aabb1[4] << ", " << aabb1[5] << ")"
|
<< ", " << aabb1[4] << ", " << aabb1[5] << ")"
|
||||||
<< "\nAABB2: center=(" << aabb2[0] << ", " << aabb2[1] << ", "
|
<< "\nAABB2: center=(" << aabb2[0] << ", " << aabb2[1]
|
||||||
<< aabb2[2] << "), half_size=(" << aabb2[3] << ", "
|
<< ", " << aabb2[2] << "), half_size=(" << aabb2[3]
|
||||||
<< aabb2[4] << ", " << aabb2[5] << ")";
|
<< ", " << aabb2[4] << ", " << aabb2[5] << ")";
|
||||||
}
|
}
|
||||||
unbalanced_pairs++;
|
unbalanced_pairs++;
|
||||||
}
|
}
|
||||||
@@ -1610,8 +1567,7 @@ TEST_F(MjCMeshTest, OctreeHangingNodeInterpolation) {
|
|||||||
|
|
||||||
// decide which node is finer and which is coarser
|
// decide which node is finer and which is coarser
|
||||||
const int finer_node_idx = (level1 > level2) ? node1_idx : node2_idx;
|
const int finer_node_idx = (level1 > level2) ? node1_idx : node2_idx;
|
||||||
const int coarser_node_idx =
|
const int coarser_node_idx = (level1 > level2) ? node2_idx : node1_idx;
|
||||||
(level1 > level2) ? node2_idx : node1_idx;
|
|
||||||
const mjtNum* coarser_aabb =
|
const mjtNum* coarser_aabb =
|
||||||
&model->oct_aabb[(octree_adr + coarser_node_idx) * 6];
|
&model->oct_aabb[(octree_adr + coarser_node_idx) * 6];
|
||||||
const mjtNum* finer_aabb =
|
const mjtNum* finer_aabb =
|
||||||
@@ -1729,8 +1685,7 @@ TEST_F(MjCMeshTest, OctreeNotComputedForNonSDF) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
mjtNum CubeSDF(mjtNum p[3], mjtNum b[3]) {
|
mjtNum CubeSDF(mjtNum p[3], mjtNum b[3]) {
|
||||||
mjtNum q[3] = {mju_abs(p[0]) - b[0],
|
mjtNum q[3] = {mju_abs(p[0]) - b[0], mju_abs(p[1]) - b[1],
|
||||||
mju_abs(p[1]) - b[1],
|
|
||||||
mju_abs(p[2]) - b[2]};
|
mju_abs(p[2]) - b[2]};
|
||||||
return mju_sqrt(std::pow(std::max(q[0], (mjtNum)0), 2) +
|
return mju_sqrt(std::pow(std::max(q[0], (mjtNum)0), 2) +
|
||||||
std::pow(std::max(q[1], (mjtNum)0), 2) +
|
std::pow(std::max(q[1], (mjtNum)0), 2) +
|
||||||
@@ -1751,12 +1706,12 @@ TEST_F(MjCMeshTest, OctreeCube) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
EXPECT_EQ(m->noct, 63497);
|
EXPECT_EQ(m->noct, 63497);
|
||||||
mjData* d = mj_makeData(m);
|
MjDataPtr d = MakeData(m);
|
||||||
ASSERT_THAT(d, NotNull());
|
ASSERT_THAT(d, NotNull());
|
||||||
mj_forward(m, d);
|
mj_forward(m.get(), d.get());
|
||||||
mjSDF sdf;
|
mjSDF sdf;
|
||||||
int instance = 0;
|
int instance = 0;
|
||||||
mjtGeom geomtype = mjGEOM_SDF;
|
mjtGeom geomtype = mjGEOM_SDF;
|
||||||
@@ -1768,11 +1723,10 @@ TEST_F(MjCMeshTest, OctreeCube) {
|
|||||||
mjtNum pnt[3][3] = {{1, 1, 1}, {.5, .5, .5}, {.5, 0, 0}};
|
mjtNum pnt[3][3] = {{1, 1, 1}, {.5, .5, .5}, {.5, 0, 0}};
|
||||||
mjtNum size[3] = {1, 1, 1};
|
mjtNum size[3] = {1, 1, 1};
|
||||||
for (int i = 0; i < 3; ++i) {
|
for (int i = 0; i < 3; ++i) {
|
||||||
EXPECT_NEAR(mjc_distance(m, d, &sdf, pnt[i]), CubeSDF(pnt[i], size), 1e-1)
|
EXPECT_NEAR(mjc_distance(m.get(), d.get(), &sdf, pnt[i]),
|
||||||
|
CubeSDF(pnt[i], size), 1e-1)
|
||||||
<< "i = " << i;
|
<< "i = " << i;
|
||||||
}
|
}
|
||||||
mj_deleteModel(m);
|
|
||||||
mj_deleteData(d);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, HemisphereSizes) {
|
TEST_F(MjCMeshTest, HemisphereSizes) {
|
||||||
@@ -1786,15 +1740,14 @@ TEST_F(MjCMeshTest, HemisphereSizes) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << error.data();
|
ASSERT_THAT(model.get(), NotNull()) << error.data();
|
||||||
EXPECT_EQ(model->mesh_vertnum[0], 2 * (0 + 1) * (0 + 2) + 2);
|
EXPECT_EQ(model->mesh_vertnum[0], 2 * (0 + 1) * (0 + 2) + 2);
|
||||||
EXPECT_EQ(model->mesh_vertnum[1], 2 * (1 + 1) * (1 + 2) + 2);
|
EXPECT_EQ(model->mesh_vertnum[1], 2 * (1 + 1) * (1 + 2) + 2);
|
||||||
EXPECT_EQ(model->mesh_vertnum[2], 2 * (2 + 1) * (2 + 2) + 2);
|
EXPECT_EQ(model->mesh_vertnum[2], 2 * (2 + 1) * (2 + 2) + 2);
|
||||||
EXPECT_EQ(model->mesh_facenum[0], 4 * (0 + 1) * (0 + 2));
|
EXPECT_EQ(model->mesh_facenum[0], 4 * (0 + 1) * (0 + 2));
|
||||||
EXPECT_EQ(model->mesh_facenum[1], 4 * (1 + 1) * (1 + 2));
|
EXPECT_EQ(model->mesh_facenum[1], 4 * (1 + 1) * (1 + 2));
|
||||||
EXPECT_EQ(model->mesh_facenum[2], 4 * (2 + 1) * (2 + 2));
|
EXPECT_EQ(model->mesh_facenum[2], 4 * (2 + 1) * (2 + 2));
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, SphereSizes) {
|
TEST_F(MjCMeshTest, SphereSizes) {
|
||||||
@@ -1808,15 +1761,14 @@ TEST_F(MjCMeshTest, SphereSizes) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << error.data();
|
ASSERT_THAT(model.get(), NotNull()) << error.data();
|
||||||
EXPECT_EQ(model->mesh_vertnum[0], 2 + 10 * std::pow(4, 0));
|
EXPECT_EQ(model->mesh_vertnum[0], 2 + 10 * std::pow(4, 0));
|
||||||
EXPECT_EQ(model->mesh_vertnum[1], 2 + 10 * std::pow(4, 1));
|
EXPECT_EQ(model->mesh_vertnum[1], 2 + 10 * std::pow(4, 1));
|
||||||
EXPECT_EQ(model->mesh_vertnum[2], 2 + 10 * std::pow(4, 2));
|
EXPECT_EQ(model->mesh_vertnum[2], 2 + 10 * std::pow(4, 2));
|
||||||
EXPECT_EQ(model->mesh_facenum[0], 20 * std::pow(4, 0));
|
EXPECT_EQ(model->mesh_facenum[0], 20 * std::pow(4, 0));
|
||||||
EXPECT_EQ(model->mesh_facenum[1], 20 * std::pow(4, 1));
|
EXPECT_EQ(model->mesh_facenum[1], 20 * std::pow(4, 1));
|
||||||
EXPECT_EQ(model->mesh_facenum[2], 20 * std::pow(4, 2));
|
EXPECT_EQ(model->mesh_facenum[2], 20 * std::pow(4, 2));
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MjCMeshTest, MeshMaterial) {
|
TEST_F(MjCMeshTest, MeshMaterial) {
|
||||||
@@ -1835,11 +1787,10 @@ TEST_F(MjCMeshTest, MeshMaterial) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << error.data();
|
ASSERT_THAT(model.get(), NotNull()) << error.data();
|
||||||
EXPECT_EQ(model->geom_matid[0], 1);
|
EXPECT_EQ(model->geom_matid[0], 1);
|
||||||
EXPECT_EQ(model->geom_matid[1], 0);
|
EXPECT_EQ(model->geom_matid[1], 0);
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
+69
-108
@@ -95,15 +95,13 @@ TEST_F(UserModelTest, WeldRootID) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, NotNull());
|
EXPECT_THAT(model.get(), NotNull());
|
||||||
|
|
||||||
EXPECT_THAT(AsVector(model->body_rootid, model->nbody),
|
EXPECT_THAT(AsVector(model->body_rootid, model->nbody),
|
||||||
ElementsAre(0, 1, 1, 3, 3, 5, 5, 7, 7, 9, 9));
|
ElementsAre(0, 1, 1, 3, 3, 5, 5, 7, 7, 9, 9));
|
||||||
EXPECT_THAT(AsVector(model->body_weldid, model->nbody),
|
EXPECT_THAT(AsVector(model->body_weldid, model->nbody),
|
||||||
ElementsAre(0, 1, 2, 3, 3, 0, 0, 0, 0, 0, 10));
|
ElementsAre(0, 1, 2, 3, 3, 0, 0, 0, 0, 0, 10));
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserModelTest, RepeatedNames) {
|
TEST_F(UserModelTest, RepeatedNames) {
|
||||||
@@ -119,8 +117,8 @@ TEST_F(UserModelTest, RepeatedNames) {
|
|||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, IsNull());
|
EXPECT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(error.data(), HasSubstr("repeated name 'geom1' in geom"));
|
EXPECT_THAT(error.data(), HasSubstr("repeated name 'geom1' in geom"));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -143,8 +141,8 @@ TEST_F(UserModelTest, SameFrame) {
|
|||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << error.data();
|
ASSERT_THAT(model.get(), NotNull()) << error.data();
|
||||||
EXPECT_EQ(model->geom_sameframe[0], mjSAMEFRAME_NONE);
|
EXPECT_EQ(model->geom_sameframe[0], mjSAMEFRAME_NONE);
|
||||||
EXPECT_EQ(model->geom_sameframe[1], mjSAMEFRAME_BODY);
|
EXPECT_EQ(model->geom_sameframe[1], mjSAMEFRAME_BODY);
|
||||||
EXPECT_EQ(model->geom_sameframe[2], mjSAMEFRAME_INERTIA);
|
EXPECT_EQ(model->geom_sameframe[2], mjSAMEFRAME_INERTIA);
|
||||||
@@ -152,8 +150,8 @@ TEST_F(UserModelTest, SameFrame) {
|
|||||||
EXPECT_EQ(model->geom_sameframe[4], mjSAMEFRAME_INERTIAROT);
|
EXPECT_EQ(model->geom_sameframe[4], mjSAMEFRAME_INERTIAROT);
|
||||||
|
|
||||||
// make data, get geom_xpos
|
// make data, get geom_xpos
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
auto geom_xpos = AsVector(data->geom_xpos, model->ngeom * 3);
|
auto geom_xpos = AsVector(data->geom_xpos, model->ngeom * 3);
|
||||||
auto geom_xmat = AsVector(data->geom_xmat, model->ngeom * 9);
|
auto geom_xmat = AsVector(data->geom_xmat, model->ngeom * 9);
|
||||||
|
|
||||||
@@ -161,8 +159,8 @@ TEST_F(UserModelTest, SameFrame) {
|
|||||||
for (int i = 0; i < model->ngeom; i++) {
|
for (int i = 0; i < model->ngeom; i++) {
|
||||||
model->geom_sameframe[i] = mjSAMEFRAME_NONE;
|
model->geom_sameframe[i] = mjSAMEFRAME_NONE;
|
||||||
}
|
}
|
||||||
mj_resetData(model, data);
|
mj_resetData(model.get(), data.get());
|
||||||
mj_kinematics(model, data);
|
mj_kinematics(model.get(), data.get());
|
||||||
auto geom_xpos2 = AsVector(data->geom_xpos, model->ngeom * 3);
|
auto geom_xpos2 = AsVector(data->geom_xpos, model->ngeom * 3);
|
||||||
auto geom_xmat2 = AsVector(data->geom_xmat, model->ngeom * 9);
|
auto geom_xmat2 = AsVector(data->geom_xmat, model->ngeom * 9);
|
||||||
|
|
||||||
@@ -170,9 +168,6 @@ TEST_F(UserModelTest, SameFrame) {
|
|||||||
constexpr double eps = 1e-6;
|
constexpr double eps = 1e-6;
|
||||||
EXPECT_THAT(geom_xpos, Pointwise(MjNear(eps, eps), geom_xpos2));
|
EXPECT_THAT(geom_xpos, Pointwise(MjNear(eps, eps), geom_xpos2));
|
||||||
EXPECT_THAT(geom_xmat, Pointwise(MjNear(eps, eps), geom_xmat2));
|
EXPECT_THAT(geom_xmat, Pointwise(MjNear(eps, eps), geom_xmat2));
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserModelTest, ActuatorSparsity) {
|
TEST_F(UserModelTest, ActuatorSparsity) {
|
||||||
@@ -194,9 +189,8 @@ TEST_F(UserModelTest, ActuatorSparsity) {
|
|||||||
</actuator>
|
</actuator>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_EQ(m->nJmom, 2);
|
ASSERT_EQ(m->nJmom, 2);
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserModelTest, FixedTendonSparsity) {
|
TEST_F(UserModelTest, FixedTendonSparsity) {
|
||||||
@@ -226,8 +220,8 @@ TEST_F(UserModelTest, FixedTendonSparsity) {
|
|||||||
</tendon>
|
</tendon>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_THAT(m, NotNull());
|
ASSERT_THAT(m.get(), NotNull());
|
||||||
|
|
||||||
EXPECT_EQ(m->nJten, 3);
|
EXPECT_EQ(m->nJten, 3);
|
||||||
EXPECT_EQ(m->ten_J_rownnz[0], 3);
|
EXPECT_EQ(m->ten_J_rownnz[0], 3);
|
||||||
@@ -237,8 +231,6 @@ TEST_F(UserModelTest, FixedTendonSparsity) {
|
|||||||
int rowadr = m->ten_J_rowadr[0];
|
int rowadr = m->ten_J_rowadr[0];
|
||||||
int* colind = m->ten_J_colind + rowadr;
|
int* colind = m->ten_J_colind + rowadr;
|
||||||
EXPECT_THAT(std::vector<int>(colind, colind + 3), ElementsAre(0, 1, 2));
|
EXPECT_THAT(std::vector<int>(colind, colind + 3), ElementsAre(0, 1, 2));
|
||||||
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserModelTest, NestedZeroMassBodiesOK) {
|
TEST_F(UserModelTest, NestedZeroMassBodiesOK) {
|
||||||
@@ -259,9 +251,8 @@ TEST_F(UserModelTest, NestedZeroMassBodiesOK) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserModelTest, NestedZeroMassBodiesWithJointOK) {
|
TEST_F(UserModelTest, NestedZeroMassBodiesWithJointOK) {
|
||||||
@@ -286,9 +277,8 @@ TEST_F(UserModelTest, NestedZeroMassBodiesWithJointOK) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, NotNull()) << error;
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserModelTest, NestedZeroMassBodiesFail) {
|
TEST_F(UserModelTest, NestedZeroMassBodiesFail) {
|
||||||
@@ -309,14 +299,13 @@ TEST_F(UserModelTest, NestedZeroMassBodiesFail) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
char error[1024];
|
char error[1024];
|
||||||
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||||
ASSERT_THAT(model, IsNull());
|
ASSERT_THAT(model.get(), IsNull());
|
||||||
EXPECT_THAT(
|
EXPECT_THAT(
|
||||||
error,
|
error,
|
||||||
HasSubstr(
|
HasSubstr(
|
||||||
"mass and inertia of moving bodies must be larger than mjMINVAL"));
|
"mass and inertia of moving bodies must be larger than mjMINVAL"));
|
||||||
EXPECT_THAT(error, HasSubstr("Element name 'bad'"));
|
EXPECT_THAT(error, HasSubstr("Element name 'bad'"));
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserModelTest, ConvexHullForCollisionMeshes) {
|
TEST_F(UserModelTest, ConvexHullForCollisionMeshes) {
|
||||||
@@ -338,14 +327,14 @@ TEST_F(UserModelTest, ConvexHullForCollisionMeshes) {
|
|||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << error.data();
|
ASSERT_THAT(model.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
int no_hull_id = mj_name2id(model, mjOBJ_MESH, "mesh_no_hull");
|
int no_hull_id = mj_name2id(model.get(), mjOBJ_MESH, "mesh_no_hull");
|
||||||
int with_hull_contype_id =
|
int with_hull_contype_id =
|
||||||
mj_name2id(model, mjOBJ_MESH, "mesh_with_hull_contype");
|
mj_name2id(model.get(), mjOBJ_MESH, "mesh_with_hull_contype");
|
||||||
int with_hull_conaffinity_id =
|
int with_hull_conaffinity_id =
|
||||||
mj_name2id(model, mjOBJ_MESH, "mesh_with_hull_conaffinity");
|
mj_name2id(model.get(), mjOBJ_MESH, "mesh_with_hull_conaffinity");
|
||||||
|
|
||||||
EXPECT_NE(no_hull_id, -1);
|
EXPECT_NE(no_hull_id, -1);
|
||||||
EXPECT_NE(with_hull_contype_id, -1);
|
EXPECT_NE(with_hull_contype_id, -1);
|
||||||
@@ -359,8 +348,6 @@ TEST_F(UserModelTest, ConvexHullForCollisionMeshes) {
|
|||||||
|
|
||||||
// mesh_with_hull_conaffinity should have a convex hull.
|
// mesh_with_hull_conaffinity should have a convex hull.
|
||||||
EXPECT_NE(model->mesh_graphadr[with_hull_conaffinity_id], -1);
|
EXPECT_NE(model->mesh_graphadr[with_hull_conaffinity_id], -1);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserModelTest, ConvexHullForPairCollisionMeshes) {
|
TEST_F(UserModelTest, ConvexHullForPairCollisionMeshes) {
|
||||||
@@ -380,17 +367,15 @@ TEST_F(UserModelTest, ConvexHullForPairCollisionMeshes) {
|
|||||||
</mujoco>)";
|
</mujoco>)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << error.data();
|
ASSERT_THAT(model.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
int mesh_in_pair_id = mj_name2id(model, mjOBJ_MESH, "mesh");
|
int mesh_in_pair_id = mj_name2id(model.get(), mjOBJ_MESH, "mesh");
|
||||||
|
|
||||||
EXPECT_NE(mesh_in_pair_id, -1);
|
EXPECT_NE(mesh_in_pair_id, -1);
|
||||||
|
|
||||||
// mesh_in_pair should have a convex hull because it is in a collision pair.
|
// mesh_in_pair should have a convex hull because it is in a collision pair.
|
||||||
EXPECT_NE(model->mesh_graphadr[mesh_in_pair_id], -1);
|
EXPECT_NE(model->mesh_graphadr[mesh_in_pair_id], -1);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ------------- test automatic inference of nuser_xxx -------------------------
|
// ------------- test automatic inference of nuser_xxx -------------------------
|
||||||
@@ -406,11 +391,10 @@ TEST_F(UserDataTest, AutoNUserBody) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_EQ(m->nuser_body, 3);
|
ASSERT_EQ(m->nuser_body, 3);
|
||||||
EXPECT_THAT(GetRow(m->body_user, m->nuser_body, 1), ElementsAre(1, 2, 3));
|
EXPECT_THAT(GetRow(m->body_user, m->nuser_body, 1), ElementsAre(1, 2, 3));
|
||||||
EXPECT_THAT(GetRow(m->body_user, m->nuser_body, 2), ElementsAre(2, 3, 0));
|
EXPECT_THAT(GetRow(m->body_user, m->nuser_body, 2), ElementsAre(2, 3, 0));
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserDataTest, AutoNUserJoint) {
|
TEST_F(UserDataTest, AutoNUserJoint) {
|
||||||
@@ -425,11 +409,10 @@ TEST_F(UserDataTest, AutoNUserJoint) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_EQ(m->nuser_jnt, 3);
|
ASSERT_EQ(m->nuser_jnt, 3);
|
||||||
EXPECT_THAT(GetRow(m->jnt_user, m->nuser_jnt, 0), ElementsAre(1, 2, 3));
|
EXPECT_THAT(GetRow(m->jnt_user, m->nuser_jnt, 0), ElementsAre(1, 2, 3));
|
||||||
EXPECT_THAT(GetRow(m->jnt_user, m->nuser_jnt, 1), ElementsAre(2, 3, 0));
|
EXPECT_THAT(GetRow(m->jnt_user, m->nuser_jnt, 1), ElementsAre(2, 3, 0));
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserDataTest, AutoNUserGeom) {
|
TEST_F(UserDataTest, AutoNUserGeom) {
|
||||||
@@ -441,11 +424,10 @@ TEST_F(UserDataTest, AutoNUserGeom) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_EQ(m->nuser_geom, 3);
|
ASSERT_EQ(m->nuser_geom, 3);
|
||||||
EXPECT_THAT(GetRow(m->geom_user, m->nuser_geom, 0), ElementsAre(1, 2, 3));
|
EXPECT_THAT(GetRow(m->geom_user, m->nuser_geom, 0), ElementsAre(1, 2, 3));
|
||||||
EXPECT_THAT(GetRow(m->geom_user, m->nuser_geom, 1), ElementsAre(2, 3, 0));
|
EXPECT_THAT(GetRow(m->geom_user, m->nuser_geom, 1), ElementsAre(2, 3, 0));
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserDataTest, AutoNUserSite) {
|
TEST_F(UserDataTest, AutoNUserSite) {
|
||||||
@@ -457,11 +439,10 @@ TEST_F(UserDataTest, AutoNUserSite) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_EQ(m->nuser_site, 3);
|
ASSERT_EQ(m->nuser_site, 3);
|
||||||
EXPECT_THAT(GetRow(m->site_user, m->nuser_site, 0), ElementsAre(1, 2, 3));
|
EXPECT_THAT(GetRow(m->site_user, m->nuser_site, 0), ElementsAre(1, 2, 3));
|
||||||
EXPECT_THAT(GetRow(m->site_user, m->nuser_site, 1), ElementsAre(2, 3, 0));
|
EXPECT_THAT(GetRow(m->site_user, m->nuser_site, 1), ElementsAre(2, 3, 0));
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserDataTest, AutoNUserCamera) {
|
TEST_F(UserDataTest, AutoNUserCamera) {
|
||||||
@@ -473,11 +454,10 @@ TEST_F(UserDataTest, AutoNUserCamera) {
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_EQ(m->nuser_cam, 3);
|
ASSERT_EQ(m->nuser_cam, 3);
|
||||||
EXPECT_THAT(GetRow(m->cam_user, m->nuser_cam, 0), ElementsAre(1, 2, 3));
|
EXPECT_THAT(GetRow(m->cam_user, m->nuser_cam, 0), ElementsAre(1, 2, 3));
|
||||||
EXPECT_THAT(GetRow(m->cam_user, m->nuser_cam, 1), ElementsAre(2, 3, 0));
|
EXPECT_THAT(GetRow(m->cam_user, m->nuser_cam, 1), ElementsAre(2, 3, 0));
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserDataTest, AutoNUserTendon) {
|
TEST_F(UserDataTest, AutoNUserTendon) {
|
||||||
@@ -499,11 +479,10 @@ TEST_F(UserDataTest, AutoNUserTendon) {
|
|||||||
</tendon>
|
</tendon>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_EQ(m->nuser_tendon, 3);
|
ASSERT_EQ(m->nuser_tendon, 3);
|
||||||
EXPECT_THAT(GetRow(m->tendon_user, m->nuser_tendon, 0), ElementsAre(1, 2, 3));
|
EXPECT_THAT(GetRow(m->tendon_user, m->nuser_tendon, 0), ElementsAre(1, 2, 3));
|
||||||
EXPECT_THAT(GetRow(m->tendon_user, m->nuser_tendon, 1), ElementsAre(2, 3, 0));
|
EXPECT_THAT(GetRow(m->tendon_user, m->nuser_tendon, 1), ElementsAre(2, 3, 0));
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserDataTest, AutoNUserActuator) {
|
TEST_F(UserDataTest, AutoNUserActuator) {
|
||||||
@@ -521,13 +500,12 @@ TEST_F(UserDataTest, AutoNUserActuator) {
|
|||||||
</actuator>
|
</actuator>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_EQ(m->nuser_actuator, 3);
|
ASSERT_EQ(m->nuser_actuator, 3);
|
||||||
EXPECT_THAT(GetRow(m->actuator_user, m->nuser_actuator, 0),
|
EXPECT_THAT(GetRow(m->actuator_user, m->nuser_actuator, 0),
|
||||||
ElementsAre(1, 2, 3));
|
ElementsAre(1, 2, 3));
|
||||||
EXPECT_THAT(GetRow(m->actuator_user, m->nuser_actuator, 1),
|
EXPECT_THAT(GetRow(m->actuator_user, m->nuser_actuator, 1),
|
||||||
ElementsAre(2, 3, 0));
|
ElementsAre(2, 3, 0));
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(UserDataTest, AutoNUserSensor) {
|
TEST_F(UserDataTest, AutoNUserSensor) {
|
||||||
@@ -542,11 +520,10 @@ TEST_F(UserDataTest, AutoNUserSensor) {
|
|||||||
</sensor>
|
</sensor>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_EQ(m->nuser_sensor, 3);
|
ASSERT_EQ(m->nuser_sensor, 3);
|
||||||
EXPECT_THAT(GetRow(m->sensor_user, m->nuser_sensor, 0), ElementsAre(1, 2, 3));
|
EXPECT_THAT(GetRow(m->sensor_user, m->nuser_sensor, 0), ElementsAre(1, 2, 3));
|
||||||
EXPECT_THAT(GetRow(m->sensor_user, m->nuser_sensor, 1), ElementsAre(2, 3, 0));
|
EXPECT_THAT(GetRow(m->sensor_user, m->nuser_sensor, 1), ElementsAre(2, 3, 0));
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ------------- test duplicate names ------------------------------------------
|
// ------------- test duplicate names ------------------------------------------
|
||||||
@@ -584,10 +561,10 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) {
|
|||||||
std::string fuse = absl::StrFormat(xml_template, "true");
|
std::string fuse = absl::StrFormat(xml_template, "true");
|
||||||
std::string no_fuse = absl::StrFormat(xml_template, "false");
|
std::string no_fuse = absl::StrFormat(xml_template, "false");
|
||||||
|
|
||||||
mjModel* m_fuse = LoadModelFromString(fuse.c_str());
|
MjModelPtr m_fuse = LoadModelFromString(fuse.c_str());
|
||||||
mjModel* m_no_fuse = LoadModelFromString(no_fuse.c_str());
|
MjModelPtr m_no_fuse = LoadModelFromString(no_fuse.c_str());
|
||||||
ASSERT_THAT(m_fuse, NotNull());
|
ASSERT_THAT(m_fuse.get(), NotNull());
|
||||||
ASSERT_THAT(m_no_fuse, NotNull());
|
ASSERT_THAT(m_no_fuse.get(), NotNull());
|
||||||
|
|
||||||
EXPECT_EQ(m_fuse->nbody, 2) << "Expecting a world body and one other body";
|
EXPECT_EQ(m_fuse->nbody, 2) << "Expecting a world body and one other body";
|
||||||
EXPECT_EQ(m_no_fuse->nbody, 3) << "Expecting a world body and two others";
|
EXPECT_EQ(m_no_fuse->nbody, 3) << "Expecting a world body and two others";
|
||||||
@@ -600,20 +577,16 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) {
|
|||||||
EXPECT_EQ(m_no_fuse->body_bvhnum[2], 3);
|
EXPECT_EQ(m_no_fuse->body_bvhnum[2], 3);
|
||||||
EXPECT_EQ(m_fuse->body_bvhnum[1], 3);
|
EXPECT_EQ(m_fuse->body_bvhnum[1], 3);
|
||||||
|
|
||||||
mjData* d_fuse = mj_makeData(m_fuse);
|
MjDataPtr d_fuse = MakeData(m_fuse);
|
||||||
mjData* d_no_fuse = mj_makeData(m_no_fuse);
|
MjDataPtr d_no_fuse = MakeData(m_no_fuse);
|
||||||
|
|
||||||
mj_step(m_fuse, d_fuse);
|
mj_step(m_fuse.get(), d_fuse.get());
|
||||||
mj_step(m_no_fuse, d_no_fuse);
|
mj_step(m_no_fuse.get(), d_no_fuse.get());
|
||||||
|
|
||||||
EXPECT_NEAR(d_fuse->qvel[0], d_no_fuse->qvel[0], MjTol(2e-17, 1e-8))
|
EXPECT_NEAR(d_fuse.get()->qvel[0], d_no_fuse.get()->qvel[0],
|
||||||
|
MjTol(2e-17, 1e-8))
|
||||||
<< "Velocity should be the same after 1 step";
|
<< "Velocity should be the same after 1 step";
|
||||||
EXPECT_NE(d_fuse->qvel[0], 0);
|
EXPECT_NE(d_fuse.get()->qvel[0], 0);
|
||||||
|
|
||||||
mj_deleteData(d_fuse);
|
|
||||||
mj_deleteData(d_no_fuse);
|
|
||||||
mj_deleteModel(m_fuse);
|
|
||||||
mj_deleteModel(m_no_fuse);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(FuseStaticTest, FuseStaticActuatorReferencedBody) {
|
TEST_F(FuseStaticTest, FuseStaticActuatorReferencedBody) {
|
||||||
@@ -642,10 +615,9 @@ TEST_F(FuseStaticTest, FuseStaticActuatorReferencedBody) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml_template, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml_template, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others";
|
EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others";
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(FuseStaticTest, FuseStaticLightReferencedBody) {
|
TEST_F(FuseStaticTest, FuseStaticLightReferencedBody) {
|
||||||
@@ -671,10 +643,9 @@ TEST_F(FuseStaticTest, FuseStaticLightReferencedBody) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml_template, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml_template, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others";
|
EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others";
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(FuseStaticTest, FuseStaticForceSensorReferencedBody) {
|
TEST_F(FuseStaticTest, FuseStaticForceSensorReferencedBody) {
|
||||||
@@ -704,10 +675,9 @@ TEST_F(FuseStaticTest, FuseStaticForceSensorReferencedBody) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml_template, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml_template, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others";
|
EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others";
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(FuseStaticTest, FuseStaticCameraInBody) {
|
TEST_F(FuseStaticTest, FuseStaticCameraInBody) {
|
||||||
@@ -727,11 +697,10 @@ TEST_F(FuseStaticTest, FuseStaticCameraInBody) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
EXPECT_EQ(m->nbody, 2) << "Static body should be fused";
|
EXPECT_EQ(m->nbody, 2) << "Static body should be fused";
|
||||||
EXPECT_EQ(m->ncam, 1);
|
EXPECT_EQ(m->ncam, 1);
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(FuseStaticTest, FuseStaticLightInBody) {
|
TEST_F(FuseStaticTest, FuseStaticLightInBody) {
|
||||||
@@ -750,11 +719,10 @@ TEST_F(FuseStaticTest, FuseStaticLightInBody) {
|
|||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(m, NotNull()) << error.data();
|
ASSERT_THAT(m.get(), NotNull()) << error.data();
|
||||||
EXPECT_EQ(m->nbody, 2) << "Static body should be fused";
|
EXPECT_EQ(m->nbody, 2) << "Static body should be fused";
|
||||||
EXPECT_EQ(m->nlight, 1);
|
EXPECT_EQ(m->nlight, 1);
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// ------------- test discardvisual --------------------------------------------
|
// ------------- test discardvisual --------------------------------------------
|
||||||
@@ -789,13 +757,12 @@ TEST_F(DiscardVisualTest, DiscardVisualKeepsInertia) {
|
|||||||
)";
|
)";
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
EXPECT_THAT(model, NotNull()) << error.data();
|
EXPECT_THAT(model.get(), NotNull()) << error.data();
|
||||||
EXPECT_THAT(model->nmesh, 1);
|
EXPECT_THAT(model->nmesh, 1);
|
||||||
EXPECT_THAT(model->body_inertia[3], model->body_inertia[6]);
|
EXPECT_THAT(model->body_inertia[3], model->body_inertia[6]);
|
||||||
EXPECT_THAT(model->body_inertia[4], model->body_inertia[7]);
|
EXPECT_THAT(model->body_inertia[4], model->body_inertia[7]);
|
||||||
EXPECT_THAT(model->body_inertia[5], model->body_inertia[8]);
|
EXPECT_THAT(model->body_inertia[5], model->body_inertia[8]);
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(DiscardVisualTest, DiscardVisualEquivalent) {
|
TEST_F(DiscardVisualTest, DiscardVisualEquivalent) {
|
||||||
@@ -877,8 +844,7 @@ TEST_F(LengthRangeTest, LengthRangeThreading) {
|
|||||||
size_t error_sz = 1024;
|
size_t error_sz = 1024;
|
||||||
std::string field = "";
|
std::string field = "";
|
||||||
|
|
||||||
static const char* const kLengthrangePath =
|
static const char* const kLengthrangePath = "user/testdata/lengthrange.xml";
|
||||||
"user/testdata/lengthrange.xml";
|
|
||||||
|
|
||||||
const std::string xml_path1 = GetTestDataFilePath(kLengthrangePath);
|
const std::string xml_path1 = GetTestDataFilePath(kLengthrangePath);
|
||||||
mjSpec* spec = mj_parseXML(xml_path1.c_str(), 0, error, error_sz);
|
mjSpec* spec = mj_parseXML(xml_path1.c_str(), 0, error, error_sz);
|
||||||
@@ -1022,20 +988,17 @@ TEST_F(MujocoTest, NestedMeshDir) {
|
|||||||
sizeof(cube));
|
sizeof(cube));
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* child_model = LoadModelFromString(child_xml, error.data(),
|
MjModelPtr child_model =
|
||||||
error.size(), vfs.get());
|
LoadModelFromString(child_xml, error.data(), error.size(), vfs.get());
|
||||||
EXPECT_THAT(child_model, NotNull()) << error.data();
|
EXPECT_THAT(child_model.get(), NotNull()) << error.data();
|
||||||
mj_deleteModel(child_model);
|
|
||||||
|
|
||||||
mjModel* parent_model = LoadModelFromString(parent_xml, error.data(),
|
MjModelPtr parent_model =
|
||||||
error.size(), vfs.get());
|
LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get());
|
||||||
EXPECT_THAT(parent_model, NotNull()) << error.data();
|
EXPECT_THAT(parent_model.get(), NotNull()) << error.data();
|
||||||
mj_deleteModel(parent_model);
|
|
||||||
|
|
||||||
mjModel* grandparent_model = LoadModelFromString(
|
MjModelPtr grandparent_model = LoadModelFromString(
|
||||||
grandparent_xml, error.data(), error.size(), vfs.get());
|
grandparent_xml, error.data(), error.size(), vfs.get());
|
||||||
EXPECT_THAT(grandparent_model, NotNull()) << error.data();
|
EXPECT_THAT(grandparent_model.get(), NotNull()) << error.data();
|
||||||
mj_deleteModel(grandparent_model);
|
|
||||||
|
|
||||||
mj_deleteVFS(vfs.get());
|
mj_deleteVFS(vfs.get());
|
||||||
}
|
}
|
||||||
@@ -1087,8 +1050,8 @@ TEST_F(DelayBufferTest, ActuatorDelayBufferSizes) {
|
|||||||
</actuator>
|
</actuator>
|
||||||
</mujoco>
|
</mujoco>
|
||||||
)";
|
)";
|
||||||
mjModel* m = LoadModelFromString(xml);
|
MjModelPtr m = LoadModelFromString(xml);
|
||||||
ASSERT_THAT(m, NotNull());
|
ASSERT_THAT(m.get(), NotNull());
|
||||||
ASSERT_EQ(m->nu, 3);
|
ASSERT_EQ(m->nu, 3);
|
||||||
|
|
||||||
// nhistory = (2+2*3) + (2+2*10) = 8 + 22 = 30
|
// nhistory = (2+2*3) + (2+2*10) = 8 + 22 = 30
|
||||||
@@ -1101,8 +1064,6 @@ TEST_F(DelayBufferTest, ActuatorDelayBufferSizes) {
|
|||||||
EXPECT_EQ(m->actuator_historyadr[0], -1);
|
EXPECT_EQ(m->actuator_historyadr[0], -1);
|
||||||
EXPECT_EQ(m->actuator_historyadr[1], 0);
|
EXPECT_EQ(m->actuator_historyadr[1], 0);
|
||||||
EXPECT_EQ(m->actuator_historyadr[2], 8);
|
EXPECT_EQ(m->actuator_historyadr[2], 8);
|
||||||
|
|
||||||
mj_deleteModel(m);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|||||||
+229
-296
File diff suppressed because it is too large
Load Diff
+33
-33
@@ -14,6 +14,8 @@
|
|||||||
|
|
||||||
// Tests for xml/xml_api.cc.
|
// Tests for xml/xml_api.cc.
|
||||||
|
|
||||||
|
#include "src/xml/xml_api.h"
|
||||||
|
|
||||||
#include <array>
|
#include <array>
|
||||||
#include <cstddef>
|
#include <cstddef>
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
@@ -23,18 +25,17 @@
|
|||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <mujoco/mjdata.h>
|
#include <mujoco/mjdata.h>
|
||||||
#include <mujoco/mjmodel.h>
|
#include <mujoco/mjmodel.h>
|
||||||
#include <mujoco/mujoco.h>
|
|
||||||
#include <mujoco/mjspec.h>
|
#include <mujoco/mjspec.h>
|
||||||
#include "src/xml/xml_api.h"
|
#include <mujoco/mujoco.h>
|
||||||
#include "test/fixture.h"
|
#include "test/fixture.h"
|
||||||
|
|
||||||
namespace mujoco {
|
namespace mujoco {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
|
using ::testing::HasSubstr;
|
||||||
using ::testing::IsNull;
|
using ::testing::IsNull;
|
||||||
using ::testing::NotNull;
|
using ::testing::NotNull;
|
||||||
using ::testing::StartsWith;
|
using ::testing::StartsWith;
|
||||||
using ::testing::HasSubstr;
|
|
||||||
|
|
||||||
static constexpr char xml[] = R"(
|
static constexpr char xml[] = R"(
|
||||||
<mujoco>
|
<mujoco>
|
||||||
@@ -57,62 +58,56 @@ using LoadXmlTest = MujocoTest;
|
|||||||
|
|
||||||
TEST_F(LoadXmlTest, EmptyModel) {
|
TEST_F(LoadXmlTest, EmptyModel) {
|
||||||
static constexpr char xml[] = "<mujoco/>";
|
static constexpr char xml[] = "<mujoco/>";
|
||||||
mjModel* model = LoadModelFromString(xml, 0, 0);
|
MjModelPtr model = LoadModelFromString(xml, 0, 0);
|
||||||
ASSERT_THAT(model, NotNull());
|
ASSERT_THAT(model.get(), NotNull());
|
||||||
EXPECT_EQ(model->nq, 0);
|
EXPECT_EQ(model->nq, 0);
|
||||||
EXPECT_EQ(model->nv, 0);
|
EXPECT_EQ(model->nv, 0);
|
||||||
EXPECT_EQ(model->nu, 0);
|
EXPECT_EQ(model->nu, 0);
|
||||||
EXPECT_EQ(model->na, 0);
|
EXPECT_EQ(model->na, 0);
|
||||||
EXPECT_EQ(model->nbody, 1); // worldbody exists even in empty model
|
EXPECT_EQ(model->nbody, 1); // worldbody exists even in empty model
|
||||||
|
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
EXPECT_THAT(data, NotNull());
|
EXPECT_THAT(data, NotNull());
|
||||||
mj_step(model, data);
|
mj_step(model.get(), data.get());
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(LoadXmlTest, InvalidXmlFailsToLoad) {
|
TEST_F(LoadXmlTest, InvalidXmlFailsToLoad) {
|
||||||
static constexpr char invalid_xml[] = "<mujoc";
|
static constexpr char invalid_xml[] = "<mujoc";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
mjModel* model = LoadModelFromString(invalid_xml, error.data(), error.size());
|
MjModelPtr model =
|
||||||
EXPECT_THAT(model, IsNull()) << "Expected model loading to fail.";
|
LoadModelFromString(invalid_xml, error.data(), error.size());
|
||||||
|
EXPECT_THAT(model.get(), IsNull()) << "Expected model loading to fail.";
|
||||||
EXPECT_GT(std::strlen(error.data()), 0);
|
EXPECT_GT(std::strlen(error.data()), 0);
|
||||||
if (model) {
|
if (model.get()) {
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(LoadXmlTest, MultipleBodies) {
|
TEST_F(LoadXmlTest, MultipleBodies) {
|
||||||
std::array<char, 1000> error;
|
std::array<char, 1000> error;
|
||||||
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
|
|
||||||
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
|
ASSERT_THAT(model.get(), NotNull())
|
||||||
|
<< "Failed to load model: " << error.data();
|
||||||
EXPECT_EQ(model->nbody, 3);
|
EXPECT_EQ(model->nbody, 3);
|
||||||
|
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
EXPECT_THAT(data, NotNull());
|
EXPECT_THAT(data, NotNull());
|
||||||
mj_step(model, data);
|
mj_step(model.get(), data.get());
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
using SaveLastXmlTest = MujocoTest;
|
using SaveLastXmlTest = MujocoTest;
|
||||||
|
|
||||||
TEST_F(SaveLastXmlTest, EmptyModel) {
|
TEST_F(SaveLastXmlTest, EmptyModel) {
|
||||||
static constexpr char xml[] = "<mujoco/>";
|
static constexpr char xml[] = "<mujoco/>";
|
||||||
mjModel* model = LoadModelFromString(xml, 0, 0);
|
MjModelPtr model = LoadModelFromString(xml, 0, 0);
|
||||||
mjData* data = mj_makeData(model);
|
MjDataPtr data = MakeData(model);
|
||||||
|
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
error.data()[0] = '\0';
|
error.data()[0] = '\0';
|
||||||
|
|
||||||
testing::internal::CaptureStdout();
|
testing::internal::CaptureStdout();
|
||||||
mj_saveLastXML(nullptr, model, error.data(), error.size());
|
mj_saveLastXML(nullptr, model.get(), error.data(), error.size());
|
||||||
|
|
||||||
EXPECT_THAT(testing::internal::GetCapturedStdout(), StartsWith("<mujoco"));
|
EXPECT_THAT(testing::internal::GetCapturedStdout(), StartsWith("<mujoco"));
|
||||||
|
|
||||||
mj_deleteData(data);
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(LoadXmlTest, NullFileFails) {
|
TEST_F(LoadXmlTest, NullFileFails) {
|
||||||
@@ -138,8 +133,9 @@ TEST_F(MujocoTest, SaveXmlShortString) {
|
|||||||
EXPECT_THAT(model, NotNull()) << "Failed to compile model: " << error.data();
|
EXPECT_THAT(model, NotNull()) << "Failed to compile model: " << error.data();
|
||||||
|
|
||||||
std::array<char, 10> out;
|
std::array<char, 10> out;
|
||||||
EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(),
|
EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), error.data(),
|
||||||
error.data(), error.size()), 273);
|
error.size()),
|
||||||
|
273);
|
||||||
EXPECT_STREQ(error.data(), "Output string too short, should be at least 274");
|
EXPECT_STREQ(error.data(), "Output string too short, should be at least 274");
|
||||||
|
|
||||||
mj_deleteSpec(spec);
|
mj_deleteSpec(spec);
|
||||||
@@ -156,10 +152,13 @@ TEST_F(MujocoTest, SaveXml) {
|
|||||||
|
|
||||||
std::array<char, 274> out;
|
std::array<char, 274> out;
|
||||||
EXPECT_THAT(mj_saveXMLString(NULL, out.data(), out.size(), error.data(),
|
EXPECT_THAT(mj_saveXMLString(NULL, out.data(), out.size(), error.data(),
|
||||||
error.size()), -1);
|
error.size()),
|
||||||
|
-1);
|
||||||
EXPECT_STREQ(error.data(), "Cannot write empty model");
|
EXPECT_STREQ(error.data(), "Cannot write empty model");
|
||||||
EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), error.data(),
|
EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), error.data(),
|
||||||
error.size()), 0) << error.data();
|
error.size()),
|
||||||
|
0)
|
||||||
|
<< error.data();
|
||||||
|
|
||||||
mjSpec* saved_spec = mj_parseXMLString(xml, 0, error.data(), error.size());
|
mjSpec* saved_spec = mj_parseXMLString(xml, 0, error.data(), error.size());
|
||||||
EXPECT_THAT(saved_spec, NotNull()) << "Invalid saved spec: " << error.data();
|
EXPECT_THAT(saved_spec, NotNull()) << "Invalid saved spec: " << error.data();
|
||||||
@@ -197,7 +196,9 @@ TEST_F(MujocoTest, SaveXmlWithDefaultMesh) {
|
|||||||
|
|
||||||
std::array<char, 1024> out;
|
std::array<char, 1024> out;
|
||||||
EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), error.data(),
|
EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), error.data(),
|
||||||
error.size()), 0) << error.data();
|
error.size()),
|
||||||
|
0)
|
||||||
|
<< error.data();
|
||||||
|
|
||||||
mjSpec* saved_spec = mj_parseXMLString(xml, 0, error.data(), error.size());
|
mjSpec* saved_spec = mj_parseXMLString(xml, 0, error.data(), error.size());
|
||||||
EXPECT_THAT(saved_spec, NotNull()) << "Invalid saved spec: " << error.data();
|
EXPECT_THAT(saved_spec, NotNull()) << "Invalid saved spec: " << error.data();
|
||||||
@@ -215,9 +216,8 @@ TEST_F(MujocoTest, SaveXmlWithDefaultMesh) {
|
|||||||
|
|
||||||
TEST_F(MujocoTest, FreeLastXml) {
|
TEST_F(MujocoTest, FreeLastXml) {
|
||||||
static constexpr char xml[] = "<mujoco/>";
|
static constexpr char xml[] = "<mujoco/>";
|
||||||
mjModel* model = LoadModelFromString(xml, 0, 0);
|
MjModelPtr model = LoadModelFromString(xml, 0, 0);
|
||||||
ASSERT_THAT(model, NotNull());
|
ASSERT_THAT(model.get(), NotNull());
|
||||||
mj_deleteModel(model);
|
|
||||||
ASSERT_NE(mj_saveLastXML(nullptr, nullptr, nullptr, 0), 0);
|
ASSERT_NE(mj_saveLastXML(nullptr, nullptr, nullptr, 0), 0);
|
||||||
mj_freeLastXML();
|
mj_freeLastXML();
|
||||||
ASSERT_EQ(mj_saveLastXML(nullptr, nullptr, nullptr, 0), 0);
|
ASSERT_EQ(mj_saveLastXML(nullptr, nullptr, nullptr, 0), 0);
|
||||||
|
|||||||
+255
-338
File diff suppressed because it is too large
Load Diff
+205
-284
File diff suppressed because it is too large
Load Diff
+15
-24
@@ -45,9 +45,8 @@ TEST_F(MujocoTest, ReadsCapsule) {
|
|||||||
</robot>
|
</robot>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1000> error;
|
std::array<char, 1000> error;
|
||||||
mjModel* model = LoadModelFromString(urdf, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << error.data();
|
ASSERT_THAT(model.get(), NotNull()) << error.data();
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, ReadsGeomNames) {
|
TEST_F(MujocoTest, ReadsGeomNames) {
|
||||||
@@ -74,19 +73,17 @@ TEST_F(MujocoTest, ReadsGeomNames) {
|
|||||||
</robot>
|
</robot>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1000> error;
|
std::array<char, 1000> error;
|
||||||
mjModel* model = LoadModelFromString(urdf, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << error.data();
|
ASSERT_THAT(model.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
// Check the geoms have been loaded with the right names
|
// Check the geoms have been loaded with the right names
|
||||||
int collision_box_id = mj_name2id(model, mjtObj::mjOBJ_GEOM, "collision_box");
|
int collision_box_id = mj_name2id(model.get(), mjtObj::mjOBJ_GEOM, "collision_box");
|
||||||
ASSERT_GE(collision_box_id, 0);
|
ASSERT_GE(collision_box_id, 0);
|
||||||
EXPECT_EQ(model->geom_type[collision_box_id], mjtGeom::mjGEOM_BOX);
|
EXPECT_EQ(model->geom_type[collision_box_id], mjtGeom::mjGEOM_BOX);
|
||||||
|
|
||||||
int visual_sphere_id = mj_name2id(model, mjtObj::mjOBJ_GEOM, "visual_sphere");
|
int visual_sphere_id = mj_name2id(model.get(), mjtObj::mjOBJ_GEOM, "visual_sphere");
|
||||||
ASSERT_GE(visual_sphere_id, 0);
|
ASSERT_GE(visual_sphere_id, 0);
|
||||||
EXPECT_EQ(model->geom_type[visual_sphere_id], mjtGeom::mjGEOM_SPHERE);
|
EXPECT_EQ(model->geom_type[visual_sphere_id], mjtGeom::mjGEOM_SPHERE);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesCollisionBeforeVisual) {
|
TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesCollisionBeforeVisual) {
|
||||||
@@ -113,16 +110,14 @@ TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesCollisionBeforeVisual) {
|
|||||||
</robot>
|
</robot>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1000> error;
|
std::array<char, 1000> error;
|
||||||
mjModel* model = LoadModelFromString(urdf, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << error.data();
|
ASSERT_THAT(model.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
// Check the collision geom gets its name from the URDF. The visual sphere
|
// Check the collision geom gets its name from the URDF. The visual sphere
|
||||||
// should not have a name to avoid duplicates.
|
// should not have a name to avoid duplicates.
|
||||||
int collision_box_id = mj_name2id(model, mjtObj::mjOBJ_GEOM, "shared_name");
|
int collision_box_id = mj_name2id(model.get(), mjtObj::mjOBJ_GEOM, "shared_name");
|
||||||
ASSERT_GE(collision_box_id, 0);
|
ASSERT_GE(collision_box_id, 0);
|
||||||
EXPECT_EQ(model->geom_type[collision_box_id], mjtGeom::mjGEOM_BOX);
|
EXPECT_EQ(model->geom_type[collision_box_id], mjtGeom::mjGEOM_BOX);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesVisualBeforeCollision) {
|
TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesVisualBeforeCollision) {
|
||||||
@@ -149,16 +144,14 @@ TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesVisualBeforeCollision) {
|
|||||||
</robot>
|
</robot>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1000> error;
|
std::array<char, 1000> error;
|
||||||
mjModel* model = LoadModelFromString(urdf, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << error.data();
|
ASSERT_THAT(model.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
// Check the visual geom gets its name from the URDF. The collision geom
|
// Check the visual geom gets its name from the URDF. The collision geom
|
||||||
// should not have a name to avoid duplicates.
|
// should not have a name to avoid duplicates.
|
||||||
int visual_sphere_id = mj_name2id(model, mjtObj::mjOBJ_GEOM, "shared_name");
|
int visual_sphere_id = mj_name2id(model.get(), mjtObj::mjOBJ_GEOM, "shared_name");
|
||||||
ASSERT_GE(visual_sphere_id, 0);
|
ASSERT_GE(visual_sphere_id, 0);
|
||||||
EXPECT_EQ(model->geom_type[visual_sphere_id], mjtGeom::mjGEOM_SPHERE);
|
EXPECT_EQ(model->geom_type[visual_sphere_id], mjtGeom::mjGEOM_SPHERE);
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, ReadsJointTypes) {
|
TEST_F(MujocoTest, ReadsJointTypes) {
|
||||||
@@ -226,8 +219,8 @@ TEST_F(MujocoTest, ReadsJointTypes) {
|
|||||||
</robot>
|
</robot>
|
||||||
)";
|
)";
|
||||||
std::array<char, 1000> error;
|
std::array<char, 1000> error;
|
||||||
mjModel* model = LoadModelFromString(urdf, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size());
|
||||||
ASSERT_THAT(model, NotNull()) << error.data();
|
ASSERT_THAT(model.get(), NotNull()) << error.data();
|
||||||
|
|
||||||
constexpr float eps = 1e-6;
|
constexpr float eps = 1e-6;
|
||||||
|
|
||||||
@@ -241,14 +234,12 @@ TEST_F(MujocoTest, ReadsJointTypes) {
|
|||||||
{0.0, 0.0, 1.0}
|
{0.0, 0.0, 1.0}
|
||||||
};
|
};
|
||||||
for (int i = 0; i < joint_names.size(); ++i) {
|
for (int i = 0; i < joint_names.size(); ++i) {
|
||||||
int id = mj_name2id(model, mjtObj::mjOBJ_JOINT, joint_names[i].c_str());
|
int id = mj_name2id(model.get(), mjtObj::mjOBJ_JOINT, joint_names[i].c_str());
|
||||||
EXPECT_EQ(model->jnt_type[id], expected_joint_types[i]);
|
EXPECT_EQ(model->jnt_type[id], expected_joint_types[i]);
|
||||||
EXPECT_NEAR(model->jnt_axis[3 * id], expected_axis[i][0], eps);
|
EXPECT_NEAR(model->jnt_axis[3 * id], expected_axis[i][0], eps);
|
||||||
EXPECT_NEAR(model->jnt_axis[3 * id + 1], expected_axis[i][1], eps);
|
EXPECT_NEAR(model->jnt_axis[3 * id + 1], expected_axis[i][1], eps);
|
||||||
EXPECT_NEAR(model->jnt_axis[3 * id + 2], expected_axis[i][2], eps);
|
EXPECT_NEAR(model->jnt_axis[3 * id + 2], expected_axis[i][2], eps);
|
||||||
}
|
}
|
||||||
|
|
||||||
mj_deleteModel(model);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(MujocoTest, RepeatedMeshName) {
|
TEST_F(MujocoTest, RepeatedMeshName) {
|
||||||
|
|||||||
Reference in New Issue
Block a user