Store index of diagonal element for C and D in mjData.

PiperOrigin-RevId: 704287869 Change-Id: I15d1faa0b5c3ff54721f692ec6329726456ec318
2024-12-09 08:00:29 -08:00
parent b8344c191f
commit 3a14ada308
10 changed files with 67 additions and 18 deletions
@@ -118,7 +118,7 @@ inline void PrintMatrix(const mjtNum* mat, int nrow, int ncol, int p = 5) {
  std::cerr << "\n";
  for (int r = 0; r < nrow; r++) {
    for (int c = 0; c < ncol; c++) {
-      std::cerr << std::fixed << std::setw(4 + p) << mat[c + r*ncol] << " ";
+      std::cerr << std::fixed << std::setw(3 + p) << mat[c + r*ncol] << " ";
    }
    std::cerr << "\n";
  }