diff --git a/doc/includes/references.h b/doc/includes/references.h index 7224e3a3..1c788150 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2170,6 +2170,7 @@ struct mjvSceneState_ { int nnames; int npaths; int nsensordata; + int narena; mjOption opt; mjVisual vis; @@ -2383,6 +2384,7 @@ struct mjvSceneState_ { mjtNum* ten_length; mjtNum* wrap_xpos; + mjtNum* bvh_aabb_dyn; mjtByte* bvh_active; int* island_dofadr; int* island_dofind; @@ -2394,6 +2396,7 @@ struct mjvSceneState_ { mjContact* contact; mjtNum* efc_force; + void* arena; } data; }; typedef struct mjvSceneState_ mjvSceneState; diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index d693df9c..dc8a4bc6 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -436,6 +436,7 @@ struct mjvSceneState_ { int nnames; int npaths; int nsensordata; + int narena; mjOption opt; mjVisual vis; @@ -649,6 +650,7 @@ struct mjvSceneState_ { mjtNum* ten_length; mjtNum* wrap_xpos; + mjtNum* bvh_aabb_dyn; mjtByte* bvh_active; int* island_dofadr; int* island_dofind; @@ -660,6 +662,7 @@ struct mjvSceneState_ { mjContact* contact; mjtNum* efc_force; + void* arena; } data; }; typedef struct mjvSceneState_ mjvSceneState; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index f4088314..86f61e0c 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -614,7 +614,7 @@ X ( mjtNum, qLD, nM, 1 ) \ X ( mjtNum, qLDiagInv, nv, 1 ) \ X ( mjtNum, qLDiagSqrtInv, nv, 1 ) \ - X ( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \ + XMJV( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \ XMJV( mjtByte, bvh_active, nbvh, 1 ) \ X ( mjtNum, flexedge_velocity, nflexedge, 1 ) \ X ( mjtNum, ten_velocity, ntendon, 1 ) \ diff --git a/introspect/structs.py b/introspect/structs.py index eea7f6cc..43e7cdc9 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -6334,6 +6334,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='', ), + StructFieldDecl( + name='narena', + type=ValueType(name='int'), + doc='', + ), StructFieldDecl( name='opt', type=ValueType(name='mjOption'), @@ -7596,6 +7601,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='bvh_aabb_dyn', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='', + ), StructFieldDecl( name='bvh_active', type=PointerType( @@ -7659,6 +7671,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='arena', + type=PointerType( + inner_type=ValueType(name='void'), + ), + doc='', + ), ), ), doc='', diff --git a/simulate/simulate.cc b/simulate/simulate.cc index abe5a23b..226e290d 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -1817,6 +1817,9 @@ void Simulate::Sync() { if (!m_) { return; } + if (this->exitrequest.load()) { + return; + } bool update_profiler = this->profiler && (this->pause_update || this->run); bool update_sensor = this->sensor && (this->pause_update || this->run); @@ -2714,9 +2717,9 @@ void Simulate::RenderLoop() { } } - if (!is_passive_){ - mjv_freeScene(&this->scn); - } else { + const MutexLock lock(this->mtx); + mjv_freeScene(&this->scn); + if (is_passive_) { mjv_freeSceneState(&scnstate_); } diff --git a/src/engine/engine_vis_state.c b/src/engine/engine_vis_state.c index 5ef3c274..55d35497 100644 --- a/src/engine/engine_vis_state.c +++ b/src/engine/engine_vis_state.c @@ -86,6 +86,11 @@ void mjv_makeSceneState(const mjModel* m, const mjData* d, mjvSceneState* scnsta #undef XMJV #undef X + // create an arena in the scnstate, to allow visualization code to use the stack. + // TODO: Consider allocating way less than narena, since stack allocations in + // visualization code are much smaller than the arena space required by the model, + // typically. + scnstate->nbuffer += roundUpToCacheLine(m->narena); // buffer space required for contacts int condimmax = mj_isPyramidal(m) ? 10 : 6; scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->contact) * maxgeom); @@ -118,6 +123,10 @@ void mjv_makeSceneState(const mjModel* m, const mjData* d, mjvSceneState* scnsta #undef XMJV #undef X + scnstate->model.narena = m->narena; + scnstate->data.arena = (void*)ptr; + ptr += roundUpToCacheLine(m->narena); + scnstate->data.contact = (mjContact*)ptr; ptr += roundUpToCacheLine(sizeof(*scnstate->data.contact) * scnstate->maxgeom); @@ -177,6 +186,7 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData* m->opt = scnstate->model.opt; m->vis = scnstate->model.vis; m->stat = scnstate->model.stat; + m->narena = scnstate->model.narena; #define X(dtype, var, dim0, dim1) #define XMJV(dtype, var, dim0, dim1) m->var = scnstate->model.var; @@ -194,10 +204,16 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData* #endif memcpy(d->warning, scnstate->data.warning, sizeof(d->warning)); + d->threadpool = 0; d->nefc = scnstate->data.nefc; d->ncon = scnstate->data.ncon; d->nisland = scnstate->data.nisland; d->time = scnstate->data.time; + d->narena = scnstate->model.narena; + d->arena = scnstate->data.arena; + d->parena = 0; + d->pbase = 0; + d->pstack = 0; #define X(dtype, var, dim0, dim1) #define XMJV(dtype, var, dim0, dim1) d->var = scnstate->data.var; diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index fb77205d..727cf955 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -99,5 +99,10 @@ target_link_libraries(engine_util_spatial_test fixture gmock) mujoco_test(engine_vfs_test) target_link_libraries(engine_vfs_test fixture gmock) -mujoco_test(engine_vis_state_test) +mujoco_test( + engine_vis_state_test + PROPERTIES + ENVIRONMENT + "MUJOCO_PLUGIN_DIR=$" +) target_link_libraries(engine_vis_state_test fixture gmock) diff --git a/test/engine/engine_vis_state_test.cc b/test/engine/engine_vis_state_test.cc index 48d48efd..6fefde47 100644 --- a/test/engine/engine_vis_state_test.cc +++ b/test/engine/engine_vis_state_test.cc @@ -35,17 +35,17 @@ static const char* const kTendonPath = "engine/testdata/island/tendon_wrap.xml"; static const char* const kFrustumPath = "engine/testdata/vis_visualize/frustum.xml"; -static const char* const kModelPath = - "testdata/model.xml"; +static const char* const kFlex = "testdata/flex.xml"; +static const char* const kModelPath = "testdata/model.xml"; #define EXPECT_ZERO(exp) EXPECT_EQ(0, exp); TEST_F(MjvSceneStateTest, CanUpdateFromState) { for (const char* path : - {kHammockPath, kTendonPath, kModelPath, kFrustumPath}) { + {kHammockPath, kTendonPath, kModelPath, kFrustumPath, kFlex}) { const std::string xml_path = GetTestDataFilePath(path); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); - ASSERT_THAT(model, NotNull()); + ASSERT_THAT(model, NotNull()) << "Failed to load model from " << path; mjData* data = mj_makeData(model); while (data->time < 2) { diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index d773cf87..213cba7a 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -6095,6 +6095,7 @@ public unsafe struct model { public int nnames; public int npaths; public int nsensordata; + public int narena; public mjOption_ opt; public mjVisual_ vis; public mjStatistic_ stat; @@ -6287,6 +6288,7 @@ public unsafe struct data { public int* wrap_obj; public double* ten_length; public double* wrap_xpos; + public double* bvh_aabb_dyn; public byte* bvh_active; public int* island_dofadr; public int* island_dofind; @@ -6296,6 +6298,7 @@ public unsafe struct data { public double* flexvert_xpos; public mjContact_* contact; public double* efc_force; + public void* arena; } [StructLayout(LayoutKind.Sequential)]