From b1ecc943bb3ccd6d09e5c23a644c312e14b22cf4 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 17 Aug 2023 10:24:44 -0700 Subject: [PATCH] Fix multiple bugs related to mjvSceneState. PiperOrigin-RevId: 557864131 Change-Id: I47d9692930c54b1304a5cc161f99bbd81d992a7c --- doc/includes/references.h | 11 ++ include/mujoco/mjdata.h | 1 + include/mujoco/mjvisualize.h | 10 ++ include/mujoco/mjxmacro.h | 71 ++++++------ introspect/structs.py | 73 ++++++++++++ src/engine/engine_core_constraint.c | 25 ++++- src/engine/engine_print.c | 7 ++ src/engine/engine_vis_state.c | 60 +++++++--- src/engine/engine_vis_visualize.c | 21 +--- test/engine/engine_vis_state_test.cc | 159 ++++++++++++++------------- unity/Runtime/Bindings/MjBindings.cs | 11 ++ 11 files changed, 302 insertions(+), 147 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 4cb9404e..6235de11 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -342,6 +342,7 @@ struct mjData_ { mjtNum* efc_KBIP; // stiffness, damping, impedance, imp' (nefc x 4) mjtNum* efc_D; // constraint mass (nefc x 1) mjtNum* efc_R; // inverse constraint mass (nefc x 1) + int* tendon_efcadr; // first efc address involving tendon; -1: none (ntendon x 1) // computed by mj_island int* island_dofadr; // address of first dof in island (nisland x 1) @@ -1952,6 +1953,7 @@ struct mjvSceneState_ { // fields in mjModel that are necessary to re-render a scene struct { + int nv; int nu; int na; int nbody; @@ -1970,6 +1972,7 @@ struct mjvSceneState_ { int nmat; int neq; int ntendon; + int ntree; int nwrap; int nsensor; int nnames; @@ -1985,6 +1988,8 @@ struct mjvSceneState_ { int* body_mocapid; int* body_jntnum; int* body_jntadr; + int* body_dofnum; + int* body_dofadr; int* body_geomnum; int* body_geomadr; mjtNum* body_iquat; @@ -2004,6 +2009,8 @@ struct mjvSceneState_ { int* geom_type; int* geom_bodyid; + int* geom_contype; + int* geom_conaffinity; int* geom_dataid; int* geom_matid; int* geom_group; @@ -2032,6 +2039,8 @@ struct mjvSceneState_ { float* light_diffuse; float* light_specular; + int* mesh_bvhadr; + int* mesh_bvhnum; int* mesh_texcoordadr; int* mesh_graphadr; @@ -2149,12 +2158,14 @@ struct mjvSceneState_ { int* ten_wrapadr; int* ten_wrapnum; int* wrap_obj; + mjtNum* ten_length; mjtNum* wrap_xpos; mjtByte* bvh_active; int* island_dofadr; int* dof_island; int* efc_island; + int* tendon_efcadr; mjContact* contact; mjtNum* efc_force; diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 9c6781d5..204222b9 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -369,6 +369,7 @@ struct mjData_ { mjtNum* efc_KBIP; // stiffness, damping, impedance, imp' (nefc x 4) mjtNum* efc_D; // constraint mass (nefc x 1) mjtNum* efc_R; // inverse constraint mass (nefc x 1) + int* tendon_efcadr; // first efc address involving tendon; -1: none (ntendon x 1) // computed by mj_island int* island_dofadr; // address of first dof in island (nisland x 1) diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index f4aee119..f406aa84 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -378,6 +378,7 @@ struct mjvSceneState_ { // fields in mjModel that are necessary to re-render a scene struct { + int nv; int nu; int na; int nbody; @@ -396,6 +397,7 @@ struct mjvSceneState_ { int nmat; int neq; int ntendon; + int ntree; int nwrap; int nsensor; int nnames; @@ -411,6 +413,8 @@ struct mjvSceneState_ { int* body_mocapid; int* body_jntnum; int* body_jntadr; + int* body_dofnum; + int* body_dofadr; int* body_geomnum; int* body_geomadr; mjtNum* body_iquat; @@ -430,6 +434,8 @@ struct mjvSceneState_ { int* geom_type; int* geom_bodyid; + int* geom_contype; + int* geom_conaffinity; int* geom_dataid; int* geom_matid; int* geom_group; @@ -458,6 +464,8 @@ struct mjvSceneState_ { float* light_diffuse; float* light_specular; + int* mesh_bvhadr; + int* mesh_bvhnum; int* mesh_texcoordadr; int* mesh_graphadr; @@ -575,12 +583,14 @@ struct mjvSceneState_ { int* ten_wrapadr; int* ten_wrapnum; int* wrap_obj; + mjtNum* ten_length; mjtNum* wrap_xpos; mjtByte* bvh_active; int* island_dofadr; int* dof_island; int* efc_island; + int* tendon_efcadr; mjContact* contact; mjtNum* efc_force; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 32eebff4..c6a222bb 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -65,7 +65,7 @@ // int fields of mjModel #define MJMODEL_INTS \ X ( nq ) \ - X ( nv ) \ + XMJV( nv ) \ XMJV( nu ) \ XMJV( na ) \ XMJV( nbody ) \ @@ -124,7 +124,7 @@ X ( nemax ) \ X ( njmax ) \ X ( nconmax ) \ - X ( ntree ) \ + XMJV( ntree ) \ X ( nstack ) \ X ( nuserdata ) \ XMJV( nsensordata ) \ @@ -165,8 +165,8 @@ XMJV( int, body_mocapid, nbody, 1 ) \ XMJV( int, body_jntnum, nbody, 1 ) \ XMJV( int, body_jntadr, nbody, 1 ) \ - X ( int, body_dofnum, nbody, 1 ) \ - X ( int, body_dofadr, nbody, 1 ) \ + XMJV( int, body_dofnum, nbody, 1 ) \ + XMJV( int, body_dofadr, nbody, 1 ) \ X ( int, body_treeid, nbody, 1 ) \ XMJV( int, body_geomnum, nbody, 1 ) \ XMJV( int, body_geomadr, nbody, 1 ) \ @@ -219,8 +219,8 @@ X ( mjtNum, dof_invweight0, nv, 1 ) \ X ( mjtNum, dof_M0, nv, 1 ) \ XMJV( int, geom_type, ngeom, 1 ) \ - X ( int, geom_contype, ngeom, 1 ) \ - X ( int, geom_conaffinity, ngeom, 1 ) \ + XMJV( int, geom_contype, ngeom, 1 ) \ + XMJV( int, geom_conaffinity, ngeom, 1 ) \ X ( int, geom_condim, ngeom, 1 ) \ XMJV( int, geom_bodyid, ngeom, 1 ) \ XMJV( int, geom_dataid, ngeom, 1 ) \ @@ -289,8 +289,8 @@ X ( int, mesh_texcoordnum, nmesh, 1 ) \ X ( int, mesh_faceadr, nmesh, 1 ) \ X ( int, mesh_facenum, nmesh, 1 ) \ - X ( int, mesh_bvhadr, nmesh, 1 ) \ - X ( int, mesh_bvhnum, nmesh, 1 ) \ + XMJV( int, mesh_bvhadr, nmesh, 1 ) \ + XMJV( int, mesh_bvhnum, nmesh, 1 ) \ XMJV( int, mesh_graphadr, nmesh, 1 ) \ X ( float, mesh_vert, nmeshvert, 3 ) \ X ( float, mesh_normal, nmeshnormal, 3 ) \ @@ -515,7 +515,7 @@ X ( int, ten_J_rownnz, ntendon, 1 ) \ X ( int, ten_J_rowadr, ntendon, 1 ) \ X ( int, ten_J_colind, ntendon, MJ_M(nv) ) \ - X ( mjtNum, ten_length, ntendon, 1 ) \ + XMJV( mjtNum, ten_length, ntendon, 1 ) \ X ( mjtNum, ten_J, ntendon, MJ_M(nv) ) \ XMJV( int, wrap_obj, nwrap, 2 ) \ XMJV( mjtNum, wrap_xpos, nwrap, 6 ) \ @@ -561,35 +561,36 @@ #define MJ_D(n) n // array of contacts -#define MJDATA_ARENA_POINTERS_CONTACT \ +#define MJDATA_ARENA_POINTERS_CONTACT \ X( mjContact, contact, MJ_D(ncon), 1 ) // array fields of mjData that are used in the primal problem -#define MJDATA_ARENA_POINTERS_PRIMAL \ - X( int, efc_type, MJ_D(nefc), 1) \ - X( int, efc_id, MJ_D(nefc), 1) \ - X( int, efc_J_rownnz, MJ_D(nefc), 1) \ - X( int, efc_J_rowadr, MJ_D(nefc), 1) \ - X( int, efc_J_rowsuper, MJ_D(nefc), 1) \ - X( int, efc_J_colind, MJ_D(nnzJ), 1) \ - X( int, efc_JT_rownnz, MJ_M(nv), 1) \ - X( int, efc_JT_rowadr, MJ_M(nv), 1) \ - X( int, efc_JT_rowsuper, MJ_M(nv), 1) \ - X( int, efc_JT_colind, MJ_D(nnzJ), 1) \ - X( mjtNum, efc_J, MJ_D(nnzJ), 1) \ - X( mjtNum, efc_JT, MJ_D(nnzJ), 1) \ - X( mjtNum, efc_pos, MJ_D(nefc), 1) \ - X( mjtNum, efc_margin, MJ_D(nefc), 1) \ - X( mjtNum, efc_frictionloss, MJ_D(nefc), 1) \ - X( mjtNum, efc_diagApprox, MJ_D(nefc), 1) \ - X( mjtNum, efc_KBIP, MJ_D(nefc), 4) \ - X( mjtNum, efc_D, MJ_D(nefc), 1) \ - X( mjtNum, efc_R, MJ_D(nefc), 1) \ - X( mjtNum, efc_vel, MJ_D(nefc), 1) \ - X( mjtNum, efc_aref, MJ_D(nefc), 1) \ - X( mjtNum, efc_b, MJ_D(nefc), 1) \ - X( mjtNum, efc_force, MJ_D(nefc), 1) \ - X( int, efc_state, MJ_D(nefc), 1) +#define MJDATA_ARENA_POINTERS_PRIMAL \ + X( int, efc_type, MJ_D(nefc), 1) \ + X( int, efc_id, MJ_D(nefc), 1) \ + X( int, efc_J_rownnz, MJ_D(nefc), 1) \ + X( int, efc_J_rowadr, MJ_D(nefc), 1) \ + X( int, efc_J_rowsuper, MJ_D(nefc), 1) \ + X( int, efc_J_colind, MJ_D(nnzJ), 1) \ + X( int, efc_JT_rownnz, MJ_M(nv), 1) \ + X( int, efc_JT_rowadr, MJ_M(nv), 1) \ + X( int, efc_JT_rowsuper, MJ_M(nv), 1) \ + X( int, efc_JT_colind, MJ_D(nnzJ), 1) \ + X( mjtNum, efc_J, MJ_D(nnzJ), 1) \ + X( mjtNum, efc_JT, MJ_D(nnzJ), 1) \ + X( mjtNum, efc_pos, MJ_D(nefc), 1) \ + X( mjtNum, efc_margin, MJ_D(nefc), 1) \ + X( mjtNum, efc_frictionloss, MJ_D(nefc), 1) \ + X( mjtNum, efc_diagApprox, MJ_D(nefc), 1) \ + X( mjtNum, efc_KBIP, MJ_D(nefc), 4) \ + X( mjtNum, efc_D, MJ_D(nefc), 1) \ + X( mjtNum, efc_R, MJ_D(nefc), 1) \ + X( int, tendon_efcadr, MJ_M(ntendon), 1) \ + X( mjtNum, efc_vel, MJ_D(nefc), 1) \ + X( mjtNum, efc_aref, MJ_D(nefc), 1) \ + X( mjtNum, efc_b, MJ_D(nefc), 1) \ + X( mjtNum, efc_force, MJ_D(nefc), 1) \ + X( int, efc_state, MJ_D(nefc), 1) // array fields of mjData that are used in the dual problem #define MJDATA_ARENA_POINTERS_DUAL \ diff --git a/introspect/structs.py b/introspect/structs.py index 24a6ead9..ea99631e 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -4332,6 +4332,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='inverse constraint mass (nefc x 1)', # pylint: disable=line-too-long ), + StructFieldDecl( + name='tendon_efcadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='first efc address involving tendon; -1: none (ntendon x 1)', # pylint: disable=line-too-long + ), StructFieldDecl( name='island_dofadr', type=PointerType( @@ -5315,6 +5322,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ name='model', type=AnonymousStructDecl( fields=( + StructFieldDecl( + name='nv', + type=ValueType(name='int'), + doc='', + ), StructFieldDecl( name='nu', type=ValueType(name='int'), @@ -5405,6 +5417,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='', ), + StructFieldDecl( + name='ntree', + type=ValueType(name='int'), + doc='', + ), StructFieldDecl( name='nwrap', type=ValueType(name='int'), @@ -5482,6 +5499,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='body_dofnum', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='', + ), + StructFieldDecl( + name='body_dofadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='', + ), StructFieldDecl( name='body_geomnum', type=PointerType( @@ -5594,6 +5625,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='geom_contype', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='', + ), + StructFieldDecl( + name='geom_conaffinity', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='', + ), StructFieldDecl( name='geom_dataid', type=PointerType( @@ -5762,6 +5807,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='mesh_bvhadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='', + ), + StructFieldDecl( + name='mesh_bvhnum', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='', + ), StructFieldDecl( name='mesh_texcoordadr', type=PointerType( @@ -6449,6 +6508,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='ten_length', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='', + ), StructFieldDecl( name='wrap_xpos', type=PointerType( @@ -6484,6 +6550,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='tendon_efcadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='', + ), StructFieldDecl( name='contact', type=PointerType( diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index b1359b33..a29c6993 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -486,7 +486,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { case mjEQ_JOINT: // couple joint values with cubic case mjEQ_TENDON: // couple tendon lengths with cubic // get scalar positions and their Jacobians - for (int j=0; j < 1+(id[1] >= 0); j++) + for (int j=0; j < 1+(id[1] >= 0); j++) { if (m->eq_type[i] == mjEQ_JOINT) { // joint object pos[j][0] = d->qpos[m->jnt_qposadr[id[j]]]; ref[j] = m->qpos0[m->jnt_qposadr[id[j]]]; @@ -511,6 +511,11 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { pos[j][0] = d->ten_length[id[j]]; ref[j] = m->tendon_length0[id[j]]; + // set tendon_efcadr + if (d->tendon_efcadr[id[j]] == -1) { + d->tendon_efcadr[id[j]] = i; + } + // copy Jacobian: sparse or dense if (issparse) { // add first or second chain @@ -527,6 +532,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mju_copy(jac[j], d->ten_J+id[j]*nv, nv); } } + } // both objects defined if (id[1] >= 0) { @@ -618,6 +624,7 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) { // find frictional tendons for (int i=0; i < m->ntendon; i++) { if (m->tendon_frictionloss[i] > 0) { + int efcadr = d->nefc; // add constraint if (mj_addConstraint(m, d, d->ten_J + (issparse ? d->ten_J_rowadr[i] : i*nv), 0, 0, m->tendon_frictionloss[i], @@ -625,6 +632,11 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) { issparse ? d->ten_J_rownnz[i] : 0, issparse ? d->ten_J_colind+d->ten_J_rowadr[i] : NULL)) { break; + } else { + // set tendon_efcadr + if (d->tendon_efcadr[i] == -1) { + d->tendon_efcadr[i] = efcadr; + } } } } @@ -757,11 +769,17 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { } // add constraint + int efcadr = d->nefc; if (mj_addConstraint(m, d, jac, &dist, &margin, 0, 1, mjCNSTR_LIMIT_TENDON, i, issparse ? d->ten_J_rownnz[i] : 0, issparse ? d->ten_J_colind+d->ten_J_rowadr[i] : NULL)) { break; + } else { + // set tendon_efcadr + if (d->tendon_efcadr[i] == -1) { + d->tendon_efcadr[i] = efcadr; + } } } } @@ -1654,6 +1672,11 @@ void mj_makeConstraint(const mjModel* m, mjData* d) { #define MJ_D(n) n // ========== end arena allocation + // clear tendon_efcadr + for (int i=0; i < m->ntendon; i++) { + d->tendon_efcadr[i] = -1; + } + // reset nefc for the instantiation functions, // and instantiate all elements of Jacobian d->nefc = 0; diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 1df38560..6a41d8de 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -1089,7 +1089,14 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, fprintf(fp, " %d", d->efc_islandnext[i]); } fprintf(fp, "\n\n"); + } + if (m->ntendon) { + fprintf(fp, NAME_FORMAT, "TENDON_EFCADR"); + for (int i = 0; i < m->ntendon; i++) { + fprintf(fp, " %d", d->tendon_efcadr[i]); + } + fprintf(fp, "\n\n"); } #ifdef MEMORY_SANITIZER diff --git a/src/engine/engine_vis_state.c b/src/engine/engine_vis_state.c index 678ffe4e..444769b6 100644 --- a/src/engine/engine_vis_state.c +++ b/src/engine/engine_vis_state.c @@ -86,9 +86,16 @@ void mjv_makeSceneState(const mjModel* m, const mjData* d, mjvSceneState* scnsta #undef XMJV #undef X + // buffer space required for contacts int condimmax = mj_isPyramidal(m) ? 10 : 6; - scnstate->nbuffer += roundUpToCacheLine(sizeof(mjContact) * maxgeom); - scnstate->nbuffer += roundUpToCacheLine(sizeof(mjtNum) * maxgeom * condimmax); + scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->contact) * maxgeom); + scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->efc_force) * maxgeom * condimmax); + + // buffer space required for islands + scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->island_dofadr) * m->ntree); + scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->dof_island) * m->nv); + scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->efc_island) * maxgeom * condimmax); + scnstate->nbuffer += roundUpToCacheLine(sizeof(*d->tendon_efcadr) * m->ntendon); scnstate->buffer = mju_malloc(scnstate->nbuffer); @@ -111,10 +118,22 @@ void mjv_makeSceneState(const mjModel* m, const mjData* d, mjvSceneState* scnsta #undef X scnstate->data.contact = (mjContact*)ptr; - ptr += roundUpToCacheLine(sizeof(mjContact) * scnstate->maxgeom); + ptr += roundUpToCacheLine(sizeof(*scnstate->data.contact) * scnstate->maxgeom); scnstate->data.efc_force = (mjtNum*)ptr; - ptr += roundUpToCacheLine(sizeof(mjtNum) * scnstate->maxgeom * condimmax); + ptr += roundUpToCacheLine(sizeof(*scnstate->data.efc_force) * scnstate->maxgeom * condimmax); + + scnstate->data.island_dofadr = (int*)ptr; + ptr += roundUpToCacheLine(sizeof(*scnstate->data.island_dofadr) * scnstate->model.ntree); + + scnstate->data.dof_island = (int*)ptr; + ptr += roundUpToCacheLine(sizeof(*scnstate->data.dof_island) * scnstate->model.nv); + + scnstate->data.efc_island = (int*)ptr; + ptr += roundUpToCacheLine(sizeof(*scnstate->data.efc_island) * scnstate->maxgeom * condimmax); + + scnstate->data.tendon_efcadr = (int*)ptr; + ptr += roundUpToCacheLine(sizeof(*scnstate->data.tendon_efcadr) * m->ntendon); // should not occur if (ptr - (char*)scnstate->buffer != scnstate->nbuffer) { @@ -184,12 +203,10 @@ void mjv_assignFromSceneState(const mjvSceneState* scnstate, mjModel* m, mjData* d->contact = scnstate->data.contact; d->efc_force = scnstate->data.efc_force; - - if (d->nisland) { - d->island_dofadr = scnstate->data.island_dofadr; - d->dof_island = scnstate->data.dof_island; - d->efc_island = scnstate->data.efc_island; - } + d->island_dofadr = scnstate->data.island_dofadr; + d->dof_island = scnstate->data.dof_island; + d->efc_island = scnstate->data.efc_island; + d->tendon_efcadr = scnstate->data.tendon_efcadr; } } @@ -298,16 +315,17 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt, } else { scnstate->data.ncon = d->ncon; } - memcpy(scnstate->data.contact, d->contact, sizeof(mjContact) * scnstate->data.ncon); + memcpy(scnstate->data.contact, d->contact, sizeof(*d->contact) * scnstate->data.ncon); } - // Copy only the entries in efc_force that correspond to contacts. + // Copy only the entries in efc_force and efc_island that correspond to contacts. { scnstate->data.nefc = 0; for (int i = 0; i < scnstate->data.ncon; ++i) { const mjContact* con = &d->contact[i]; scnstate->data.nefc += con->dim; } + scnstate->data.nefc += scnstate->model.ntendon; int efc_address = 0; int ispyramid = mj_isPyramidal(m); @@ -319,20 +337,30 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt, } for (int j = 0; j < dim; ++j) { scnstate->data.efc_force[efc_address + j] = d->efc_force[con->efc_address + j]; + if (d->nisland) { + scnstate->data.efc_island[efc_address + j] = d->efc_island[con->efc_address + j]; + } } con->efc_address = efc_address; efc_address += dim; } + if (d->nisland) { + for (int i = 0; i < scnstate->model.ntendon; ++i) { + int efcadr = d->tendon_efcadr[i]; + if (efcadr != -1) { + scnstate->data.efc_island[efcadr] = d->efc_island[efcadr]; + } + } + } } // Copy island data. scnstate->data.nisland = d->nisland; if (d->nisland) { - memcpy(scnstate->data.island_dofadr, d->island_dofadr, sizeof(int) * d->nisland); - memcpy(scnstate->data.dof_island, d->dof_island, sizeof(int) * m->nv); - memcpy(scnstate->data.efc_island, d->efc_island, sizeof(int) * d->nefc); + memcpy(scnstate->data.island_dofadr, d->island_dofadr, sizeof(*d->island_dofadr) * d->nisland); + memcpy(scnstate->data.dof_island, d->dof_island, sizeof(*d->dof_island) * m->nv); + memcpy(scnstate->data.tendon_efcadr, d->tendon_efcadr, sizeof(*d->tendon_efcadr) * m->ntendon); } - } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index ccc357ca..a83850f3 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -73,6 +73,7 @@ static void makeLabel(const mjModel* m, mjtObj type, int id, char* label) { return; \ } else { \ thisgeom = scn->geoms + scn->ngeom; \ + memset(thisgeom, 0, sizeof(mjvGeom)); \ mjv_initGeom(thisgeom, mjGEOM_NONE, NULL, NULL, NULL, NULL); \ thisgeom->objtype = objtype; \ thisgeom->objid = i; \ @@ -1580,23 +1581,9 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, if (vopt->flags[mjVIS_ISLAND] && d->nisland) { tendon_matid = -1; rgba = rgba_island; - int frictional = m->tendon_frictionloss[i] > 0; - int limited = m->tendon_limited[i] && (ten_length <= lower || ten_length >= upper); - if (frictional || limited) { - // search for tendon's island - int island = -1; - for (int k=0; k < d->nefc; k++) { - int istendon = d->efc_type[k] == mjCNSTR_FRICTION_TENDON || - d->efc_type[k] == mjCNSTR_LIMIT_TENDON; - if (istendon && d->efc_id[k] == i) { - island = d->efc_island[k]; - break; - } - } - if (island > -1) { - // set color using island's first dof - islandColor(rgba_island, d->island_dofadr[island]); - } + if (d->tendon_efcadr[i] != -1) { + // set color using island's first dof + islandColor(rgba_island, d->island_dofadr[d->efc_island[d->tendon_efcadr[i]]]); } } setMaterial(m, thisgeom, tendon_matid, rgba, vopt->flags); diff --git a/test/engine/engine_vis_state_test.cc b/test/engine/engine_vis_state_test.cc index 04b87d6a..bc43e8c2 100644 --- a/test/engine/engine_vis_state_test.cc +++ b/test/engine/engine_vis_state_test.cc @@ -29,87 +29,90 @@ using MjvSceneStateTest = MujocoTest; constexpr int kMaxGeom = 10000; +static const char* const kHammockPath = + "engine/testdata/hammock/hammock.xml"; +static const char* const kTendonPath = + "engine/testdata/island/tendon_wrap.xml"; +static const char* const kModelPath = "testdata/model.xml"; + +#define EXPECT_ZERO(exp) EXPECT_EQ(0, exp); + TEST_F(MjvSceneStateTest, CanUpdateFromState) { - constexpr char path[] = "engine/testdata/hammock/hammock.xml"; - const std::string xml_path = GetTestDataFilePath(path); - mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); - ASSERT_THAT(model, NotNull()); - mjData* data = mj_makeData(model); + for (const char* path : {kHammockPath, kTendonPath, kModelPath}) { + const std::string xml_path = GetTestDataFilePath(path); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); - while (data->time < 2) { - mj_step(model, data); + while (data->time < 2) { + mj_step(model, data); + } + + mjvScene scn1; + mjv_defaultScene(&scn1); + mjv_makeScene(model, &scn1, kMaxGeom); + + mjvOption opt; + mjv_defaultOption(&opt); + + mjvPerturb pert; + mjv_defaultPerturb(&pert); + + mjvCamera cam; + mjv_defaultFreeCamera(model, &cam); + + // Enable all flags to exercise all code paths + for (int i = 0; i < mjNVISFLAG; ++i) { + opt.flags[i] = 1; + } + + mjv_updateScene(model, data, &opt, &pert, &cam, mjCAT_ALL, &scn1); + EXPECT_GT(scn1.ngeom, 0); + if (model->nskin) EXPECT_GT(scn1.nskin, 0); + EXPECT_GT(scn1.nlight, 0); + + mjvSceneState scnstate; + mjv_defaultSceneState(&scnstate); + mjv_makeSceneState(model, data, &scnstate, kMaxGeom); + mjv_updateSceneState(model, data, &opt, &scnstate); + + mjvScene scn2; + mjv_defaultScene(&scn2); + mjv_makeScene(model, &scn2, kMaxGeom); + mjv_updateSceneFromState(&scnstate, &opt, &pert, &cam, mjCAT_ALL, &scn2); + + EXPECT_EQ(scn1.ngeom, scn2.ngeom); + for (int i = 0; i < scn1.ngeom; ++i) { + EXPECT_ZERO(std::memcmp(&scn1.geoms[i], &scn2.geoms[i], sizeof(mjvGeom))); + } + // NB: scn->geomorder is a scratch space for use by mjr_render, so we don't + // need to compare them here. + + EXPECT_LE(scn1.nskin, scn2.nskin); + EXPECT_ZERO(std::memcmp(scn1.skinfacenum, scn2.skinfacenum, + sizeof(*scn2.skinfacenum) * scn2.nskin)); + EXPECT_ZERO(std::memcmp(scn1.skinvertadr, scn2.skinvertadr, + sizeof(*scn2.skinvertadr) * scn2.nskin)); + EXPECT_ZERO(std::memcmp(scn1.skinvertnum, scn2.skinvertnum, + sizeof(*scn2.skinvertnum) * scn2.nskin)); + EXPECT_ZERO(std::memcmp(scn1.skinvert, scn2.skinvert, + sizeof(*scn2.skinvert) * scn2.nskin)); + EXPECT_ZERO(std::memcmp(scn1.skinnormal, scn2.skinnormal, + sizeof(*scn2.skinnormal) * scn2.nskin)); + + auto scn1_cmp_begin = reinterpret_cast(&scn1.nlight); + auto scn2_cmp_begin = reinterpret_cast(&scn2.nlight); + auto cmp_bytes = + sizeof(mjvScene) - (scn2_cmp_begin - reinterpret_cast(&scn2)); + EXPECT_ZERO(std::memcmp(scn1_cmp_begin, scn2_cmp_begin, cmp_bytes)); + + mjv_freeScene(&scn1); + mjv_freeScene(&scn2); + mjv_freeSceneState(&scnstate); + + mj_deleteData(data); + mj_deleteModel(model); } - ASSERT_GT(data->ncon, 10); - - mjvScene scn1; - mjv_defaultScene(&scn1); - mjv_makeScene(model, &scn1, kMaxGeom); - - mjvOption opt; - mjv_defaultOption(&opt); - - mjvPerturb pert; - mjv_defaultPerturb(&pert); - - mjvCamera cam; - mjv_defaultFreeCamera(model, &cam); - - // Enable all flags to exercise all code paths - for (int i = 0; i < mjNVISFLAG; ++i) { - opt.flags[i] = 1; - } - - mjv_updateScene(model, data, &opt, &pert, &cam, mjCAT_ALL, &scn1); - EXPECT_GT(scn1.ngeom, 0); - EXPECT_GT(scn1.nskin, 0); - EXPECT_GT(scn1.nlight, 0); - - mjvSceneState scnstate; - mjv_defaultSceneState(&scnstate); - mjv_makeSceneState(model, data, &scnstate, kMaxGeom); - mjv_updateSceneState(model, data, &opt, &scnstate); - - mjvScene scn2; - mjv_defaultScene(&scn2); - mjv_makeScene(model, &scn2, kMaxGeom); - mjv_updateSceneFromState(&scnstate, &opt, &pert, &cam, mjCAT_ALL, &scn2); - - EXPECT_EQ(scn1.ngeom, scn2.ngeom); - for (int i = 0; i < scn1.ngeom; ++i) { - EXPECT_EQ(std::memcmp(&scn1.geoms[i], &scn2.geoms[i], sizeof(mjvGeom)), 0); - } - // NB: scn->geomorder is a scratch space for use by mjr_render, so we don't - // need to compare them here. - - EXPECT_LE(scn1.nskin, scn2.nskin); - EXPECT_EQ(std::memcmp(scn1.skinfacenum, scn2.skinfacenum, - sizeof(*scn2.skinfacenum) * scn2.nskin), - 0); - EXPECT_EQ(std::memcmp(scn1.skinvertadr, scn2.skinvertadr, - sizeof(*scn2.skinvertadr) * scn2.nskin), - 0); - EXPECT_EQ(std::memcmp(scn1.skinvertnum, scn2.skinvertnum, - sizeof(*scn2.skinvertnum) * scn2.nskin), - 0); - EXPECT_EQ(std::memcmp(scn1.skinvert, scn2.skinvert, - sizeof(*scn2.skinvert) * scn2.nskin), - 0); - EXPECT_EQ(std::memcmp(scn1.skinnormal, scn2.skinnormal, - sizeof(*scn2.skinnormal) * scn2.nskin), - 0); - - auto scn1_cmp_begin = reinterpret_cast(&scn1.nlight); - auto scn2_cmp_begin = reinterpret_cast(&scn2.nlight); - auto cmp_bytes = - sizeof(mjvScene) - (scn2_cmp_begin - reinterpret_cast(&scn2)); - EXPECT_EQ(std::memcmp(scn1_cmp_begin, scn2_cmp_begin, cmp_bytes), 0); - - mjv_freeScene(&scn1); - mjv_freeScene(&scn2); - mjv_freeSceneState(&scnstate); - - mj_deleteData(data); - mj_deleteModel(model); } } // namespace diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index d4dce44d..3b01e5ab 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1713,6 +1713,7 @@ public unsafe struct mjData_ { public double* efc_KBIP; public double* efc_D; public double* efc_R; + public int* tendon_efcadr; public int* island_dofadr; public int* island_efcadr; public int* dof_island; @@ -2775,6 +2776,7 @@ public unsafe struct mjvFigure_ { [StructLayout(LayoutKind.Sequential)] public unsafe struct model { + public int nv; public int nu; public int na; public int nbody; @@ -2793,6 +2795,7 @@ public unsafe struct model { public int nmat; public int neq; public int ntendon; + public int ntree; public int nwrap; public int nsensor; public int nnames; @@ -2806,6 +2809,8 @@ public unsafe struct model { public int* body_mocapid; public int* body_jntnum; public int* body_jntadr; + public int* body_dofnum; + public int* body_dofadr; public int* body_geomnum; public int* body_geomadr; public double* body_iquat; @@ -2822,6 +2827,8 @@ public unsafe struct model { public int* jnt_group; public int* geom_type; public int* geom_bodyid; + public int* geom_contype; + public int* geom_conaffinity; public int* geom_dataid; public int* geom_matid; public int* geom_group; @@ -2846,6 +2853,8 @@ public unsafe struct model { public float* light_ambient; public float* light_diffuse; public float* light_specular; + public int* mesh_bvhadr; + public int* mesh_bvhnum; public int* mesh_texcoordadr; public int* mesh_graphadr; public int* skin_matid; @@ -2955,11 +2964,13 @@ public unsafe struct data { public int* ten_wrapadr; public int* ten_wrapnum; public int* wrap_obj; + public double* ten_length; public double* wrap_xpos; public byte* bvh_active; public int* island_dofadr; public int* dof_island; public int* efc_island; + public int* tendon_efcadr; public mjContact_* contact; public double* efc_force; }