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

Add reduced sparse dof-dof matrix structures to mjData.

Browse Source 
PiperOrigin-RevId: 670919263
Change-Id: I3592de802ae84648d97230115c2e9c3437424968
This commit is contained in:
Yuval Tassa
2024-09-04 04:22:43 -07:00
committed by Copybara-Service
parent 42eb669e4e
commit 3437361bfb
14 changed files with 265 additions and 201 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_print.c
+49 -21
View File
@@ -979,6 +979,55 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
printArray("QLDIAGINV", m->nv, 1, d->qLDiagInv, fp, float_format);
printArray("QLDIAGSQRTINV", m->nv, 1, d->qLDiagSqrtInv, fp, float_format);
// B_rownnz
fprintf(fp, NAME_FORMAT, "B_rownnz");
for (int i = 0; i < m->nbody; i++) {
fprintf(fp, " %d", d->B_rownnz[i]);
}
fprintf(fp, "\n\n");
// B_rowadr
fprintf(fp, NAME_FORMAT, "B_rowadr");
for (int i = 0; i < m->nbody; i++) {
fprintf(fp, " %d", d->B_rowadr[i]);
}
fprintf(fp, "\n\n");
// B_colind
fprintf(fp, NAME_FORMAT, "B_colind");
for (int i = 0; i < m->nB; i++) {
fprintf(fp, " %d", d->B_colind[i]);
}
fprintf(fp, "\n\n");
// C_rownnz
fprintf(fp, NAME_FORMAT, "C_rownnz");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, " %d", d->C_rownnz[i]);
}
fprintf(fp, "\n\n");
// C_rowadr
fprintf(fp, NAME_FORMAT, "C_rowadr");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, " %d", d->C_rowadr[i]);
}
fprintf(fp, "\n\n");
// C_colind
fprintf(fp, NAME_FORMAT, "C_colind");
for (int i = 0; i < m->nC; i++) {
fprintf(fp, " %d", d->C_colind[i]);
}
fprintf(fp, "\n\n");
// mapM2C
fprintf(fp, NAME_FORMAT, "mapM2C");
for (int i = 0; i < m->nC; i++) {
fprintf(fp, " %d", d->mapM2C[i]);
}
fprintf(fp, "\n\n");
// D_rownnz
fprintf(fp, NAME_FORMAT, "D_rownnz");
for (int i = 0; i < m->nv; i++) {
@@ -1014,27 +1063,6 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
}
fprintf(fp, "\n\n");
// B_rownnz
fprintf(fp, NAME_FORMAT, "B_rownnz");
for (int i = 0; i < m->nbody; i++) {
fprintf(fp, " %d", d->B_rownnz[i]);
}
fprintf(fp, "\n\n");
// B_rowadr
fprintf(fp, NAME_FORMAT, "B_rowadr");
for (int i = 0; i < m->nbody; i++) {
fprintf(fp, " %d", d->B_rowadr[i]);
}
fprintf(fp, "\n\n");
// B_colind
fprintf(fp, NAME_FORMAT, "B_colind");
for (int i = 0; i < m->nB; i++) {
fprintf(fp, " %d", d->B_colind[i]);
}
fprintf(fp, "\n\n");
// print qDeriv
mju_sparse2dense(M, d->qDeriv, m->nv, m->nv, d->D_rownnz, d->D_rowadr, d->D_colind);
printArray("QDERIV", m->nv, m->nv, M, fp, float_format);