chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix multiple bugs related to mjvSceneState.

Browse Source 
PiperOrigin-RevId: 557864131
Change-Id: I47d9692930c54b1304a5cc161f99bbd81d992a7c
This commit is contained in:
Yuval Tassa
2023-08-17 10:24:44 -07:00
committed by Copybara-Service
parent cf310079e2
commit b1ecc943bb
11 changed files with 302 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/includes/references.h
+11
View File
@@ -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;
include/mujoco/mjdata.h
+1
View File
@@ -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)
include/mujoco/mjvisualize.h
+10
View File
@@ -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;
include/mujoco/mjxmacro.h
+36 -35
View File
@@ -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 \
introspect/structs.py
+73
View File
@@ -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(
src/engine/engine_core_constraint.c
+24 -1
View File
@@ -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;
src/engine/engine_print.c
+7
View File
@@ -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
src/engine/engine_vis_state.c
+44 -16
View File
@@ -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);
}
}
src/engine/engine_vis_visualize.c
+4 -17
View File
@@ -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);
test/engine/engine_vis_state_test.cc
+81 -78
View File
@@ -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<char*>(&scn1.nlight);
auto scn2_cmp_begin = reinterpret_cast<char*>(&scn2.nlight);
auto cmp_bytes =
sizeof(mjvScene) - (scn2_cmp_begin - reinterpret_cast<char*>(&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<char*>(&scn1.nlight);
auto scn2_cmp_begin = reinterpret_cast<char*>(&scn2.nlight);
auto cmp_bytes =
sizeof(mjvScene) - (scn2_cmp_begin - reinterpret_cast<char*>(&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
unity/Runtime/Bindings/MjBindings.cs
+11
View File
@@ -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;
}