chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 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
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);