diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index c7cfb3be..8767aecf 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -1215,7 +1215,7 @@ Print model to text file. .. mujoco-include:: mj_printFormattedData Print mjData to text file, specifying format. -float_format must be a valid printf-style format string for a single float value +float_format must be a valid printf-style format string for a single float value. .. _mj_printData: @@ -1253,6 +1253,25 @@ Print sparse matrix to screen. Print internal XML schema as plain text or HTML, with style-padding or `` ``. +.. _mj_printScene: + +`mj_printScene <#mj_printScene>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_printScene + +Print scene to text file. + +.. _mj_printFormattedScene: + +`mj_printFormattedScene <#mj_printFormattedScene>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_printFormattedScene + +Print scene to text file, specifying format. +float_format must be a valid printf-style format string for a single float value. + .. _Virtualfilesystem: Virtual file system diff --git a/doc/includes/references.h b/doc/includes/references.h index a5e0144a..25cc0b4a 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3066,6 +3066,9 @@ void mju_printMatSparse(const mjtNum* mat, int nr, const int* rownnz, const int* rowadr, const int* colind); int mj_printSchema(const char* filename, char* buffer, int buffer_sz, int flg_html, int flg_pad); +void mj_printScene(const mjvScene* s, const char* filename); +void mj_printFormattedScene(const mjvScene* s, const char* filename, + const char* float_format); void mj_fwdPosition(const mjModel* m, mjData* d); void mj_fwdVelocity(const mjModel* m, mjData* d); void mj_fwdActuation(const mjModel* m, mjData* d); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 86a79dd5..1809c0b2 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -291,7 +291,7 @@ MJAPI void mj_printFormattedModel(const mjModel* m, const char* filename, const MJAPI void mj_printModel(const mjModel* m, const char* filename); // Print mjData to text file, specifying format. -// float_format must be a valid printf-style format string for a single float value +// float_format must be a valid printf-style format string for a single float value. MJAPI void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filename, const char* float_format); @@ -309,6 +309,14 @@ MJAPI void mju_printMatSparse(const mjtNum* mat, int nr, MJAPI int mj_printSchema(const char* filename, char* buffer, int buffer_sz, int flg_html, int flg_pad); +// Print scene to text file. +MJAPI void mj_printScene(const mjvScene* s, const char* filename); + +// Print scene to text file, specifying format. +// float_format must be a valid printf-style format string for a single float value. +MJAPI void mj_printFormattedScene(const mjvScene* s, const char* filename, + const char* float_format); + //---------------------------------- Components ---------------------------------------------------- diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 767ed879..2968cfbd 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -174,6 +174,8 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); Def(pymodule); Def(pymodule); + Def(pymodule); + Def(pymodule); DEF_WITH_OMITTED_PY_ARGS(traits::mju_printMat, "nr", "nc")( pymodule, [](Eigen::Ref mat) { diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 5aa97ca2..33ac3921 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -1274,7 +1274,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc='Print mjData to text file, specifying format. float_format must be a valid printf-style format string for a single float value', # pylint: disable=line-too-long + doc='Print mjData to text file, specifying format. float_format must be a valid printf-style format string for a single float value.', # pylint: disable=line-too-long )), ('mj_printData', FunctionDecl( @@ -1392,6 +1392,52 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Print internal XML schema as plain text or HTML, with style-padding or  .', # pylint: disable=line-too-long )), + ('mj_printScene', + FunctionDecl( + name='mj_printScene', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='s', + type=PointerType( + inner_type=ValueType(name='mjvScene', is_const=True), + ), + ), + FunctionParameterDecl( + name='filename', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + ), + doc='Print scene to text file.', + )), + ('mj_printFormattedScene', + FunctionDecl( + name='mj_printFormattedScene', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='s', + type=PointerType( + inner_type=ValueType(name='mjvScene', is_const=True), + ), + ), + FunctionParameterDecl( + name='filename', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + FunctionParameterDecl( + name='float_format', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + ), + doc='Print scene to text file, specifying format. float_format must be a valid printf-style format string for a single float value.', # pylint: disable=line-too-long + )), ('mj_fwdPosition', FunctionDecl( name='mj_fwdPosition', diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 046d554f..ae02c9f4 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -32,6 +32,7 @@ #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 @@ -47,8 +48,38 @@ //----------------------------------- 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 printArray(const char* str, int nr, int nc, const mjtNum* data, FILE* fp, +static void printArray2d(const char* str, int nr, int nc, const mjtNum* data, FILE* fp, const char* float_format) { if (!data) { return; @@ -69,7 +100,7 @@ static void printArray(const char* str, int nr, int nc, const mjtNum* data, FILE // print 2D array of int into file -static void printArrayInt(const char* str, int nr, int nc, const int* data, FILE* fp) { +static void printArray2dInt(const char* str, int nr, int nc, const int* data, FILE* fp) { if (!data) { return; } @@ -991,26 +1022,26 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char* // 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); - printArrayInt("B_ROWNNZ", 1, m->nbody, m->B_rownnz, fp); - printArrayInt("B_ROWADR", 1, m->nbody, m->B_rowadr, fp); - printArrayInt("B_COLIND", 1, m->nB, 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); - printArrayInt("M_ROWNNZ", 1, m->nv, m->M_rownnz, fp); - printArrayInt("M_ROWADR", 1, m->nv, m->M_rowadr, fp); - printArrayInt("M_COLIND", 1, m->nC, m->M_colind, fp); - printArrayInt("MAPM2M", 1, m->nC, m->mapM2M, 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); - printArrayInt("D_ROWNNZ", 1, m->nv, m->D_rownnz, fp); - printArrayInt("D_ROWADR", 1, m->nv, m->D_rowadr, fp); - printArrayInt("D_COLIND", 1, m->nD, m->D_colind, fp); - printArrayInt("MAPM2D", 1, m->nD, m->mapM2D, fp); - printArrayInt("MAPD2M", 1, m->nC, m->mapD2M, 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"); @@ -1185,13 +1216,13 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena fprintf(fp, float_format, d->time); fprintf(fp, "\n\n"); - printArray("QPOS", m->nq, 1, d->qpos, fp, float_format); - printArray("QVEL", m->nv, 1, d->qvel, fp, float_format); - printArray("ACT", m->na, 1, d->act, fp, float_format); - printArray("QACC_WARMSTART", m->nv, 1, d->qacc_warmstart, fp, float_format); - printArray("CTRL", m->nu, 1, d->ctrl, fp, float_format); - printArray("QFRC_APPLIED", m->nv, 1, d->qfrc_applied, fp, float_format); - printArray("XFRC_APPLIED", m->nbody, 6, d->xfrc_applied, fp, float_format); + 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++) { @@ -1199,56 +1230,56 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena } fprintf(fp, "\n\n"); } - printArray("MOCAP_POS", m->nmocap, 3, d->mocap_pos, fp, float_format); - printArray("MOCAP_QUAT", m->nmocap, 4, d->mocap_quat, fp, float_format); - printArray("QACC", m->nv, 1, d->qacc, fp, float_format); - printArray("ACT_DOT", m->na, 1, d->act_dot, fp, float_format); - printArray("USERDATA", m->nuserdata, 1, d->userdata, fp, float_format); - printArray("SENSOR", m->nsensordata, 1, d->sensordata, fp, float_format); + 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); - printArray("XPOS", m->nbody, 3, d->xpos, fp, float_format); - printArray("XQUAT", m->nbody, 4, d->xquat, fp, float_format); - printArray("XMAT", m->nbody, 9, d->xmat, fp, float_format); - printArray("XIPOS", m->nbody, 3, d->xipos, fp, float_format); - printArray("XIMAT", m->nbody, 9, d->ximat, fp, float_format); - printArray("XANCHOR", m->njnt, 3, d->xanchor, fp, float_format); - printArray("XAXIS", m->njnt, 3, d->xaxis, fp, float_format); - printArray("GEOM_XPOS", m->ngeom, 3, d->geom_xpos, fp, float_format); - printArray("GEOM_XMAT", m->ngeom, 9, d->geom_xmat, fp, float_format); - printArray("SITE_XPOS", m->nsite, 3, d->site_xpos, fp, float_format); - printArray("SITE_XMAT", m->nsite, 9, d->site_xmat, fp, float_format); - printArray("CAM_XPOS", m->ncam, 3, d->cam_xpos, fp, float_format); - printArray("CAM_XMAT", m->ncam, 9, d->cam_xmat, fp, float_format); - printArray("LIGHT_XPOS", m->nlight, 3, d->light_xpos, fp, float_format); - printArray("LIGHT_XDIR", m->nlight, 3, d->light_xdir, 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); - printArray("SUBTREE_COM", m->nbody, 3, d->subtree_com, fp, float_format); - printArray("CDOF", m->nv, 6, d->cdof, fp, float_format); - printArray("CINERT", m->nbody, 10, d->cinert, 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); - printArray("FLEXVERT_XPOS", m->nflexvert, 3, d->flexvert_xpos, fp, float_format); - printArray("FLEXELEM_AABB", m->nflexelem, 6, d->flexelem_aabb, 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)) { - printArray("FLEXEDGE_J", m->nflexedge, m->nv, d->flexedge_J, fp, float_format); + 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); - printArrayInt("FLEXEDGE_J_ROWNNZ", m->nflexedge, 1, d->flexedge_J_rownnz, fp); - printArrayInt("FLEXEDGE_J_ROWADR", m->nflexedge, 1, d->flexedge_J_rowadr, 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); } - printArray("FLEXEDGE_LENGTH", m->nflexedge, 1, d->flexedge_length, fp, float_format); + printArray2d("FLEXEDGE_LENGTH", m->nflexedge, 1, d->flexedge_length, fp, float_format); - printArray("TEN_LENGTH", m->ntendon, 1, d->ten_length, fp, float_format); + printArray2d("TEN_LENGTH", m->ntendon, 1, d->ten_length, fp, float_format); if (!mj_isSparse(m)) { - printArray("TEN_MOMENT", m->ntendon, m->nv, d->ten_J, fp, float_format); + 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); - printArrayInt("TEN_J_ROWNNZ", m->ntendon, 1, d->ten_J_rownnz, fp); - printArrayInt("TEN_J_ROWADR", m->ntendon, 1, d->ten_J_rowadr, 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); } @@ -1261,18 +1292,18 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena fprintf(fp, "\n"); } - printArray("ACTUATOR_LENGTH", m->nu, 1, d->actuator_length, fp, float_format); + 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); - printArray("CRB", m->nbody, 10, d->crb, 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); - printArray("QLDIAGINV", m->nv, 1, d->qLDiagInv, 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 @@ -1287,7 +1318,7 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena 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); - printArray("QHDIAGINV", m->nv, 1, d->qHDiagInv, fp, float_format); + printArray2d("QHDIAGINV", m->nv, 1, d->qHDiagInv, fp, float_format); } // print qDeriv @@ -1347,21 +1378,21 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena } if (d->ncon) fprintf(fp, "\n"); - printArrayInt("EFC_TYPE", d->nefc, 1, d->efc_type, fp); - printArrayInt("EFC_ID", d->nefc, 1, d->efc_id, fp); + printArray2dInt("EFC_TYPE", d->nefc, 1, d->efc_type, fp); + printArray2dInt("EFC_ID", d->nefc, 1, d->efc_id, fp); if (!mj_isSparse(m)) { - printArray("EFC_J", d->nefc, m->nv, d->efc_J, fp, float_format); + 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); } - printArray("EFC_AR", d->nefc, d->nefc, d->efc_AR, fp, float_format); + printArray2d("EFC_AR", d->nefc, d->nefc, d->efc_AR, fp, float_format); } else { - printArrayInt("EFC_J_ROWNNZ", d->nefc, 1, d->efc_J_rownnz, fp); - printArrayInt("EFC_J_ROWADR", d->nefc, 1, d->efc_J_rowadr, fp); + 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, @@ -1376,8 +1407,8 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena } if (mj_isDual(m)) { - printArrayInt("EFC_AR_ROWNNZ", d->nefc, 1, d->efc_AR_rownnz, fp); - printArrayInt("EFC_AR_ROWADR", d->nefc, 1, d->efc_AR_rowadr, fp); + 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, @@ -1385,51 +1416,51 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena } } - printArray("EFC_POS", d->nefc, 1, d->efc_pos, fp, float_format); - printArray("EFC_MARGIN", d->nefc, 1, d->efc_margin, fp, float_format); - printArray("EFC_FRICTIONLOSS", d->nefc, 1, d->efc_frictionloss, fp, float_format); - printArray("EFC_DIAGAPPROX", d->nefc, 1, d->efc_diagApprox, fp, float_format); - printArray("EFC_KBIP", d->nefc, 4, d->efc_KBIP, fp, float_format); - printArray("EFC_D", d->nefc, 1, d->efc_D, fp, float_format); - printArray("EFC_R", d->nefc, 1, d->efc_R, fp, float_format); + 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); - printArray("FLEXEDGE_VELOCITY", m->nflexedge, 1, d->flexedge_velocity, fp, float_format); - printArray("TEN_VELOCITY", m->ntendon, 1, d->ten_velocity, fp, float_format); - printArray("ACTUATOR_VELOCITY", m->nu, 1, d->actuator_velocity, 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); - printArray("CVEL", m->nbody, 6, d->cvel, fp, float_format); - printArray("CDOF_DOT", m->nv, 6, d->cdof_dot, 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); - printArray("QFRC_BIAS", m->nv, 1, d->qfrc_bias, fp, float_format); + printArray2d("QFRC_BIAS", m->nv, 1, d->qfrc_bias, fp, float_format); - printArray("QFRC_SPRING", m->nv, 1, d->qfrc_spring, fp, float_format); - printArray("QFRC_DAMPER", m->nv, 1, d->qfrc_damper, fp, float_format); - printArray("QFRC_GRAVCOMP", m->nv, 1, d->qfrc_gravcomp, fp, float_format); - printArray("QFRC_FLUID", m->nv, 1, d->qfrc_fluid, fp, float_format); - printArray("QFRC_PASSIVE", m->nv, 1, d->qfrc_passive, 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); - printArray("EFC_VEL", d->nefc, 1, d->efc_vel, fp, float_format); - printArray("EFC_AREF", d->nefc, 1, d->efc_aref, 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); - printArray("SUBTREE_LINVEL", m->nbody, 3, d->subtree_linvel, fp, float_format); - printArray("SUBTREE_ANGMOM", m->nbody, 3, d->subtree_angmom, 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); - printArray("ACTUATOR_FORCE", m->nu, 1, d->actuator_force, fp, float_format); - printArray("QFRC_ACTUATOR", m->nv, 1, d->qfrc_actuator, 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); - printArray("QFRC_SMOOTH", m->nv, 1, d->qfrc_smooth, fp, float_format); - printArray("QACC_SMOOTH", m->nv, 1, d->qacc_smooth, 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); - printArray("EFC_B", d->nefc, 1, d->efc_b, fp, float_format); - printArray("EFC_FORCE", d->nefc, 1, d->efc_force, fp, float_format); - printArrayInt("EFC_STATE", d->nefc, 1, d->efc_state, fp); - printArray("QFRC_CONSTRAINT", m->nv, 1, d->qfrc_constraint, 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); - printArray("QFRC_INVERSE", m->nv, 1, d->qfrc_inverse, fp, float_format); + printArray2d("QFRC_INVERSE", m->nv, 1, d->qfrc_inverse, fp, float_format); - printArray("CACC", m->nbody, 6, d->cacc, fp, float_format); - printArray("CFRC_INT", m->nbody, 6, d->cfrc_int, fp, float_format); - printArray("CFRC_EXT", m->nbody, 6, d->cfrc_ext, 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"); @@ -1528,3 +1559,155 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena 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); + } +} diff --git a/src/engine/engine_print.h b/src/engine/engine_print.h index 6c1563c6..27036240 100644 --- a/src/engine/engine_print.h +++ b/src/engine/engine_print.h @@ -20,6 +20,7 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -46,6 +47,14 @@ MJAPI void mj_printData(const mjModel* m, const mjData* d, const char* filename) MJAPI 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); +// print scene to text file +MJAPI void mj_printScene(const mjvScene* s, const char* filename); + +// print scene to text file, specifying format +// float_format must be a valid printf-style format string for a single float value +MJAPI void mj_printFormattedScene(const mjvScene* s, const char* filename, + const char* float_format); + #ifdef __cplusplus } #endif diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index a9b3b0bd..62837b05 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -6480,6 +6480,12 @@ public static unsafe extern void mju_printMatSparse(double* mat, int nr, int* ro [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern int mj_printSchema([MarshalAs(UnmanagedType.LPStr)]string filename, StringBuilder buffer, int buffer_sz, int flg_html, int flg_pad); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mj_printScene(mjvScene_* s, [MarshalAs(UnmanagedType.LPStr)]string filename); + +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mj_printFormattedScene(mjvScene_* s, [MarshalAs(UnmanagedType.LPStr)]string filename, [MarshalAs(UnmanagedType.LPStr)]string float_format); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_fwdPosition(mjModel_* m, mjData_* d);