chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity
Files
70d03adc71445bbcf17f3dbe98f804f47f532d55
Mujoco_WASM/src/engine/engine_print.c
T
Haroon Qureshi 6a7566872a Functions for printing the contents of mjvScene. 
PiperOrigin-RevId: 812699642
Change-Id: Iafff53c952c952624129ff01a8c1b0aa5d38eec5
2025-09-29 02:47:17 -07:00

1714 lines
57 KiB
C
Raw Blame History

// Copyright 2021 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "engine/engine_print.h"
#include <stdbool.h>
#include <stddef.h>
#include <stdio.h>
#include <string.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjsan.h> // IWYU pragma: keep
#include <mujoco/mjxmacro.h>
#include "engine/engine_core_constraint.h"
#include "engine/engine_core_util.h"
#include "engine/engine_io.h"
#include "engine/engine_name.h"
#include "engine/engine_macro.h"
#include "engine/engine_support.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_vis_init.h"
#ifdef MEMORY_SANITIZER
#include <sanitizer/msan_interface.h>
#endif
#define FLOAT_FORMAT "% -9.2g"
#define FLOAT_FORMAT_MAX_LEN 20
#define INT_FORMAT " %d"
#define SIZE_T_FORMAT " %zu"
#define NAME_FORMAT "%-21s"
//----------------------------------- static utility functions -------------------------------------
static void printInt(FILE* fp, const char* name, int value) {
fprintf(fp, NAME_FORMAT, name);
fprintf(fp, INT_FORMAT, value);
fprintf(fp, "\n");
}
static void printStr(FILE* fp, const char* name, const char* value) {
fprintf(fp, NAME_FORMAT, name);
fprintf(fp, "%s", value ? value : "");
fprintf(fp, "\n");
}
static void printNum(FILE* fp, const char* name, float value, const char* float_format) {
fprintf(fp, NAME_FORMAT, name);
fprintf(fp, float_format, value);
fprintf(fp, "\n");
}
static void printArr(FILE* fp, const char* name, const float* data, int n, const char* float_format) {
if (!data) {
return;
}
fprintf(fp, NAME_FORMAT, name);
for (int i = 0; i < n; ++i) {
fprintf(fp, float_format, data[i]);
fprintf(fp, " ");
}
fprintf(fp, "\n");
}
// print 2D array of mjtNum into file
static void printArray2d(const char* str, int nr, int nc, const mjtNum* data, FILE* fp,
const char* float_format) {
if (!data) {
return;
}
if (nr && nc) {
fprintf(fp, "%s\n", str);
for (int r=0; r < nr; r++) {
fprintf(fp, " ");
for (int c=0; c < nc; c++) {
fprintf(fp, " ");
fprintf(fp, float_format, data[c + r*nc]);
}
fprintf(fp, "\n");
}
fprintf(fp, "\n");
}
}
// print 2D array of int into file
static void printArray2dInt(const char* str, int nr, int nc, const int* data, FILE* fp) {
if (!data) {
return;
}
if (nr && nc) {
fprintf(fp, "%s\n", str);
for (int r=0; r < nr; r++) {
fprintf(fp, " ");
for (int c=0; c < nc; c++) {
fprintf(fp, " ");
fprintf(fp, "%d", data[c + r*nc]);
}
fprintf(fp, "\n");
}
fprintf(fp, "\n");
}
}
// print sparse matrix
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 no data, or too many rows to be visually useful, return
if (!mat || !nr || nr > 300) {
return;
}
fprintf(fp, "%s\n", str);
for (int r=0; r < nr; r++) {
fprintf(fp, " ");
for (int adr=rowadr[r]; adr < rowadr[r]+rownnz[r]; adr++) {
fprintf(fp, " ");
fprintf(fp, "%2d: ", colind[adr]);
fprintf(fp, float_format, mat[adr]);
}
fprintf(fp, "\n");
}
fprintf(fp, "\n");
}
// print block-diagonal dense matrix, embedded in a larger matrix
static void printBlockArray(const char* str, const mjtNum* data, int nr, int nc,
int nisland, const int* island_nr, const int* island_nc,
const int* island_r, const int* island_c,
FILE* fp, const char* float_format) {
if (!data || !nr || !nc) {
return;
}
fprintf(fp, "%s\n", str);
// determine the width of the float format (already validated by validateFloatFormat)
char dummy_buffer[100];
int format_width = snprintf(dummy_buffer, sizeof(dummy_buffer), float_format, 0.0);
for (int b = 0; b < nisland; b++) {
int bnr = island_nr[b];
int bnc = island_nc[b];
int r_start = island_r[b];
int c_start = island_c[b];
const mjtNum* data_ptr = data + r_start * nc;
// print rows for this block
for (int r_block = 0; r_block < bnr; r_block++) {
fprintf(fp, " ");
// leading dots
for (int c = 0; c < c_start; c++) {
for (int i = 0; i < format_width; i++) fprintf(fp, ".");
fprintf(fp, " ");
}
// block data
for (int c = 0; c < bnc; c++) {
fprintf(fp, " ");
fprintf(fp, float_format, *data_ptr++);
}
// trailing dots
for (int c = c_start + bnc; c < nc; c++) {
for (int i = 0; i < format_width; i++) fprintf(fp, ".");
fprintf(fp, " ");
}
fprintf(fp, "\n");
}
}
fprintf(fp, "\n");
}
// print sparse inertia-like matrix
static void printInertia(const char* str, const mjtNum* mat, const mjModel* m,
FILE* fp, const char* float_format) {
int nv = m->nv;
// if no data, or too many rows to be visually useful, return
if (!mat || !nv || nv > 300) {
return;
}
// get length of string produced by float_format
char test[100];
int len = snprintf(test, sizeof(test), float_format, 0.0);
fprintf(fp, "%s\n", str);
for (int i=0; i < nv; i++) {
fprintf(fp, " ");
int adr = (i == nv-1) ? m->nM - 1 : m->dof_Madr[i+1] - 1;
for (int k=0; k <= i; k++) {
int j = i;
while (j != k && j >= 0) {
j = m->dof_parentid[j];
}
if (j == k) {
fprintf(fp, " ");
fprintf(fp, float_format, mat[adr--]);
} else {
for (int d=0; d < len+1; d++) fprintf(fp, " ");
}
}
fprintf(fp, "\n");
}
fprintf(fp, "\n");
}
// print sparse matrix structure
void mj_printSparsity(const char* str, int nr, int nc, const int* rowadr, const int* diag,
const int* rownnz, const int* rowsuper, 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) {
if (diag && diag[r] == nnz) {
fprintf(fp, "D");
} else {
fprintf(fp, "x");
}
nnz++;
} else {
fprintf(fp, " ");
}
}
fprintf(fp, " |");
if (rowsuper && rowsuper[r] > 0) fprintf(fp, " %d", rowsuper[r]);
fprintf(fp, "\n");
if (r < nr-1) fprintf(fp, " ");
}
for (int c=0; c < nc+2; c++) fprintf(fp, "-");
fprintf(fp, "\n\n");
}
// print block-diagonal sparse matrix structure
void mj_printBlockSparsity(const char* str, int nr, int nc, int nisland,
const int* island_block_ncols,
const int* island_col_offset,
const int* entity_island,
const int* map_row_to_entity,
const int* rownnz, const int* rowadr, const int* colind,
const int* rowsuper, 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++) {
fprintf(fp, " ");
int entity_r = map_row_to_entity[r];
int island = entity_island[entity_r];
// SHOULD NOT OCCUR
if (island < 0 || island >= nisland) {
for (int c = 0; c < nc; c++) fprintf(fp, " ");
fprintf(fp, " | Error: invalid island %d for row %d (entity %d)\n", island, r, entity_r);
continue;
}
int c_start = island_col_offset[island];
int bnc = island_block_ncols[island];
int current_nnz = 0;
int adr = rowadr[r];
char nz_char = (island < 10) ? ('0' + island) : 'x';
for (int c = 0; c < nc; c++) { // c is the global column index
bool nonzero = false;
if (c >= c_start && c < c_start + bnc) {
int c_block = c - c_start; // c_block is the island-local column index
// search for c_block in colind for the current row r
while (current_nnz < rownnz[r] && colind[adr + current_nnz] < c_block) {
current_nnz++;
}
if (current_nnz < rownnz[r] && colind[adr + current_nnz] == c_block) {
nonzero = true;
}
}
fprintf(fp, "%c", nonzero ? nz_char : ' ');
}
fprintf(fp, " |");
if (rowsuper && rowsuper[r] > 0) fprintf(fp, " %d", rowsuper[r]);
fprintf(fp, "\n");
}
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) {
if (!data || !n) {
return;
}
// print str
fprintf(fp, "%s", str);
// print data
for (int i=0; i < n; i++) {
fprintf(fp, " ");
fprintf(fp, float_format, data[i]);
}
fprintf(fp, "\n");
}
// print human readable memory size
static const char* memorySize(size_t nbytes) {
static mjTHREADLOCAL char message[20];
int k = 1024;
if (nbytes < k) {
snprintf(message, sizeof(message), "%5zu bytes", nbytes);
} else if (nbytes < k*k) {
snprintf(message, sizeof(message), "%5.1f KB", (double)nbytes / k);
} else if (nbytes < k*k*k) {
snprintf(message, sizeof(message), "%5.1f MB", (double)nbytes / (k*k));
} else {
snprintf(message, sizeof(message), "%5.1f GB", (double)nbytes / (k*k*k));
}
return message;
}
// return memory footprint of all significant mesh-related arrays
static size_t sizeMesh(const mjModel* m) {
size_t nbytes = 0;
nbytes += sizeof(float) * 3*m->nmeshvert; // mesh_vert
nbytes += sizeof(float) * 3*m->nmeshnormal; // mesh_normal
nbytes += sizeof(float) * 2*m->nmeshtexcoord; // mesh_texcoord
nbytes += sizeof(int) * 3*m->nmeshface; // mesh_face
nbytes += sizeof(int) * 3*m->nmeshface; // mesh_facenormal
nbytes += sizeof(int) * 3*m->nmeshface; // mesh_facetexcoord
nbytes += sizeof(int) * m->nmeshgraph; // mesh_graph
return nbytes;
}
// return memory footprint of all significant skin-related arrays
static size_t sizeSkin(const mjModel* m) {
size_t nbytes = 0;
nbytes += sizeof(float) * 3*m->nskinvert; // skin_vert
nbytes += sizeof(float) * 2*m->nskintexvert; // skin_texcoord
nbytes += sizeof(int) * 3*m->nskinface; // skin_face
nbytes += sizeof(int) * m->nskinbone; // skin_bonevertadr
nbytes += sizeof(int) * m->nskinbone; // skin_bonevertnum
nbytes += sizeof(float) * 3*m->nskinbone; // skin_bonebindpos
nbytes += sizeof(float) * 4*m->nskinbone; // skin_bonebindquat
nbytes += sizeof(int) * m->nskinbone; // skin_bonebodyid
nbytes += sizeof(int) * m->nskinbonevert; // skin_bonevertid
nbytes += sizeof(float) * m->nskinbonevert; // skin_bonevertweight
return nbytes;
}
// return whether float_format is a valid format string for a single float
static bool validateFloatFormat(const char* float_format) {
// check for nullptr;
if (!float_format) {
return false;
}
// example valid format string: "% -9.2g"
if (strnlen(float_format, FLOAT_FORMAT_MAX_LEN + 1) > FLOAT_FORMAT_MAX_LEN) {
mju_warning("Format string longer than limit of %d.", FLOAT_FORMAT_MAX_LEN);
return false;
}
int cur_idx = 0;
if (float_format[cur_idx] != '%') {
mju_warning("Format string must start with '%%'.");
return false;
}
cur_idx++;
// flag characters. allow at most one of each flag
const char flag_characters[] = "-+ #0";
int flag_character_counts[sizeof(flag_characters)] = { 0 };
char* c;
while (c = strchr(flag_characters, float_format[cur_idx]), c != NULL) {
int flag_idx = (c - flag_characters)/sizeof(char);
flag_character_counts[flag_idx]++;
if (flag_character_counts[flag_idx] > 1) {
mju_warning("Format string contains repeated flag.");
return false;
}
cur_idx++;
}
// width. disallow *, which requires additional argument
while (strchr("0123456789", float_format[cur_idx]) != NULL) {
cur_idx++;
}
// precision. disallow *, which requires additional argument
if (float_format[cur_idx] == '.') {
cur_idx++;
while (strchr("0123456789", float_format[cur_idx]) != NULL) {
cur_idx++;
}
}
// length
if (float_format[cur_idx] == 'L') {
cur_idx++;
}
// specifier must be a valid float format
if (strchr("fgGeE", float_format[cur_idx]) == NULL) {
mju_warning("Format string specifier must be one of \"fgGeE\".");
return false;
}
cur_idx++;
if (float_format[cur_idx] == '\0') {
return true;
} else {
mju_warning("Unable to match format string %s with expected pattern for a single float.",
float_format);
return false;
}
}
// Clang sometimes goes OOM when the -Wuninitialized warning is enabled for this function
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wuninitialized"
#endif
//------------------------------ printing functions ------------------------------------------------
// print mjModel to text file, specifying format. float_format must be a
// valid printf-style format string for a single float value
void mj_printFormattedModel(const mjModel* m, const char* filename, const char* float_format) {
// get file
FILE* fp;
if (filename) {
fp = fopen(filename, "wt");
} else {
fp = stdout;
}
// check for nullptr
if (!fp) {
mju_warning("Could not open file '%s' for writing mjModel", filename);
return;
}
// validate format string
if (!validateFloatFormat(float_format)) {
mju_warning("WARNING: Received invalid float_format. Using default instead.");
float_format = FLOAT_FORMAT;
}
// compute total body mass
mjtNum totalmass = 0;
for (int i=0; i < m->nbody; i++) {
totalmass += m->body_mass[i];
}
// software version and model name
fprintf(fp, "MuJoCo version %s\n", mj_versionString());
fprintf(fp, "model name %s\n\n", m->names);
// memory footprint
fprintf(fp, "MEMORY\n");
fprintf(fp, " total %s\n", memorySize(mj_sizeModel(m)));
if (m->nmesh) {
fprintf(fp, " meshes %s\n", memorySize(sizeMesh(m)));
}
if (m->ntex) {
fprintf(fp, " textures %s\n", memorySize(m->ntexdata));
}
if (m->nskin) {
fprintf(fp, " skins %s\n", memorySize(sizeSkin(m)));
}
fprintf(fp, "\n");
// sizes
fprintf(fp, "SIZES\n");
#define X( name ) \
if (m->name) { \
const char* format = _Generic( \
m->name, \
size_t : SIZE_T_FORMAT, \
default : INT_FORMAT); \
fprintf(fp, NAME_FORMAT, " " #name); \
fprintf(fp, format, m->name); \
fprintf(fp, "\n"); \
}
MJMODEL_INTS
#undef X
fprintf(fp, "\n");
// scalar options
fprintf(fp, "OPTION\n");
#define X( type, name ) \
fprintf(fp, NAME_FORMAT, " " #name); \
fprintf(fp, float_format, m->opt.name); \
fprintf(fp, "\n");
MJOPTION_FLOATS
#undef X
#define X( type, name ) \
fprintf(fp, NAME_FORMAT, " " #name); \
fprintf(fp, INT_FORMAT "\n", m->opt.name);
MJOPTION_INTS
#undef X
// vector options
#define X( name, sz ) \
fprintf(fp, NAME_FORMAT, " " #name); \
for (int i=0; i < sz; i++) { \
fprintf(fp, float_format, m->opt.name[i]); \
fprintf(fp, " "); \
} \
fprintf(fp, "\n");
MJOPTION_VECTORS
#undef X
fprintf(fp, "\n");
// total mass
fprintf(fp, NAME_FORMAT, "totalmass");
fprintf(fp, float_format, totalmass);
fprintf(fp, "\n\n");
// statistics
fprintf(fp, "STATISTIC\n");
fprintf(fp, NAME_FORMAT, " meaninertia");
fprintf(fp, float_format, m->stat.meaninertia);
fprintf(fp, "\n");
fprintf(fp, NAME_FORMAT, " meanmass");
fprintf(fp, float_format, m->stat.meanmass);
fprintf(fp, "\n");
fprintf(fp, NAME_FORMAT, " meansize");
fprintf(fp, float_format, m->stat.meansize);
fprintf(fp, "\n");
fprintf(fp, NAME_FORMAT, " extent");
fprintf(fp, float_format, m->stat.extent);
fprintf(fp, "\n");
fprintf(fp, NAME_FORMAT, " center");
fprintf(fp, float_format, m->stat.center[0]);
fprintf(fp, float_format, m->stat.center[1]);
fprintf(fp, float_format, m->stat.center[2]);
fprintf(fp, "\n\n");
// qpos0
fprintf(fp, NAME_FORMAT, "qpos0");
for (int i=0; i < m->nq; i++) {
fprintf(fp, float_format, m->qpos0[i]);
fprintf(fp, " ");
}
fprintf(fp, "\n\n");
// qpos_spring
fprintf(fp, NAME_FORMAT, "qpos_spring");
for (int i=0; i < m->nq; i++) {
fprintf(fp, float_format, m->qpos_spring[i]);
fprintf(fp, " ");
}
fprintf(fp, "\n\n");
// values used by MJMODEL_POINTERS macro
MJMODEL_POINTERS_PREAMBLE(m)
// touch unused sizes to avoid unused variable warning
(void)nq;
(void)nv;
(void)na;
(void)nu;
(void)nmocap;
const int* object_class;
#define X(type, name, num, sz) \
if (&m->num == object_class && sz > 0) { \
const char* format = _Generic(*m->name, \
double: float_format, \
float: float_format, \
int: INT_FORMAT, \
mjtByte: INT_FORMAT, \
default: NULL); \
if (format) { \
fprintf(fp, " "); \
fprintf(fp, NAME_FORMAT, #name); \
for (int j = 0; j < sz; j++) { \
fprintf(fp, format, m->name[sz * i + j]); \
fprintf(fp, " "); \
} \
fprintf(fp, "\n"); \
} \
}
#define XNV X
// bodies
object_class = &m->nbody;
for (int i=0; i < m->nbody; i++) {
fprintf(fp, "\nBODY %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_bodyadr[i]);
MJMODEL_POINTERS_BODY
}
if (m->nbody) fprintf(fp, "\n");
// joints
object_class = &m->njnt;
for (int i=0; i < m->njnt; i++) {
fprintf(fp, "\nJOINT %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_jntadr[i]);
MJMODEL_POINTERS_JOINT
}
if (m->njnt) fprintf(fp, "\n");
// dofs
object_class = &m->nv;
for (int i=0; i < m->nv; i++) {
fprintf(fp, "\nDOF %d:\n", i);
MJMODEL_POINTERS_DOF
}
if (m->nv) fprintf(fp, "\n");
// geoms
object_class = &m->ngeom;
for (int i=0; i < m->ngeom; i++) {
fprintf(fp, "\nGEOM %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_geomadr[i]);
MJMODEL_POINTERS_GEOM
}
if (m->ngeom) fprintf(fp, "\n");
// sites
object_class = &m->nsite;
for (int i=0; i < m->nsite; i++) {
fprintf(fp, "\nSITE %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_siteadr[i]);
MJMODEL_POINTERS_SITE
}
if (m->nsite) fprintf(fp, "\n");
// cameras
object_class = &m->ncam;
for (int i=0; i < m->ncam; i++) {
fprintf(fp, "\nCAMERA %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_camadr[i]);
MJMODEL_POINTERS_CAMERA
}
if (m->ncam) fprintf(fp, "\n");
// lights
object_class = &m->nlight;
for (int i=0; i < m->nlight; i++) {
fprintf(fp, "\nLIGHT %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_lightadr[i]);
MJMODEL_POINTERS_LIGHT
}
if (m->nlight) fprintf(fp, "\n");
// flexes
object_class = &m->nflex;
for (int i=0; i < m->nflex; i++) {
fprintf(fp, "\nFLEX %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_flexadr[i]);
MJMODEL_POINTERS_FLEX
}
if (m->nflex) fprintf(fp, "\n");
// meshes
object_class = &m->nmesh;
for (int i=0; i < m->nmesh; i++) {
fprintf(fp, "\nMESH %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_meshadr[i]);
MJMODEL_POINTERS_MESH
if (m->mesh_graphadr[i] >= 0) {
fprintf(fp, " " NAME_FORMAT, "qhull face");
fprintf(fp, " %d\n", m->mesh_graph[m->mesh_graphadr[i]+1]);
fprintf(fp, " " NAME_FORMAT, "qhull vert");
fprintf(fp, " %d\n", m->mesh_graph[m->mesh_graphadr[i]]);
}
}
if (m->nmesh) fprintf(fp, "\n");
// skins
object_class = &m->nskin;
for (int i=0; i < m->nskin; i++) {
fprintf(fp, "\nSKIN %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_skinadr[i]);
MJMODEL_POINTERS_SKIN
}
if (m->nskin) fprintf(fp, "\n");
// hfields
object_class = &m->nhfield;
for (int i=0; i < m->nhfield; i++) {
fprintf(fp, "\nHEIGHTFIELD %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_hfieldadr[i]);
MJMODEL_POINTERS_HFIELD
}
if (m->nhfield) fprintf(fp, "\n");
// textures
object_class = &m->ntex;
for (int i=0; i < m->ntex; i++) {
fprintf(fp, "\nTEXTURE %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_texadr[i]);
MJMODEL_POINTERS_TEXTURE
}
if (m->ntex) fprintf(fp, "\n");
// materials
object_class = &m->nmat;
for (int i=0; i < m->nmat; i++) {
fprintf(fp, "\nMATERIAL %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_matadr[i]);
MJMODEL_POINTERS_MATERIAL
}
if (m->nmat) fprintf(fp, "\n");
// pairs
object_class = &m->npair;
for (int i=0; i < m->npair; i++) {
fprintf(fp, "\nPAIR %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_pairadr[i]);
MJMODEL_POINTERS_PAIR
}
if (m->npair) fprintf(fp, "\n");
// excludes
object_class = &m->nexclude;
for (int i=0; i < m->nexclude; i++) {
fprintf(fp, "\nEXCLUDE %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_excludeadr[i]);
MJMODEL_POINTERS_EXCLUDE
}
if (m->nexclude) fprintf(fp, "\n");
// equality constraints
object_class = &m->neq;
for (int i=0; i < m->neq; i++) {
fprintf(fp, "\nEQUALITY %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_eqadr[i]);
MJMODEL_POINTERS_EQUALITY
}
if (m->neq) fprintf(fp, "\n");
// tendons
object_class = &m->ntendon;
for (int i=0; i < m->ntendon; i++) {
fprintf(fp, "\nTENDON %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_tendonadr[i]);
MJMODEL_POINTERS_TENDON
fprintf(fp, " path\n");
fprintf(fp, " type objid prm\n");
for (int j=0; j < m->tendon_num[i]; j++) {
int k = m->tendon_adr[i]+j;
fprintf(fp, " %d %d ", m->wrap_type[k], m->wrap_objid[k]);
fprintf(fp, float_format, m->wrap_prm[k]);
fprintf(fp, "\n");
}
fprintf(fp, "\n");
}
if (m->ntendon) fprintf(fp, "\n");
// actuators
object_class = &m->nu;
for (int i=0; i < m->nu; i++) {
fprintf(fp, "\nACTUATOR %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_actuatoradr[i]);
MJMODEL_POINTERS_ACTUATOR
}
if (m->nu) fprintf(fp, "\n");
// sensors
object_class = &m->nsensor;
for (int i=0; i < m->nsensor; i++) {
fprintf(fp, "\nSENSOR %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_sensoradr[i]);
MJMODEL_POINTERS_SENSOR
}
if (m->nsensor) fprintf(fp, "\n");
// custom numeric parameters
for (int i=0; i < m->nnumeric; i++) {
fprintf(fp, "\nNUMERIC %d:\n", i);
fprintf(fp, " name %s\n", m->names + m->name_numericadr[i]);
fprintf(fp, " size %d\n", m->numeric_size[i]);
fprintf(fp, " value ");
for (int j=0; j < m->numeric_size[i]; j++) {
fprintf(fp, float_format, m->numeric_data[m->numeric_adr[i]+j]);
}
fprintf(fp, "\n");
}
if (m->nnumeric) fprintf(fp, "\n");
// custom text parameters
for (int i=0; i < m->ntext; i++) {
fprintf(fp, "\nTEXT %d:\n", i);
fprintf(fp, " name %s\n", m->names + m->name_textadr[i]);
fprintf(fp, " size %d\n", m->text_size[i]);
fprintf(fp, " value %s\n", m->text_data + m->text_adr[i]);
}
if (m->ntext) fprintf(fp, "\n");
// custom tuple parameters
for (int i=0; i < m->ntuple; i++) {
fprintf(fp, "\nTUPLE %d:\n", i);
fprintf(fp, " name %s\n", m->names + m->name_tupleadr[i]);
fprintf(fp, " size %d\n", m->tuple_size[i]);
fprintf(fp, " elements\n");
for (int j=m->tuple_adr[i]; j < m->tuple_adr[i]+m->tuple_size[i]; j++) {
fprintf(fp, " %s %d, prm = ",
mju_type2Str(m->tuple_objtype[j]), m->tuple_objid[j]);
fprintf(fp, float_format, m->tuple_objprm[j]);
fprintf(fp, "\n");
}
}
if (m->ntuple) fprintf(fp, "\n");
// keyframes (only if different from default)
for (int i=0; i < m->nkey; i++) {
// print name
if (m->names[m->name_keyadr[i]]) {
fprintf(fp, "key_name%d %s\n", i, m->names + m->name_keyadr[i]);
}
// print time if non-0
if (m->key_time[i] != 0) {
fprintf(fp, "key_time%d %.4f\n", i, m->key_time[i]);
}
// check qpos for difference
int k = 0;
for (int j=0; j < m->nq; j++)
if (m->qpos0[j] != m->key_qpos[i*m->nq + j]) {
k = 1;
}
// print if different
if (k == 1) {
fprintf(fp, "key_qpos%d ", i);
for (int j=0; j < m->nq; j++) {
fprintf(fp, float_format, m->key_qpos[i*m->nq + j]);
}
fprintf(fp, "\n");
}
// check qvel for nonzero
for (int j=0; j < m->nv; j++)
if (m->key_qvel[i*m->nv + j]) {
k = 2;
}
// print if nonzero
if (k == 2) {
fprintf(fp, "key_qvel%d ", i);
for (int j=0; j < m->nv; j++) {
fprintf(fp, float_format, m->key_qvel[i*m->nv + j]);
}
fprintf(fp, "\n");
}
// check act for nonzero
for (int j=0; j < m->na; j++)
if (m->key_act[i*m->na + j]) {
k = 3;
}
// print if nonzero
if (k == 3) {
fprintf(fp, "key_act%d ", i);
for (int j=0; j < m->na; j++) {
fprintf(fp, float_format, m->key_act[i*m->na + j]);
}
fprintf(fp, "\n");
}
// check mpos for difference
if (m->nmocap) {
for (int j=0; j < m->nbody; j++) {
if (m->body_mocapid[j] >= 0) {
int id = m->body_mocapid[j];
if (m->body_pos[3*j] != m->key_mpos[i*3*m->nmocap + 3*id] ||
m->body_pos[3*j+1] != m->key_mpos[i*3*m->nmocap + 3*id+1] ||
m->body_pos[3*j+2] != m->key_mpos[i*3*m->nmocap + 3*id+2]) {
k = 4;
break;
}
}
}
}
// print if nonzero
if (k == 4) {
fprintf(fp, "key_mpos%d ", i);
for (int j=0; j < 3*m->nmocap; j++) {
fprintf(fp, float_format, m->key_mpos[i*3*m->nmocap + j]);
}
fprintf(fp, "\n");
}
// check mquat for difference
if (m->nmocap) {
for (int j=0; j < m->nbody; j++) {
if (m->body_mocapid[j] >= 0) {
int id = m->body_mocapid[j];
if (m->body_quat[4*j] != m->key_mquat[i*4*m->nmocap + 4*id] ||
m->body_quat[4*j+1] != m->key_mquat[i*4*m->nmocap + 4*id+1] ||
m->body_quat[4*j+2] != m->key_mquat[i*4*m->nmocap + 4*id+2] ||
m->body_quat[4*j+3] != m->key_mquat[i*4*m->nmocap + 4*id+3]) {
k = 5;
break;
}
}
}
}
// print if nonzero
if (k == 5) {
fprintf(fp, "key_mquat%d ", i);
for (int j=0; j < 4*m->nmocap; j++) {
fprintf(fp, float_format, m->key_mquat[i*4*m->nmocap + j]);
}
fprintf(fp, "\n");
}
// check ctrl for nonzero
for (int j=0; j < m->nu; j++) {
if (m->key_ctrl[i*m->nu + j]) {
k = 6;
break;
}
}
// print if nonzero
if (k == 6) {
fprintf(fp, "key_ctrl%d ", i);
for (int j=0; j < m->nu; j++) {
fprintf(fp, float_format, m->key_ctrl[i*m->nu + j]);
}
fprintf(fp, "\n");
}
// new line if any data was written
if (k) {
fprintf(fp, "\n");
}
}
#undef X
// B sparse structure
mj_printSparsity("B: body-dof matrix", m->nbody, m->nv, m->B_rowadr, NULL, m->B_rownnz, NULL,
m->B_colind, fp);
printArray2dInt("B_ROWNNZ", 1, m->nbody, m->B_rownnz, fp);
printArray2dInt("B_ROWADR", 1, m->nbody, m->B_rowadr, fp);
printArray2dInt("B_COLIND", 1, m->nB, m->B_colind, fp);
// M sparse structure
mj_printSparsity("M: reduced inertia matrix", m->nv, m->nv, m->M_rowadr, NULL, m->M_rownnz,
NULL, m->M_colind, fp);
printArray2dInt("M_ROWNNZ", 1, m->nv, m->M_rownnz, fp);
printArray2dInt("M_ROWADR", 1, m->nv, m->M_rowadr, fp);
printArray2dInt("M_COLIND", 1, m->nC, m->M_colind, fp);
printArray2dInt("MAPM2M", 1, m->nC, m->mapM2M, fp);
// D sparse structure
mj_printSparsity("D: dof-dof matrix", m->nv, m->nv,
m->D_rowadr, m->D_diag, m->D_rownnz, NULL, m->D_colind, fp);
printArray2dInt("D_ROWNNZ", 1, m->nv, m->D_rownnz, fp);
printArray2dInt("D_ROWADR", 1, m->nv, m->D_rowadr, fp);
printArray2dInt("D_COLIND", 1, m->nD, m->D_colind, fp);
printArray2dInt("MAPM2D", 1, m->nD, m->mapM2D, fp);
printArray2dInt("MAPD2M", 1, m->nC, m->mapD2M, fp);
// signature
fprintf(fp, "\nSIGNATURE\n");
fprintf(fp, " %lu\n", (unsigned long) m->signature);
fprintf(fp, "\n");
// BVHs
fprintf(fp, "BVH:\n");
fprintf(fp, " %-8s%-8s%-8s%-10s%-s\n","id", "depth", "nodeid", "child[0]" ,"child[1]");
for (int i=0; i < m->nbvh; i++) {
fprintf(fp, " %-8d%-8d% -8d% -10d% -d\n",
i, m->bvh_depth[i], m->bvh_nodeid[i], m->bvh_child[2*i], m->bvh_child[2*i+1]);
}
fprintf(fp, "\n");
if (filename) {
fclose(fp);
}
}
// print mjModel to text file
void mj_printModel(const mjModel* m, const char* filename) {
mj_printFormattedModel(m, filename, FLOAT_FORMAT);
}
// print mjModel to text file, specifying format. float_format must be a
// valid printf-style format string for a single float value
void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filename,
const char* float_format) {
// stack in use, SHOULD NOT OCCUR
if (d->pstack) {
mjERROR("attempting to print mjData when stack is in use");
}
// check format string
if (!validateFloatFormat(float_format)) {
mju_warning("WARNING: Received invalid float_format. Using default instead.");
float_format = FLOAT_FORMAT;
}
// get file
FILE* fp;
if (filename) {
fp = fopen(filename, "wt");
} else {
fp = stdout;
}
// check for nullptr
if (!fp) {
mju_warning("Could not open file '%s' for writing mjModel", filename);
return;
}
#ifdef MEMORY_SANITIZER
// If memory sanitizer is active, d->buffer will be marked as poisoned, even
// though it's really initialized to 0. This catches unintentionally
// using uninitialized values, but in engine_print it's OK to output zeroes.
// save current poison status of buffer before marking unpoisoned
void* shadow = mju_malloc(d->nbuffer);
__msan_copy_shadow(shadow, d->buffer, d->nbuffer);
__msan_unpoison(d->buffer, d->nbuffer);
#endif
fprintf(fp, "MEMORY\n");
fprintf(fp, " total %s\n", memorySize(sizeof(mjData) + d->nbuffer + d->narena));
fprintf(fp, " struct %s\n", memorySize(sizeof(mjData)));
fprintf(fp, " buffer %s\n", memorySize(d->nbuffer));
double arena_percent = 100 * d->maxuse_arena/(double)(d->narena);
fprintf(fp, " arena %s, used %.1f%%\n\n", memorySize(d->narena), arena_percent);
// ---------------------------------- print mjData fields
fprintf(fp, "SIZES\n");
#define X(type, name) \
if (strcmp(#name, "pstack") != 0 && \
strcmp(#name, "pbase") != 0 && \
strcmp(#name, "parena") != 0 && \
strcmp(#name, "threadpool") != 0) { \
const char* format = _Generic( \
d->name, \
int : INT_FORMAT, \
size_t : SIZE_T_FORMAT, \
default : NULL); \
if (format) { \
fprintf(fp, " "); \
fprintf(fp, NAME_FORMAT, #name); \
fprintf(fp, format, d->name); \
fprintf(fp, "\n"); \
} \
}
MJDATA_SCALAR
#undef X
int threadpool = 0;
if (d->threadpool) {
threadpool = 1;
}
fprintf(fp, " ");
fprintf(fp, NAME_FORMAT, "threadpool");
fprintf(fp, INT_FORMAT, threadpool);
fprintf(fp, "\n");
fprintf(fp, "\n");
// WARNING
int active_warnings = 0;
for (int i=0; i < mjNWARNING; i++) {
active_warnings += d->warning[i].number;
}
if (active_warnings) {
fprintf(fp, "WARNING\n");
for (int i=0; i < mjNWARNING; i++)
if (d->warning[i].number)
fprintf(fp, " %d: lastinfo = %d number = %d\n",
i, d->warning[i].lastinfo, d->warning[i].number);
fprintf(fp, "\n");
}
// TIMER
mjtNum active_timers = 0;
for (int i=0; i < mjNTIMER; i++) {
active_timers += d->timer[i].duration;
}
if (active_timers) {
fprintf(fp, "TIMER\n");
for (int i=0; i < mjNTIMER; i++) {
fprintf(fp, " %d: duration = ", i);
fprintf(fp, float_format, d->timer[i].duration);
fprintf(fp, " number = %d\n", d->timer[i].number);
}
fprintf(fp, "\n");
}
// SOLVER STAT
if (d->nefc) {
fprintf(fp, "SOLVER STAT\n");
printVector(" solver_fwdinv = ", d->solver_fwdinv, 2, fp, float_format);
int nisland_stat = mjMAX(1, mjMIN(d->nisland, mjNISLAND));
for (int island=0; island < nisland_stat; island++) {
int niter_stat = mjMIN(mjNSOLVER, d->solver_niter[island]);
if (niter_stat) {
fprintf(fp, " ISLAND %d\n", island);
fprintf(fp, " solver_niter = %d\n", d->solver_niter[island]);
fprintf(fp, " solver_nnz = %d\n", d->solver_nnz[island]);
for (int i=0; i < niter_stat; i++) {
const mjSolverStat* stat = d->solver + island*mjNSOLVER + i;
fprintf(fp, " %d: improvement = ", i);
fprintf(fp, float_format, stat->improvement);
fprintf(fp, " gradient = ");
fprintf(fp, float_format, stat->gradient);
fprintf(fp, " lineslope = ");
fprintf(fp, float_format, stat->lineslope);
fprintf(fp, "\n");
fprintf(fp, " nactive = %d nchange = %d neval = %d nupdate = %d\n",
stat->nactive, stat->nchange,
stat->neval, stat->nupdate);
}
fprintf(fp, "\n");
}
}
}
printVector("ENERGY = ", d->energy, 2, fp, float_format);
fprintf(fp, "\n");
fprintf(fp, "TIME = ");
fprintf(fp, float_format, d->time);
fprintf(fp, "\n\n");
printArray2d("QPOS", m->nq, 1, d->qpos, fp, float_format);
printArray2d("QVEL", m->nv, 1, d->qvel, fp, float_format);
printArray2d("ACT", m->na, 1, d->act, fp, float_format);
printArray2d("QACC_WARMSTART", m->nv, 1, d->qacc_warmstart, fp, float_format);
printArray2d("CTRL", m->nu, 1, d->ctrl, fp, float_format);
printArray2d("QFRC_APPLIED", m->nv, 1, d->qfrc_applied, fp, float_format);
printArray2d("XFRC_APPLIED", m->nbody, 6, d->xfrc_applied, fp, float_format);
if (m->neq) {
fprintf(fp, NAME_FORMAT, "EQ_ACTIVE");
for (int c=0; c < m->neq; c++) {
fprintf(fp, " %d", d->eq_active[c]);
}
fprintf(fp, "\n\n");
}
printArray2d("MOCAP_POS", m->nmocap, 3, d->mocap_pos, fp, float_format);
printArray2d("MOCAP_QUAT", m->nmocap, 4, d->mocap_quat, fp, float_format);
printArray2d("QACC", m->nv, 1, d->qacc, fp, float_format);
printArray2d("ACT_DOT", m->na, 1, d->act_dot, fp, float_format);
printArray2d("USERDATA", m->nuserdata, 1, d->userdata, fp, float_format);
printArray2d("SENSOR", m->nsensordata, 1, d->sensordata, fp, float_format);
printArray2d("XPOS", m->nbody, 3, d->xpos, fp, float_format);
printArray2d("XQUAT", m->nbody, 4, d->xquat, fp, float_format);
printArray2d("XMAT", m->nbody, 9, d->xmat, fp, float_format);
printArray2d("XIPOS", m->nbody, 3, d->xipos, fp, float_format);
printArray2d("XIMAT", m->nbody, 9, d->ximat, fp, float_format);
printArray2d("XANCHOR", m->njnt, 3, d->xanchor, fp, float_format);
printArray2d("XAXIS", m->njnt, 3, d->xaxis, fp, float_format);
printArray2d("GEOM_XPOS", m->ngeom, 3, d->geom_xpos, fp, float_format);
printArray2d("GEOM_XMAT", m->ngeom, 9, d->geom_xmat, fp, float_format);
printArray2d("SITE_XPOS", m->nsite, 3, d->site_xpos, fp, float_format);
printArray2d("SITE_XMAT", m->nsite, 9, d->site_xmat, fp, float_format);
printArray2d("CAM_XPOS", m->ncam, 3, d->cam_xpos, fp, float_format);
printArray2d("CAM_XMAT", m->ncam, 9, d->cam_xmat, fp, float_format);
printArray2d("LIGHT_XPOS", m->nlight, 3, d->light_xpos, fp, float_format);
printArray2d("LIGHT_XDIR", m->nlight, 3, d->light_xdir, fp, float_format);
printArray2d("SUBTREE_COM", m->nbody, 3, d->subtree_com, fp, float_format);
printArray2d("CDOF", m->nv, 6, d->cdof, fp, float_format);
printArray2d("CINERT", m->nbody, 10, d->cinert, fp, float_format);
printArray2d("FLEXVERT_XPOS", m->nflexvert, 3, d->flexvert_xpos, fp, float_format);
printArray2d("FLEXELEM_AABB", m->nflexelem, 6, d->flexelem_aabb, fp, float_format);
if (!mj_isSparse(m)) {
printArray2d("FLEXEDGE_J", m->nflexedge, m->nv, d->flexedge_J, fp, float_format);
} else {
mj_printSparsity("FLEXEDGE_J: flex edge connectivity", m->nflexedge, m->nv,
d->flexedge_J_rowadr, NULL, d->flexedge_J_rownnz, NULL, d->flexedge_J_colind,
fp);
printArray2dInt("FLEXEDGE_J_ROWNNZ", m->nflexedge, 1, d->flexedge_J_rownnz, fp);
printArray2dInt("FLEXEDGE_J_ROWADR", m->nflexedge, 1, d->flexedge_J_rowadr, fp);
printSparse("FLEXEDGE_J", d->flexedge_J, m->nflexedge, d->flexedge_J_rownnz,
d->flexedge_J_rowadr, d->flexedge_J_colind, fp, float_format);
}
printArray2d("FLEXEDGE_LENGTH", m->nflexedge, 1, d->flexedge_length, fp, float_format);
printArray2d("TEN_LENGTH", m->ntendon, 1, d->ten_length, fp, float_format);
if (!mj_isSparse(m)) {
printArray2d("TEN_MOMENT", m->ntendon, m->nv, d->ten_J, fp, float_format);
} else {
mj_printSparsity("TEN_J: tendon moments", m->ntendon, m->nv, d->ten_J_rowadr, NULL,
d->ten_J_rownnz, NULL, d->ten_J_colind, fp);
printArray2dInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp);
printArray2dInt("TEN_J_ROWADR", m->ntendon, 1, d->ten_J_rowadr, fp);
printSparse("TEN_J", d->ten_J, m->ntendon, d->ten_J_rownnz,
d->ten_J_rowadr, d->ten_J_colind, fp, float_format);
}
for (int i=0; i < m->ntendon; i++) {
fprintf(fp, "TENDON %d: %d wrap points\n", i, d->ten_wrapnum[i]);
for (int j=0; j < d->ten_wrapnum[i]; j++) {
fprintf(fp, " %d: ", d->wrap_obj[d->ten_wrapadr[i]+j]);
printVector("", d->wrap_xpos+3*(d->ten_wrapadr[i]+j), 3, fp, float_format);
}
fprintf(fp, "\n");
}
printArray2d("ACTUATOR_LENGTH", m->nu, 1, d->actuator_length, fp, float_format);
mj_printSparsity("actuator_moment", m->nu, m->nv,
d->moment_rowadr, NULL, d->moment_rownnz, NULL, d->moment_colind, fp);
printSparse("ACTUATOR_MOMENT", d->actuator_moment, m->nu, d->moment_rownnz,
d->moment_rowadr, d->moment_colind, fp, float_format);
printArray2d("CRB", m->nbody, 10, d->crb, fp, float_format);
printInertia("QM", d->qM, m, fp, float_format);
printSparse("M", d->M, m->nv, m->M_rownnz,
m->M_rowadr, m->M_colind, fp, float_format);
printSparse("QLD", d->qLD, m->nv, m->M_rownnz,
m->M_rowadr, m->M_colind, fp, float_format);
printArray2d("QLDIAGINV", m->nv, 1, d->qLDiagInv, fp, float_format);
if (d->nisland) {
// the static full inertia structure is already printed in printModel, so we only repeat it here
// if islands are present, for comparison
mj_printSparsity("M: inertia structure", m->nv, m->nv, m->M_rowadr, NULL,
m->M_rownnz, NULL, m->M_colind, fp);
mj_printBlockSparsity("iM: block-diagonal inertia (nnzs are island ids)",
d->nidof, d->nidof, d->nisland,
d->island_nv, d->island_idofadr,
d->dof_island, d->map_idof2dof,
d->iM_rownnz, d->iM_rowadr, d->iM_colind, NULL, fp);
}
if (!mju_isZero(d->qHDiagInv, m->nv)) {
printSparse("QH", d->qH, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind, fp, float_format);
printArray2d("QHDIAGINV", m->nv, 1, d->qHDiagInv, fp, float_format);
}
// print qDeriv
if (!mju_isZero(d->qDeriv, m->nD)) {
printSparse("QDERIV", d->qDeriv, m->nv, m->D_rownnz, m->D_rowadr, m->D_colind,
fp, float_format);
}
// print qLU
if (!mju_isZero(d->qLU, m->nD)) {
printSparse("QLU", d->qLU, m->nv, m->D_rownnz, m->D_rowadr, m->D_colind, fp, float_format);
}
// contact
fprintf(fp, "CONTACT\n");
for (int i=0; i < d->ncon; i++) {
fprintf(fp, " %d:\n dim %d\n", i, d->contact[i].dim);
int g1 = d->contact[i].geom[0];
int g2 = d->contact[i].geom[1];
// special case for geom-geom contacts
if (g1 > -1 && g2 > -1) {
fprintf(fp, " geoms ");
const char* geom1 = mj_id2name(m, mjOBJ_GEOM, g1);
const char* geom2 = mj_id2name(m, mjOBJ_GEOM, g2);
if (geom1) {
fprintf(fp, "%s : ", geom1);
} else {
fprintf(fp, "%d : ", g1);
}
if (geom2) {
fprintf(fp, "%s\n", geom2);
} else {
fprintf(fp, "%d\n", g2);
}
} else {
fprintf(fp, " gfev %d %d %d %d : %d %d %d %d\n",
d->contact[i].geom[0], d->contact[i].flex[0],
d->contact[i].elem[0], d->contact[i].vert[0],
d->contact[i].geom[1], d->contact[i].flex[1],
d->contact[i].elem[1], d->contact[i].vert[1]);
}
fprintf(fp, " exclude %d\n efc_address %d\n",
d->contact[i].exclude, d->contact[i].efc_address);
printVector(" solref ", d->contact[i].solref, mjNREF, fp, float_format);
printVector(" solimp ", d->contact[i].solimp, mjNIMP, fp, float_format);
printVector(" dist ", &d->contact[i].dist, 1, fp, float_format);
printVector(" includemargin", &d->contact[i].includemargin, 1, fp, float_format);
printVector(" pos ", d->contact[i].pos, 3, fp, float_format);
printVector(" frame ", d->contact[i].frame, 9, fp, float_format);
printVector(" friction ", d->contact[i].friction, 5, fp, float_format);
printVector(" mu ", &d->contact[i].mu, 1, fp, float_format);
mjtNum force[6] = {0};
mj_contactForce(m, d, i, force);
printVector(" force ", force, 6, fp, float_format);
}
if (d->ncon) fprintf(fp, "\n");
printArray2dInt("EFC_TYPE", d->nefc, 1, d->efc_type, fp);
printArray2dInt("EFC_ID", d->nefc, 1, d->efc_id, fp);
if (!mj_isSparse(m)) {
printArray2d("EFC_J", d->nefc, m->nv, d->efc_J, fp, float_format);
if (d->nisland) {
printBlockArray("IEFC_J", d->iefc_J, d->nefc, d->nidof,
d->nisland, d->island_nefc, d->island_nv,
d->island_iefcadr, d->island_idofadr,
fp, float_format);
}
printArray2d("EFC_AR", d->nefc, d->nefc, d->efc_AR, fp, float_format);
} else {
printArray2dInt("EFC_J_ROWNNZ", d->nefc, 1, d->efc_J_rownnz, fp);
printArray2dInt("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);
mj_printSparsity("J: constraint Jacobian", d->nefc, m->nv, d->efc_J_rowadr, NULL,
d->efc_J_rownnz, d->efc_J_rowsuper, d->efc_J_colind, fp);
if (d->nisland) {
mj_printBlockSparsity("IEFC_J: block-diagonalized constraint Jacobian (nnzs are island ids)",
d->nefc, d->nidof, d->nisland,
d->island_nv, d->island_idofadr,
d->efc_island, d->map_iefc2efc,
d->iefc_J_rownnz, d->iefc_J_rowadr, d->iefc_J_colind,
d->iefc_J_rowsuper, fp);
}
if (mj_isDual(m)) {
printArray2dInt("EFC_AR_ROWNNZ", d->nefc, 1, d->efc_AR_rownnz, fp);
printArray2dInt("EFC_AR_ROWADR", d->nefc, 1, d->efc_AR_rowadr, fp);
printSparse("EFC_AR", d->efc_AR, d->nefc, d->efc_AR_rownnz,
d->efc_AR_rowadr, d->efc_AR_colind, fp, float_format);
mj_printSparsity("efc_AR: inverse constraint inertia", d->nefc, d->nefc, d->efc_AR_rowadr,
NULL, d->efc_AR_rownnz, NULL, d->efc_AR_colind, fp);
}
}
printArray2d("EFC_POS", d->nefc, 1, d->efc_pos, fp, float_format);
printArray2d("EFC_MARGIN", d->nefc, 1, d->efc_margin, fp, float_format);
printArray2d("EFC_FRICTIONLOSS", d->nefc, 1, d->efc_frictionloss, fp, float_format);
printArray2d("EFC_DIAGAPPROX", d->nefc, 1, d->efc_diagApprox, fp, float_format);
printArray2d("EFC_KBIP", d->nefc, 4, d->efc_KBIP, fp, float_format);
printArray2d("EFC_D", d->nefc, 1, d->efc_D, fp, float_format);
printArray2d("EFC_R", d->nefc, 1, d->efc_R, fp, float_format);
printArray2d("FLEXEDGE_VELOCITY", m->nflexedge, 1, d->flexedge_velocity, fp, float_format);
printArray2d("TEN_VELOCITY", m->ntendon, 1, d->ten_velocity, fp, float_format);
printArray2d("ACTUATOR_VELOCITY", m->nu, 1, d->actuator_velocity, fp, float_format);
printArray2d("CVEL", m->nbody, 6, d->cvel, fp, float_format);
printArray2d("CDOF_DOT", m->nv, 6, d->cdof_dot, fp, float_format);
printArray2d("QFRC_BIAS", m->nv, 1, d->qfrc_bias, fp, float_format);
printArray2d("QFRC_SPRING", m->nv, 1, d->qfrc_spring, fp, float_format);
printArray2d("QFRC_DAMPER", m->nv, 1, d->qfrc_damper, fp, float_format);
printArray2d("QFRC_GRAVCOMP", m->nv, 1, d->qfrc_gravcomp, fp, float_format);
printArray2d("QFRC_FLUID", m->nv, 1, d->qfrc_fluid, fp, float_format);
printArray2d("QFRC_PASSIVE", m->nv, 1, d->qfrc_passive, fp, float_format);
printArray2d("EFC_VEL", d->nefc, 1, d->efc_vel, fp, float_format);
printArray2d("EFC_AREF", d->nefc, 1, d->efc_aref, fp, float_format);
printArray2d("SUBTREE_LINVEL", m->nbody, 3, d->subtree_linvel, fp, float_format);
printArray2d("SUBTREE_ANGMOM", m->nbody, 3, d->subtree_angmom, fp, float_format);
printArray2d("ACTUATOR_FORCE", m->nu, 1, d->actuator_force, fp, float_format);
printArray2d("QFRC_ACTUATOR", m->nv, 1, d->qfrc_actuator, fp, float_format);
printArray2d("QFRC_SMOOTH", m->nv, 1, d->qfrc_smooth, fp, float_format);
printArray2d("QACC_SMOOTH", m->nv, 1, d->qacc_smooth, fp, float_format);
printArray2d("EFC_B", d->nefc, 1, d->efc_b, fp, float_format);
printArray2d("EFC_FORCE", d->nefc, 1, d->efc_force, fp, float_format);
printArray2dInt("EFC_STATE", d->nefc, 1, d->efc_state, fp);
printArray2d("QFRC_CONSTRAINT", m->nv, 1, d->qfrc_constraint, fp, float_format);
printArray2d("QFRC_INVERSE", m->nv, 1, d->qfrc_inverse, fp, float_format);
printArray2d("CACC", m->nbody, 6, d->cacc, fp, float_format);
printArray2d("CFRC_INT", m->nbody, 6, d->cfrc_int, fp, float_format);
printArray2d("CFRC_EXT", m->nbody, 6, d->cfrc_ext, fp, float_format);
if (d->nisland) {
fprintf(fp, NAME_FORMAT, "DOF_ISLAND");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, " %d", d->dof_island[i]);
}
fprintf(fp, "\n\n");
fprintf(fp, NAME_FORMAT, "ISLAND_NV");
for (int i = 0; i < d->nisland; i++) {
fprintf(fp, " %d", d->island_nv[i]);
}
fprintf(fp, "\n\n");
fprintf(fp, NAME_FORMAT, "ISLAND_IDOFADR");
for (int i = 0; i < d->nisland; i++) {
fprintf(fp, " %d", d->island_idofadr[i]);
}
fprintf(fp, "\n\n");
fprintf(fp, NAME_FORMAT, "MAP_IDOF2DOF");
for (int i = 0; i < m->nv; i++) {
int dof = d->map_idof2dof[i];
if (i > 0) {
int dofprev = d->map_idof2dof[i-1];
// print '|' at island boundaries
if (d->dof_island[dof] != d->dof_island[dofprev]) {
fprintf(fp, " |");
}
}
fprintf(fp, " %d", dof);
}
fprintf(fp, "\n\n");
fprintf(fp, NAME_FORMAT, "EFC_ISLAND");
for (int i = 0; i < d->nefc; i++) {
fprintf(fp, " %d", d->efc_island[i]);
}
fprintf(fp, "\n\n");
fprintf(fp, NAME_FORMAT, "ISLAND_NEFC");
for (int i = 0; i < d->nisland; i++) {
fprintf(fp, " %d", d->island_nefc[i]);
}
fprintf(fp, "\n\n");
fprintf(fp, NAME_FORMAT, "ISLAND_IEFCADR");
for (int i = 0; i < d->nisland; i++) {
fprintf(fp, " %d", d->island_iefcadr[i]);
}
fprintf(fp, "\n\n");
fprintf(fp, NAME_FORMAT, "MAP_IEFC2EFC");
for (int i = 0; i < d->nefc; i++) {
int efc = d->map_iefc2efc[i];
if (i > 0) {
int efcprev = d->map_iefc2efc[i-1];
// print '|' at island boundaries
if (d->efc_island[efc] != d->efc_island[efcprev]) {
fprintf(fp, " |");
}
}
fprintf(fp, " %d", efc);
}
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
// restore poisoned status
__msan_copy_shadow(d->buffer, shadow, d->nbuffer);
mju_free(shadow);
#endif
if (filename) {
fclose(fp);
}
}
#ifdef __clang__
#pragma clang diagnostic pop
#endif
// print mjData to text file
void mj_printData(const mjModel* m, const mjData* d, const char* filename) {
mj_printFormattedData(m, d, filename, FLOAT_FORMAT);
}
void mj_printScene(const mjvScene* s, const char* filename) {
mj_printFormattedScene(s, filename, FLOAT_FORMAT);
}
void mj_printFormattedScene(const mjvScene* s, const char* filename, const char* float_format) {
// get file
FILE* fp;
if (filename) {
fp = fopen(filename, "wt");
} else {
fp = stdout;
}
// check for nullptr
if (!fp) {
mju_warning("Could not open file '%s' for writing mjModel", filename);
return;
}
// validate format string
if (!validateFloatFormat(float_format)) {
mju_warning("WARNING: Received invalid float_format. Using default instead.");
float_format = FLOAT_FORMAT;
}
fprintf(fp, "GEOMS %d\n", s->ngeom);
for (int i = 0; i < s->ngeom; ++i) {
const mjvGeom* geom = &s->geoms[i];
fprintf(fp, " GEOM %d\n", i);
printInt(fp, " type", geom->type);
printInt(fp, " category", geom->category);
printStr(fp, " label", geom->label);
printInt(fp, " objtype", geom->objtype);
printInt(fp, " objid", geom->objid);
printArr(fp, " pos", geom->pos, 3, float_format);
printArr(fp, " mat", geom->mat, 9, float_format);
printArr(fp, " size", geom->size, 3, float_format);
printInt(fp, " segid", geom->segid);
printInt(fp, " dataid", geom->dataid);
printInt(fp, " matid", geom->matid);
printInt(fp, " texcoord", geom->texcoord);
printArr(fp, " rgba", geom->rgba, 4, float_format);
printNum(fp, " emission", geom->emission, float_format);
printNum(fp, " specular", geom->specular, float_format);
printNum(fp, " shininess", geom->shininess, float_format);
printNum(fp, " reflectance", geom->reflectance, float_format);
fprintf(fp, "\n");
}
fprintf(fp, "\n");
fprintf(fp, "LIGHTS %d\n", s->nlight);
for (int i = 0; i < s->nlight; ++i) {
const mjvLight* light = &s->lights[i];
fprintf(fp, " LIGHT %d\n", i);
printInt(fp, " id", light->id);
printArr(fp, " pos", light->pos, 3, float_format);
printArr(fp, " dir", light->dir, 3, float_format);
printInt(fp, " type", light->type);
printInt(fp, " castshadow", light->castshadow);
printInt(fp, " headlight", light->headlight);
printNum(fp, " intensity", light->intensity, float_format);
printNum(fp, " range", light->range, float_format);
printArr(fp, " ambient", light->ambient, 3, float_format);
printArr(fp, " diffuse", light->diffuse, 3, float_format);
printArr(fp, " specular", light->specular, 3, float_format);
printInt(fp, " texid", light->texid);
printNum(fp, " exponent", light->exponent, float_format);
printArr(fp, " attenuation", light->attenuation, 3, float_format);
printNum(fp, " cutoff", light->cutoff, float_format);
printNum(fp, " bulbradius", light->bulbradius, float_format);
fprintf(fp, "\n");
}
fprintf(fp, "\n");
fprintf(fp, "CAMERAS %d\n", 2);
for (int i = 0; i < 2; ++i) {
const mjvGLCamera* camera = &s->camera[i];
fprintf(fp, " CAMERA %d\n", i);
printArr(fp, " pos", camera->pos, 3, float_format);
printArr(fp, " forward", camera->forward, 3, float_format);
printArr(fp, " up", camera->up, 3, float_format);
printInt(fp, " orthographic", camera->orthographic);
printNum(fp, " frustum_center", camera->frustum_center, float_format);
printNum(fp, " frustum_width", camera->frustum_width, float_format);
printNum(fp, " frustum_bottom", camera->frustum_bottom, float_format);
printNum(fp, " frustum_top", camera->frustum_top, float_format);
printNum(fp, " frustum_near", camera->frustum_near, float_format);
printNum(fp, " frustum_far", camera->frustum_far, float_format);
fprintf(fp, "\n");
}
fprintf(fp, "\n");
fprintf(fp, "FLEX DATA %d\n", s->nflex);
for (int i = 0; i < s->nflex; ++i) {
fprintf(fp, " FLEX DATA %d\n", i);
printInt(fp, " face_used", s->flexfaceused[i]);
printInt(fp, " edge_adr", s->flexedgeadr[i]);
printInt(fp, " edge_num", s->flexedgenum[i]);
printInt(fp, " vert_adr", s->flexvertadr[i]);
printInt(fp, " vert_num", s->flexvertnum[i]);
printInt(fp, " face_adr", s->flexfaceadr[i]);
printInt(fp, " face_num", s->flexfacenum[i]);
printStr(fp, " edges", "..."); // int* flexedge: 2*nflexedge
printStr(fp, " verts", "..."); // float* flexvert: 3*nflexvert
printStr(fp, " faces", "..."); // float* flexface: 9*sum(flexfacenum)
printStr(fp, " normals", "..."); // float* flexnormal: 9*sum(flexfacenum)
printStr(fp, " texcoords", "..."); // float* flextexcoord: 6*sum(flexfacenum)
fprintf(fp, "\n");
}
fprintf(fp, "\n");
fprintf(fp, "SKIN DATA %d\n", s->nskin);
for (int i = 0; i < s->nskin; ++i) {
fprintf(fp, " SKIN DATA %d\n", i);
printInt(fp, " face_num", s->skinfacenum[i]);
printInt(fp, " vert_adr", s->skinvertadr[i]);
printInt(fp, " vert_num", s->skinvertnum[i]);
printStr(fp, " verts", "..."); // float* skinvert: 3*nskinvert
printStr(fp, " normals", "..."); // float* skinnormal: 3*nskinvert
fprintf(fp, "\n");
}
fprintf(fp, "\n");
fprintf(fp, "FLAGS\n");
for (int i = 0; i < mjNRNDFLAG; ++i) {
fprintf(fp, " ");
fprintf(fp, NAME_FORMAT, mjRNDSTRING[i][0]);
fprintf(fp, INT_FORMAT, s->flags[i]);
fprintf(fp, "\n");
}
printInt(fp, " flexvertopt", s->flexvertopt);
printInt(fp, " flexedgeopt", s->flexedgeopt);
printInt(fp, " flexfaceopt", s->flexfaceopt);
printInt(fp, " flexskinopt", s->flexskinopt);
printInt(fp, " stereo", s->stereo);
fprintf(fp, "\n\n");
fprintf(fp, "TRANSFORM %d\n", s->enabletransform);
if (s->enabletransform) {
printArr(fp, " translate", s->translate, 3, float_format);
printArr(fp, " rotate", s->rotate, 4, float_format);
printNum(fp, " scale", s->scale, float_format);
fprintf(fp, "\n");
}
fprintf(fp, "\n");
fflush(fp);
if (filename) {
fclose(fp);
}
}
Reference in New Issue View Git Blame Copy Permalink