diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index cef1233d..a42edd07 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -909,6 +909,7 @@ Euler integrator, semi-implicit in velocity. def test_mj_ray(self): # mj_ray has tricky argument types geomid = np.zeros(1, np.int32) + mujoco.mj_forward(self.model, self.data) mujoco.mj_ray(self.model, self.data, [0, 0, 0], [0, 0, 1], None, 0, 0, geomid) mujoco.mj_ray(self.model, self.data, [0, 0, 0], [0, 0, 1], diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index 3e215f18..481dd3b1 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -806,7 +806,13 @@ void MjDataWrapper::Serialize(std::ostream& output) const { #undef X // Write buffer contents - WriteBytes(output, ptr_->buffer, ptr_->nbuffer); + { + MJDATA_POINTERS_PREAMBLE((&this->metadata_)) +#define X(type, name, nr, nc) \ + WriteBytes(output, ptr_->name, sizeof(type)*(this->metadata_.nr)*(nc)); + MJDATA_POINTERS +#undef X + } } MjDataWrapper MjDataWrapper::Deserialize(std::istream& input) { @@ -857,7 +863,13 @@ MjDataWrapper MjDataWrapper::Deserialize(std::istream& input) { #undef X // Read buffer contents - ReadBytes(input, d->buffer, d->nbuffer); + { + MJDATA_POINTERS_PREAMBLE((&m)) +#define X(type, name, nr, nc) \ + ReadBytes(input, d->name, sizeof(type)*(m.nr)*(nc)); + MJDATA_POINTERS +#undef X + } CheckInput(input, "mjData"); // All bytes should have been used. diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 9a967f11..5a088f15 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -27,10 +27,16 @@ #include "engine/engine_util_errmem.h" #include "engine/engine_vfs.h" +#ifdef MEMORY_SANITIZER +#include +#endif + #ifdef _MSC_VER #pragma warning (disable: 4305) // tell MSVC to not complain that float x = 0.1 should be 0.1f #endif +#define PTRDIFF(x, y) ((void*)(x) - (void*)(y)) + //------------------------------ mjLROpt ----------------------------------------------------------- @@ -437,6 +443,10 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt, // clear, set pointers in buffer memset(m->buffer, 0, m->nbuffer); +#ifdef MEMORY_SANITIZER + // Tell msan to treat the entire buffer as uninitialized + __msan_allocated_memory(m->buffer, m->nbuffer); +#endif mj_setPtrModel(m); // set default options @@ -480,8 +490,14 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { dest->buffer = save_bufptr; mj_setPtrModel(dest); - // copy buffer, respect padding - memcpy((char*)dest->buffer, (char*)src->buffer, src->nbuffer); + // copy buffer + { + MJMODEL_POINTERS_PREAMBLE(src) + #define X(type, name, nr, nc) \ + memcpy((char*)dest->name, (const char*)src->name, sizeof(type)*(src->nr)*nc); + MJMODEL_POINTERS + #undef X + } return dest; } @@ -512,14 +528,26 @@ void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buff fwrite((void*)&m->opt, sizeof(mjOption), 1, fp); fwrite((void*)&m->vis, sizeof(mjVisual), 1, fp); fwrite((void*)&m->stat, sizeof(mjStatistic), 1, fp); - fwrite((void*)m->buffer, 1, m->nbuffer, fp); + { + MJMODEL_POINTERS_PREAMBLE(m) + #define X(type, name, nr, nc) \ + fwrite((void*)m->name, sizeof(type), (m->nr)*(nc), fp); + MJMODEL_POINTERS + #undef X + } } else { bufwrite(header, sizeof(int)*4, buffer_sz, buffer, &ptrbuf); bufwrite(m, sizeof(int)*getnint(), buffer_sz, buffer, &ptrbuf); bufwrite((void*)&m->opt, sizeof(mjOption), buffer_sz, buffer, &ptrbuf); bufwrite((void*)&m->vis, sizeof(mjVisual), buffer_sz, buffer, &ptrbuf); bufwrite((void*)&m->stat, sizeof(mjStatistic), buffer_sz, buffer, &ptrbuf); - bufwrite((void*)m->buffer, m->nbuffer, buffer_sz, buffer, &ptrbuf); + { + MJMODEL_POINTERS_PREAMBLE(m) + #define X(type, name, nr, nc) \ + bufwrite((void*)m->name, sizeof(type)*(m->nr)*(nc), buffer_sz, buffer, &ptrbuf); + MJMODEL_POINTERS + #undef X + } } if (fp) { @@ -640,15 +668,27 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) { mju_warning("Model file does not have a complete mjStatistic"); return 0; } - if (fread(m->buffer, 1, m->nbuffer, fp) != m->nbuffer) { - mju_warning("Model file does not contain a large enough buffer"); - return 0; + { + MJMODEL_POINTERS_PREAMBLE(m) + #define X(type, name, nr, nc) \ + if (fread(m->name, sizeof(type), (m->nr)*(nc), fp) != (m->nr)*(nc)) { \ + mju_warning("Model file does not contain a large enough buffer"); \ + return 0; \ + } + MJMODEL_POINTERS + #undef X } } else { bufread((void*)&m->opt, sizeof(mjOption), buffer_sz, buffer, &ptrbuf); bufread((void*)&m->vis, sizeof(mjVisual), buffer_sz, buffer, &ptrbuf); bufread((void*)&m->stat, sizeof(mjStatistic), buffer_sz, buffer, &ptrbuf); - bufread(m->buffer, m->nbuffer, buffer_sz, buffer, &ptrbuf); + { + MJMODEL_POINTERS_PREAMBLE(m) + #define X(type, name, nr, nc) \ + bufread(m->name, sizeof(type)*(m->nr)*(nc), buffer_sz, buffer, &ptrbuf); + MJMODEL_POINTERS + #undef X + } } // make sure file size is correct @@ -810,7 +850,13 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) { mj_setPtrData(m, dest); // copy buffer - memcpy((char*)dest->buffer, (char*)src->buffer, src->nbuffer); + { + MJDATA_POINTERS_PREAMBLE(m) + #define X(type, name, nr, nc) \ + memcpy((char*)dest->name, (const char*)src->name, sizeof(type)*(m->nr)*nc); + MJDATA_POINTERS + #undef X + } return dest; } @@ -875,27 +921,31 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { // fill buffer with debug_value (normally 0) memset(d->buffer, (int)debug_value, d->nbuffer); +#ifdef MEMORY_SANITIZER + // Tell msan to treat the entire buffer as uninitialized + __msan_allocated_memory(d->buffer, d->nbuffer); +#endif + + // zero out arrays that are not affected by mj_forward + mju_zero(d->qpos, m->nq); + mju_zero(d->qvel, m->nv); + mju_zero(d->act, m->na); + mju_zero(d->ctrl, m->nu); + mju_zero(d->qfrc_applied, m->nv); + mju_zero(d->xfrc_applied, 6*m->nbody); + mju_zero(d->qacc, m->nv); + mju_zero(d->qacc_warmstart, m->nv); + mju_zero(d->act_dot, m->na); + mju_zero(d->userdata, m->nuserdata); + mju_zero(d->sensordata, m->nsensordata); + mju_zero(d->mocap_pos, 3*m->nmocap); + mju_zero(d->mocap_quat, 4*m->nmocap); + // copy qpos0 from model if (m->qpos0) { memcpy(d->qpos, m->qpos0, m->nq*sizeof(mjtNum)); } - // debugging: zero the rest of the main input section - if (debug_value) { - mju_zero(d->qvel, m->nv); - mju_zero(d->act, m->na); - mju_zero(d->ctrl, m->nu); - mju_zero(d->qfrc_applied, m->nv); - mju_zero(d->xfrc_applied, 6*m->nbody); - mju_zero(d->qacc, m->nv); - mju_zero(d->qacc_warmstart, m->nv); - mju_zero(d->act_dot, m->na); - mju_zero(d->userdata, m->nuserdata); - mju_zero(d->sensordata, m->nsensordata); - mju_zero(d->mocap_pos, 3*m->nmocap); - mju_zero(d->mocap_quat, 4*m->nmocap); - } - // set mocap_pos/quat = body_pos/quat for mocap bodies if (m->body_mocapid) { for (int i=0; inbody; i++) { diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 6b28eb8b..02bb5bd3 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -307,9 +307,6 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) { const mjtNum tol = 1e-14; // tolerance for floating point numbers EXPECT_EQ(m1->nv, m2->nv); - for (int i = 0; i < m1->nv; i++) { - EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol); - } mj_forward(m2, d2); mj_forward(m1, d1); diff --git a/test/engine/engine_io_test.cc b/test/engine/engine_io_test.cc index a805566e..0dbc71ae 100644 --- a/test/engine/engine_io_test.cc +++ b/test/engine/engine_io_test.cc @@ -70,12 +70,17 @@ TEST_F(EngineIoTest, MakeDataFromPartialModel) { ASSERT_THAT(data_from_partial, NotNull()); EXPECT_EQ(data_from_partial->nbuffer, data_from_model->nbuffer); - int nbuffer = data_from_partial->nbuffer; // If there are no mocap bodies and qpos0 is all zero, mjData should be the // same whether it was made from the full model or the partial model. - EXPECT_EQ( - std::memcmp(data_from_partial->buffer, data_from_model->buffer, nbuffer), - 0) << "mjData content differs"; + { + MJDATA_POINTERS_PREAMBLE((&partial_model)) + #define X(type, name, nr, nc) \ + EXPECT_EQ(std::memcmp(data_from_partial->name, data_from_model->name, \ + sizeof(type)*(partial_model.nr)*(nc)), \ + 0) << "mjData::" #name " differs"; + MJDATA_POINTERS + #undef X + } mj_deleteData(data_from_model); mj_deleteData(data_from_partial); @@ -158,10 +163,15 @@ TEST_F(EngineIoTest, CopyDataWithPartialModel) { EXPECT_EQ(copy->nbuffer, data->nbuffer); EXPECT_EQ(copy->qpos[0], 1); - int nbuffer = copy->nbuffer; - EXPECT_EQ( - std::memcmp(copy->buffer, data->buffer, nbuffer), - 0) << "mjData content differs"; + { + MJDATA_POINTERS_PREAMBLE((&partial_model)) + #define X(type, name, nr, nc) \ + EXPECT_EQ(std::memcmp(copy->name, data->name, \ + sizeof(type)*(partial_model.nr)*(nc)), \ + 0) << "mjData::" #name " differs"; + MJDATA_POINTERS + #undef X + } mj_deleteData(data); mj_deleteData(copy);