From 5fe2b887264ae0a9be4ebf29092c5b729b283ee9 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 11 Oct 2024 08:16:05 -0700 Subject: [PATCH] Print ASCII sparse matrix structures in `mj_printData`. Also: don't print sparse matrices if they have more than 300 rows. PiperOrigin-RevId: 684843230 Change-Id: If91d0b56cbbb6565904c3ee55857999e21b0cc62 --- src/engine/engine_print.c | 58 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 56 insertions(+), 2 deletions(-) diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index f0384f93..fb1cc962 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -92,7 +92,8 @@ static void printArrayInt(const char* str, int nr, int nc, const int* data, FILE static void printSparse(const char* str, const mjtNum* mat, int nr, const int* rownnz, const int* rowadr, const int* colind, FILE* fp, const char* float_format) { - if (!mat) { + // if no data, or too many rows to be visually useful, return + if (!mat || nr > 300) { return; } fprintf(fp, "%s\n", str); @@ -111,6 +112,38 @@ static void printSparse(const char* str, const mjtNum* mat, int nr, +// print sparse matrix structure +static void printSparsity(const char* str, int nr, int nc, + const int* rowadr, const int* rownnz, const int* colind, FILE* fp) { + // if no rows / columns, or too many columns to be visually useful, return + if (!nr || !nc || nc > 300) { + return; + } + fprintf(fp, "%s\n", str); + + for (int c=0; c < nc+2; c++) fprintf(fp, "-"); + fprintf(fp, "\n "); + + for (int r=0; r < nr; r++) { + int adr = rowadr[r]; + int nnz = 0; + for (int c=0; c < nc; c++) { + if (nnz < rownnz[r] && colind[adr + nnz] == c) { + fprintf(fp, "x"); + nnz++; + } else { + fprintf(fp, " "); + } + } + fprintf(fp, " |\n"); + if (r < nr-1) fprintf(fp, " "); + } + for (int c=0; c < nc+2; c++) fprintf(fp, "-"); + fprintf(fp, "\n\n"); +} + + + // print vector static void printVector(const char* str, const mjtNum* data, int n, FILE* fp, const char* float_format) { @@ -942,6 +975,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, if (!mj_isSparse(m)) { printArray("FLEXEDGE_J", m->nflexedge, m->nv, d->flexedge_J, fp, float_format); } else { + printSparsity("FLEXEDGE_J: flex edge connectivity", m->nflexedge, m->nv, + d->flexedge_J_rowadr, d->flexedge_J_rownnz, d->flexedge_J_colind, fp); printArrayInt("FLEXEDGE_J_ROWNNZ", m->nflexedge, 1, d->flexedge_J_rownnz, fp); printArrayInt("FLEXEDGE_J_ROWADR", m->nflexedge, 1, d->flexedge_J_rowadr, fp); printSparse("FLEXEDGE_J", d->flexedge_J, m->nflexedge, d->flexedge_J_rownnz, @@ -953,6 +988,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, if (!mj_isSparse(m)) { printArray("TEN_MOMENT", m->ntendon, m->nv, d->ten_J, fp, float_format); } else { + printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, + d->ten_J_rowadr, d->ten_J_rownnz, d->ten_J_colind, fp); printArrayInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp); printArrayInt("TEN_J_ROWADR", m->ntendon, 1, d->ten_J_rowadr, fp); printSparse("TEN_J", d->ten_J, m->ntendon, d->ten_J_rownnz, @@ -984,6 +1021,9 @@ 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 sparse structure + printSparsity("B: body-dof matrix", m->nbody, m->nv, d->B_rowadr, d->B_rownnz, d->B_colind, fp); + // B_rownnz fprintf(fp, NAME_FORMAT, "B_rownnz"); for (int i = 0; i < m->nbody; i++) { @@ -1005,7 +1045,9 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, } fprintf(fp, "\n\n"); - // C_rownnz + // C sparse structure + printSparsity("C: reduced dof-dof matrix", m->nv, m->nv, d->C_rowadr, d->C_rownnz, d->C_colind, fp); + fprintf(fp, NAME_FORMAT, "C_rownnz"); for (int i = 0; i < m->nv; i++) { fprintf(fp, " %d", d->C_rownnz[i]); @@ -1033,6 +1075,9 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, } fprintf(fp, "\n\n"); + // D sparse structure + printSparsity("D: dof-dof matrix", m->nv, m->nv, d->D_rowadr, d->D_rownnz, d->D_colind, fp); + // D_rownnz fprintf(fp, NAME_FORMAT, "D_rownnz"); for (int i = 0; i < m->nv; i++) { @@ -1128,11 +1173,20 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, printArray("EFC_J", d->nefc, m->nv, d->efc_J, fp, float_format); printArray("EFC_AR", d->nefc, d->nefc, d->efc_AR, fp, float_format); } else { + printSparsity("J: constraint Jacobian", d->nefc, m->nv, + d->efc_J_rowadr, d->efc_J_rownnz, d->efc_J_colind, fp); printArrayInt("EFC_J_ROWNNZ", d->nefc, 1, d->efc_J_rownnz, fp); printArrayInt("EFC_J_ROWADR", d->nefc, 1, d->efc_J_rowadr, fp); printSparse("EFC_J", d->efc_J, d->nefc, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, fp, float_format); + if (d->nnzL) { + // L sparse structure + printSparsity("L: Newton reverse Cholesky factor", m->nv, m->nv, + d->L_rowadr, d->L_rownnz, d->L_colind, fp); + printSparse("L", d->L, m->nv, d->L_rownnz, d->L_rowadr, d->L_colind, fp, float_format); + } + printArrayInt("EFC_AR_ROWNNZ", d->nefc, 1, d->efc_AR_rownnz, fp); printArrayInt("EFC_AR_ROWADR", d->nefc, 1, d->efc_AR_rowadr, fp); printSparse("EFC_AR", d->efc_AR, d->nefc, d->efc_AR_rownnz,