From 58fd72f53dde9eb876aad36ac641a331b9aecc30 Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Thu, 6 Oct 2022 10:02:35 -0700 Subject: [PATCH] Dynamically allocate `contact` and `efc_` arrays on a new memory arena. - Add private function `mj_arenaAlloc`. This is used internally to allocate memory from the arena. - Add private function `mj_nefc` to count constraints. This function returns a tight upper bound on `d->nefc`. The number of counted constraints can be slightly bigger than exact `d->nefc` in the case of constraints with empty Jacobian, as when placing a frictional tendon between two world sites. - Add new `memory` attribute to the `size` XML element for specification of arena memory size. This attribute is mutually exclusive with `nstack` and `njmax` specifications, which are now deprecated (but left around for the time being for legacy compatibility). - Move `d->stack` to the end of the new arena space. The stack now grows in reverse from the end. PiperOrigin-RevId: 479341539 Change-Id: Ie019c202e0908577ffc6f833a37920858116f667 --- doc/APIreference.rst | 2 + doc/XMLreference.rst | 44 ++--- doc/changelog.rst | 17 +- doc/modeling.rst | 77 +++++--- doc/programming.rst | 33 ++-- include/mujoco/mjdata.h | 87 +++++---- include/mujoco/mjxmacro.h | 81 +++++--- python/mujoco/bindings_test.py | 28 ++- python/mujoco/mjdata_meta.h | 8 +- python/mujoco/serialization.h | 17 +- python/mujoco/structs.cc | 186 +++++++++++++----- python/mujoco/structs.h | 80 +++++--- simulate/simulate.cc | 9 +- src/engine/engine_collision_driver.c | 3 +- src/engine/engine_core_constraint.c | 239 +++++++++++++++++++++-- src/engine/engine_io.c | 122 +++++++----- src/engine/engine_io.h | 3 + src/engine/engine_macro.h | 33 ++++ src/engine/engine_print.c | 19 +- src/engine/engine_util_misc.c | 9 +- src/user/user_model.cc | 48 +++-- src/user/user_model.h | 3 +- src/xml/xml_native_reader.cc | 133 ++++++++++++- src/xml/xml_native_writer.cc | 28 +++ test/user/user_model_test.cc | 18 +- test/user/user_objects_test.cc | 18 +- test/xml/xml_native_reader_test.cc | 175 +++++++++++++---- test/xml/xml_native_writer_test.cc | 132 +++++++++++-- unity/Runtime/Bindings/MujocoBindings.cs | 62 +++--- 29 files changed, 1281 insertions(+), 433 deletions(-) diff --git a/doc/APIreference.rst b/doc/APIreference.rst index 4371055b..9cad1c67 100644 --- a/doc/APIreference.rst +++ b/doc/APIreference.rst @@ -4188,6 +4188,8 @@ mj_rnePostConstraint RNE with complete data: compute cacc, cfrc_ext, cfrc_int. +.. _mj_collision: + mj_collision ~~~~~~~~~~~~ diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index a2bd8ec5..611d23f6 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -479,7 +479,7 @@ from its default. This flag enables the simulation of sensor noise. When disabled (which is the default) noise is not added to sensordata, even if the sensors specify non-zero noise amplitudes. When enabled, zero-mean Gaussian noise is added to the underlying deterministic sensor data. Its standard deviation is determined by the noise parameter of each sensor. -:at:`multiccd`: :at-val:`[disable, enable], "disable"` **(experimental feature)** +:at:`multiccd`: :at-val:`[disable, enable], "disable"` |nbsp| |nbsp| |nbsp| (experimental feature) This flag enables multiple-contact collision detection for geom pairs that use the general-purpose convex-convex collider based on :ref:`libccd ` e.g., mesh-mesh collisions. This can be useful when the contacting geoms have a flat surface, and the single contact point generated by the convex-convex collider cannot accurately capture @@ -497,29 +497,25 @@ This element specifies size parameters that cannot be inferred from the number o fields of mjOption which can be modified at runtime, sizes are structural parameters and should not be modified after compilation. -:at:`njmax`: :at-val:`int, "-1"` - This and the next two attributes specify the maximum sizes of the dynamic arrays in mjData, i.e., arrays whose - effective length varies at runtime. This attribute specifies the maximum number of scalar constraints (or - equivalently, rows of the constraint Jacobian) that can be handled at runtime. If the number of active constraints is - about to exceed this maximum (usually because too many contacts become active) the extra constraints are discarded - and a warning is generated. The number of active constraints is stored in mjData.nefc. The default setting of -1 - instructs the compiler to guess how much space to allocate (using heuristics that can be improved). This default is - effectively an undefined state. If the user specifies a positive value, the compiler heuristics are disabled and the - specified value is used. Modern computers have sufficient memory to handle very large models (larger than one would - normally have the patience to simulate) so tuning this setting aggressively is not necessary. When size-related - warnings or errors are generated, simply increase the value of the corresponding attribute. -:at:`nconmax`: :at-val:`int, "-1"` - This attribute specifies the maximum number of contacts (both frictional and frictionless) that can be handled at - runtime. If the number of active contacts is about to exceed this value, the extra contacts are discarded and a - warning is generated. The actual number of contacts is stored in mjData.ncon. If this value is negative, the compiler - will use a heuristic to guess an appropriate number. -:at:`nstack`: :at-val:`int, "-1"` - This attribute specifies the size of the preallocated stack in mjData, in units of sizeof(mjtNum) which is currently - defined as double; thus the size in bytes is 8 times larger. The custom stack is used by all MuJoCo functions that - need dynamically allocated memory. We do not use heap memory allocation at runtime, so as to speed up processing as - well as avoid heap fragmentation. Note that the internal allocator keeps track of how much stack space has ever been - utilized, in the field mjData.maxstackuse of mjData. If the stack size is exceeded at runtime, MuJoCo will generate - an error. If this value is negative, the compiler will use a heuristic to guess an appropriate number. +:at:`memory`: :at-val:`string, "-1"` + This attribute specifies the size of memory allocated for dynamic arrays in the ``mjData.arena`` memory space, in + bytes. The default setting of ``-1`` instructs the compiler to guess how much space to allocate. Appending the digits + with one of the letters {K, M, G, T, P, E} sets the unit to be {kilo, mega, giga, tera, peta, exa}-byte, + respectively. Thus "16M" means "allocate 16 megabytes of ``arena`` memory". + See the :ref:`Memory allocation ` section for details. +:at:`njmax`: :at-val:`int, "-1"` |nbsp| |nbsp| |nbsp| (legacy) + This is a deprecated legacy attribute. In versions prior to 2.3.0, it determined the maximum allowed number + of constraints. Currently it means "allocate as much memory as would have previously been required for this number of + constraints". Specifying both :at:`njmax` and :at:`memory` leads to an error. +:at:`nconmax`: :at-val:`int, "-1"` |nbsp| |nbsp| |nbsp| (legacy) + This attribute specifies the maximum number of contacts that will be generated at runtime. If the number of active + contacts is about to exceed this value, the extra contacts are discarded and a warning is generated. This is a + deprecated legacy attribute which prior to version 2.3.0 affected memory allocation. It is kept for backwards + compatibillity and debugging purposes. +:at:`nstack`: :at-val:`int, "-1"` |nbsp| |nbsp| |nbsp| (legacy) + This is a deprecated legacy attribute. In versions prior to 2.3.0, it determined the maximum size of the + :ref:`stack `. Currently it is synonymous with the :at:`memory` attribute above, but is in units of + ``sizeof(mjtNum)`` rather than bytes. Specifying both :at:`nstack` and :at:`memory` leads to an error. :at:`nuserdata`: :at-val:`int, "0"` The size of the field mjData.userdata of mjData. This field should be used to store custom dynamic variables. See also :ref:`CUser`. diff --git a/doc/changelog.rst b/doc/changelog.rst index efb6102e..d28b4985 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -9,9 +9,17 @@ Upcoming version (not yet released) General ^^^^^^^ -.. youtube:: RHnXD6uO3Mg - :align: right - :height: 150px +- The ``contact`` array and arrays prefixed with ``efc_`` in ``mjData`` were moved out of the ``buffer`` into a new + ``arena`` memory space. These arrays are no longer allocated with fixed sizes when ``mjData`` is created. + Instead, the exact memory requirement is determined during each call to :ref:`mj_forward` (specifically, + in :ref:`mj_collision` and :ref:`mj_makeConstraint`) and the arrays are allocated from the ``arena`` space. The + ``stack`` now also shares its available memory with ``arena``. This change reduces the memory footprint of ``mjData`` + in models that do not use the PGS solver, and will allow for significant memory reductions in the future. + See the :ref:`Memory allocation ` section for details. + + .. youtube:: RHnXD6uO3Mg + :align: right + :height: 150px - Added colab notebook tutorial showing how to balance the humanoid on one leg with a Linear Quadratic Regulator. The notebook uses MuJoCo's native Python bindings, and includes a draft ``Renderer`` class, for easy rendering in Python. @@ -59,6 +67,9 @@ Python bindings `named accessor `_ objects. These provide more Pythonic API access to ``mj_name2id`` and ``mj_id2name`` respectively. +- The length of ``MjData.contact`` is now ``ncon`` rather than ``nconmax``, allowing it to be straightforwardly used as + an iterator without needing to check ``ncon``. + Version 2.2.2 (September 7, 2022) --------------------------------- diff --git a/doc/modeling.rst b/doc/modeling.rst index 44ae8625..cc293f44 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -1276,6 +1276,57 @@ MuJoCo Forum for an example; the plots below are generated with that model. |image18| |image19| +.. _CSize: + +Memory allocation +~~~~~~~~~~~~~~~~~ + +MuJoCo preallocates all the memory needed at runtime in ``mjData``, and does not access the heap allocator after +model creation. Memory in ``mjData`` is allocated by :ref:`mj_makeData` in two contiguous blocks: + + - ``mjData.buffer`` contains fixed-size arrays. + - ``mjData.arena`` contains dynamically-sized arrays. + +There are two types of dynamic arrays allocated in the ``arena`` memory space. + + - contacts and constraint-related arrays are laid out from the beginning of the ``arena``. + - :ref:`stack ` arrays are laid out from the end of the ``arena``. + +By allocating dynamic quantities from both sides of the ``arena`` space, variable-sized memory allocation is controlled +by a single number: the :at:`memory` attribute of the :ref:`size ` MJCF element. Unlike the fixed-size arrays in +the ``buffer``, variable-sized arrays in the arena can be ``NULL``, for example after a call to :ref:`mj_resetData`. +When ``arena`` memory runs out, one of three things will happen, depending on the type of memory requested: + + - If memory runs out during contact allocation, a warning will be raised and subsequent contacts will not be added in + this step, but simulation continues as usual. + - If memory runs out during constraint-related allocation, a warning will be raised and the constraint solver will be + disabled in this step, but simulation continues as usual. Note that physics without the constraint solver will + generally be very different, but allowing the simulation to continue can still be useful, e.g. during + scene initialization when many bodies are temporarily overlapping. + - If memory runs out during stack array allocation, a hard error will occur. + +Unlike the size of the ``buffer``, the size of the ``arena`` cannot be pre-computed, since the number of contacts and +stack usage is not known in advance. So how should one choose it? The following simple heuristic is currently used, +though it may be improved in the future: enough memory is allocated for 100 contacts and 500 scalar constraints, under +worst-case conditions. If this heuristic is insufficient, we recommend the following procedure. Increase the ``arena`` +memory significantly using the :at:`memory` attribute, and inspect the actual memory used at runtime. +``mjData.maxuse_arena`` keeps track of the maximum ``arena`` memory utilization since the last reset. The :ref:`simulate +` viewer shows this number as a fraction of the total arena space (in the info window in the lower-left +corner). So one can start with a large number, simulate for a while, and if the fractions are small go back to the XML +and reduce the allocation size. Keep in mind though that memory utilization can change dramatically in the course of the +simulation, depending on how many constraints are active and which constraint solver is used. The CG solver is the most +memory efficient, followed by the Newton solver, while the PGS solver is the most memory intensive. When we design +models, we usually aim for 50% utilization in the worst-case scenario encountered while exploring the model. If you only +intend to use the CG solver, you can get away with significantly smaller arena allocation. + +.. attention:: + + Memory allocation behaviour changed in MuJoCo 2.3.0. Before this version, the :at:`njmax`, :at:`nconmax` and + :at:`nstack` attributes of the :ref:`size ` MJCF element had the semantics of maximum memory allocated for + contacts, constraints and stack, respectively. If you are using an earlier version of MuJoCo, please switch to an + `earlier `_ documentation version to read about the + previous behaviour. + .. _Tips: Tips and tricks @@ -1377,32 +1428,6 @@ in a visible way, and the energy fluctuates around the initial value instead of -.. _CSize: - -Model sizes -~~~~~~~~~~~ - -MuJoCo preallocates all the memory needed at runtime in mjData, and does not access the C/C++ memory manager after -model creation. It is therefore essential to allocate enough memory. The allocation is controlled by three size -parameters specified in the :ref:`size ` element, namely the stack size :at:`nstack`, the -maximum number of contacts :at:`nconmax`, and the maximum number of scalar constraints :at:`njmax`. The default -size settings use heuristics to allocate sufficient memory, but the true memory needs for a given model can only be -determined during simulation. If nstack is insufficient the simulator calls mju_error and gives up. If nconmax or -njmax are insufficient the remaining contacts or other constraints are discarded, and the simulation continues but the -results are not as desired. If on the other hand the allocation is too large, clearing mjData with mj_reset takes -longer, and in multi-threaded applications simulating many large models in parallel the machine could run out of -memory, or cache performance could be adversely affected. And even if nothing bad happens, allocating a lot more -memory than needed is just poor style. - -So how do we know how much memory to allocate? mjData has fields maxuse_stack, maxuse_con and maxuse_efc which keep -track of the maximum memory utilization in each category since the last reset. The code sample :ref:`simulate.cc ` -shows this data as a fraction of the maximum allocation (in the info window in the lower-left corner). So one can start with -the defaults, simulate for a while, and if the fractions are too small go back to the XML and set the allocation sizes -explicitly. Keep in mind though that memory utilization can change dramatically in the course of the simulation, -depending on how many constraints are active and also which constraint solver is used. -For example if the stack size is just sufficient for the CG solver, the Newton and PGS solvers will run out of stack. -When we design models, we usually aim for 50% utilization in the worst-case scenario encountered while exploring the -model. If you only intend to use the CG solver, you can get away with significantly smaller stack allocation. .. |image0| image:: images/modeling/impedance.png :width: 600px diff --git a/doc/programming.rst b/doc/programming.rst index 61425b74..10cd98de 100644 --- a/doc/programming.rst +++ b/doc/programming.rst @@ -503,10 +503,10 @@ preallocated data arrays for all intermediate results, as well as an :ref:`inter to allocate all necessary heap memory at the beginning of the simulation, and free it after the simulation is done, so that we never have to call the C memory allocation and deallocation functions during the simulation. This is done for speed, avoidance of memory fragmentation, future GPU portability, and ease of managing the state of the entire -simulator during a reset. It also means however that the maximal sizes :at:`njmax`, :at:`nconmax` and -:at:`nstack` in the XML element :ref:`size `, which affect the allocation of mjData, must be -set to sufficiently large values. If these maximal sizes are exceeded during the simulation, they are not increased -dynamically, but instead errors or warnings are generated. See also :ref:`diagnostics ` below. +simulator during a reset. It also means however that the maximal variable-memory allocation given by the +:at:`memory` attribute in the :ref:`size ` MJCF element, which affects the allocation of ``mjData``, must be +set to a sufficiently large value. If this maximal size is exceeded during simulation, it is not increased +dynamically, but instead an error is generated. See also :ref:`diagnostics ` below. First we must call one of the functions that allocates and initializes mjModel and returns a pointer to it. The available options are @@ -1308,22 +1308,15 @@ termination have similar order-of-magnitude as the numbers in ``mjData.fwdinv``, different diagnostics. Since MuJoCo's runtime works with compiled models, memory is preallocated when a model is compiled or loaded. Recall the -:ref:`size ` element in MJCF, which has the attributes :at:`njmax`, :at:`nconmax` and :at:`nstack`. They determine -the maximum number of scalar constraints that can be active simultaneously, the maximum number of contact points that -can be included in ``mjData.contact``, and the size of the internal stack. How is the user supposed to know what the -appropriate settings are? If there were a reliable recipe we would have implemented it in the compiler, but there isn't -one. The theoretical worst-case, namely all geoms contacting all other geoms, calls for huge allocation which is almost -never needed in practice. So our approach is to provide default settings in MJCF which are sufficient for most models, -and allow the user to adjust them manually with the above attributes. If the simulator runs out of stack space at -runtime it will trigger an error. If it runs out of space for contacts or scalar constraints, it will trigger a warning -and omit the contacts and constraints that do not fit in the allocated buffers. When such errors or warnings are -triggered, the user should adjust the sizes. The fields ``mjData.maxuse_stack``, ``mjData.maxuse_con``, -``mjData.maxuse_efc`` are designed to help with this adjustment. They keep track of the maximum stack allocation, -number of contacts and number of scalar constraints respectively since the last reset. So one strategy is to make very -large allocation, then monitor these ``maxuse_XXX`` statistics during typical simulations, and use them to reduce the -allocation. Of course modern computers have so much memory that most users will not bother with such adjustment once -they get rid of the out-of-memory errors and warnings, but nevertheless we provide this mechanism for the -perfectionist. +:at:`memory` attribute of the :ref:`size ` element in MJCF. It determines the preallocated space for dynamic +arrays. How is the user supposed to know what the appropriate value is? If there were a reliable recipe we would have +implemented it in the compiler, but there isn't one. The theoretical worst-case, namely all geoms contacting all other +geoms, calls for huge allocation which is almost never needed in practice. Our approach is to provide default settings +in MJCF which are sufficient for most models, and allow the user to adjust them manually with the above attribute. If +the simulator runs out of dynamic memory at runtime it will trigger an error. When such errors are triggered, the user +should increase :at:`memory`. The field ``mjData.maxuse_arena`` is designed to help with this adjustment. It keeps track +of the maximum arena use since the last reset. So one strategy is to make very large allocation, then monitor +``mjData.maxuse_memory`` statistics during typical simulations, and use it to reduce the allocation. The kinetic and potential energy are computed and stored in ``mjData.energy`` when the corresponding flag in ``mjModel.opt.enableflags`` is set. This can be used as another diagnostic. In general, simulation instability is diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 5ef00a87..858d0b1e 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -15,6 +15,7 @@ #ifndef MUJOCO_MJDATA_H_ #define MUJOCO_MJDATA_H_ +#include #include #include @@ -124,15 +125,19 @@ typedef struct mjSolverStat_ mjSolverStat; struct mjData_ { // constant sizes - int nstack; // number of mjtNums that can fit in stack + int nstack; // number of mjtNums that can fit in the arena+stack space int nbuffer; // size of main buffer in bytes int nplugin; // number of plugin instances // stack pointer - int pstack; // first available mjtNum address in stack + size_t pstack; // first available mjtNum address in stack + + // arena pointer + size_t parena; // first available byte in arena // memory utilization stats int maxuse_stack; // maximum stack allocation + size_t maxuse_arena; // maximum arena allocation int maxuse_con; // maximum number of contacts int maxuse_efc; // maximum number of scalar constraints @@ -157,8 +162,8 @@ struct mjData_ { //-------------------------------- end of info header // buffers - void* buffer; // main buffer; all pointers point in it (nbuffer bytes) - mjtNum* stack; // stack buffer (nstack mjtNums) + void* buffer; // main buffer; all pointers point in it (nbuffer bytes) + void* arena; // arena+stack buffer (nstack*sizeof(mjtNum) bytes) //-------------------------------- main inputs and outputs of the computation @@ -240,36 +245,6 @@ struct mjData_ { mjtNum* qLDiagInv; // 1/diag(D) (nv x 1) mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1) - // computed by mj_fwdPosition/mj_collision - mjContact* contact; // list of all detected contacts (nconmax x 1) - - // computed by mj_fwdPosition/mj_makeConstraint - int* efc_type; // constraint type (mjtConstraint) (njmax x 1) - int* efc_id; // id of object of specified type (njmax x 1) - int* efc_J_rownnz; // number of non-zeros in Jacobian row (njmax x 1) - int* efc_J_rowadr; // row start address in colind array (njmax x 1) - int* efc_J_rowsuper; // number of subsequent rows in supernode (njmax x 1) - int* efc_J_colind; // column indices in Jacobian (njmax x nv) - int* efc_JT_rownnz; // number of non-zeros in Jacobian row T (nv x 1) - int* efc_JT_rowadr; // row start address in colind array T (nv x 1) - int* efc_JT_rowsuper; // number of subsequent rows in supernode T (nv x 1) - int* efc_JT_colind; // column indices in Jacobian T (nv x njmax) - mjtNum* efc_J; // constraint Jacobian (njmax x nv) - mjtNum* efc_JT; // constraint Jacobian transposed (nv x njmax) - mjtNum* efc_pos; // constraint position (equality, contact) (njmax x 1) - mjtNum* efc_margin; // inclusion margin (contact) (njmax x 1) - mjtNum* efc_frictionloss; // frictionloss (friction) (njmax x 1) - mjtNum* efc_diagApprox; // approximation to diagonal of A (njmax x 1) - mjtNum* efc_KBIP; // stiffness, damping, impedance, imp' (njmax x 4) - mjtNum* efc_D; // constraint mass (njmax x 1) - mjtNum* efc_R; // inverse constraint mass (njmax x 1) - - // computed by mj_fwdPosition/mj_projectConstraint - int* efc_AR_rownnz; // number of non-zeros in AR (njmax x 1) - int* efc_AR_rowadr; // row start address in colind array (njmax x 1) - int* efc_AR_colind; // column indices in sparse AR (njmax x njmax) - mjtNum* efc_AR; // J*inv(M)*J' + R (njmax x njmax) - //-------------------------------- POSITION, VELOCITY dependent // computed by mj_fwdVelocity @@ -287,8 +262,8 @@ struct mjData_ { mjtNum* qfrc_passive; // passive force (nv x 1) // computed by mj_fwdVelocity/mj_referenceConstraint - mjtNum* efc_vel; // velocity in constraint space: J*qvel (njmax x 1) - mjtNum* efc_aref; // reference pseudo-acceleration (njmax x 1) + mjtNum* efc_vel; // velocity in constraint space: J*qvel (nefc x 1) + mjtNum* efc_aref; // reference pseudo-acceleration (nefc x 1) // computed by mj_sensorVel/mj_subtreeVel if needed mjtNum* subtree_linvel; // linear velocity of subtree com (nbody x 3) @@ -320,9 +295,6 @@ struct mjData_ { mjtNum* qacc_smooth; // unconstrained acceleration (nv x 1) // computed by mj_fwdConstraint/mj_inverse - mjtNum* efc_b; // linear cost term: J*qacc_smooth - aref (njmax x 1) - mjtNum* efc_force; // constraint force in constraint space (njmax x 1) - int* efc_state; // constraint state (mjtConstraintState) (njmax x 1) mjtNum* qfrc_constraint; // constraint force (nv x 1) // computed by mj_inverse @@ -333,6 +305,43 @@ struct mjData_ { mjtNum* cacc; // com-based acceleration (nbody x 6) mjtNum* cfrc_int; // com-based interaction force with parent (nbody x 6) mjtNum* cfrc_ext; // com-based external force on body (nbody x 6) + + //-------------------------------- ARENA-ALLOCATED ARRAYS + + // computed by mj_collision + mjContact* contact; // list of all detected contacts (ncon x 1) + + // computed by mj_makeConstraint + int* efc_type; // constraint type (mjtConstraint) (nefc x 1) + int* efc_id; // id of object of specified type (nefc x 1) + int* efc_J_rownnz; // number of non-zeros in Jacobian row (nefc x 1) + int* efc_J_rowadr; // row start address in colind array (nefc x 1) + int* efc_J_rowsuper; // number of subsequent rows in supernode (nefc x 1) + int* efc_J_colind; // column indices in Jacobian (nefc x nv) + int* efc_JT_rownnz; // number of non-zeros in Jacobian row T (nv x 1) + int* efc_JT_rowadr; // row start address in colind array T (nv x 1) + int* efc_JT_rowsuper; // number of subsequent rows in supernode T (nv x 1) + int* efc_JT_colind; // column indices in Jacobian T (nv x nefc) + mjtNum* efc_J; // constraint Jacobian (nefc x nv) + mjtNum* efc_JT; // constraint Jacobian transposed (nv x nefc) + mjtNum* efc_pos; // constraint position (equality, contact) (nefc x 1) + mjtNum* efc_margin; // inclusion margin (contact) (nefc x 1) + mjtNum* efc_frictionloss; // frictionloss (friction) (nefc x 1) + mjtNum* efc_diagApprox; // approximation to diagonal of A (nefc x 1) + mjtNum* efc_KBIP; // stiffness, damping, impedance, imp' (nefc x 4) + mjtNum* efc_D; // constraint mass (nefc x 1) + mjtNum* efc_R; // inverse constraint mass (nefc x 1) + + // computed by mj_fwdConstraint/mj_inverse + mjtNum* efc_b; // linear cost term: J*qacc_smooth - aref (nefc x 1) + mjtNum* efc_force; // constraint force in constraint space (nefc x 1) + int* efc_state; // constraint state (mjtConstraintState) (nefc x 1) + + // computed by mj_projectConstraint + int* efc_AR_rownnz; // number of non-zeros in AR (nefc x 1) + int* efc_AR_rowadr; // row start address in colind array (nefc x 1) + int* efc_AR_colind; // column indices in sparse AR (nefc x nefc) + mjtNum* efc_AR; // J*inv(M)*J' + R (nefc x nefc) }; typedef struct mjData_ mjData; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index f6d53c09..d6600773 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -434,8 +434,7 @@ // define symbols needed in MJDATA_POINTERS (corresponding to number of columns) #define MJDATA_POINTERS_PREAMBLE( m ) \ - int nv = m->nv; \ - int njmax = m->njmax; + int nv = m->nv; // pointer fields of mjData @@ -490,38 +489,12 @@ X( mjtNum, qLD, nM, 1 ) \ X( mjtNum, qLDiagInv, nv, 1 ) \ X( mjtNum, qLDiagSqrtInv, nv, 1 ) \ - X( mjContact, contact, nconmax, 1 ) \ - X( int, efc_type, njmax, 1 ) \ - X( int, efc_id, njmax, 1 ) \ - X( int, efc_J_rownnz, njmax, 1 ) \ - X( int, efc_J_rowadr, njmax, 1 ) \ - X( int, efc_J_rowsuper, njmax, 1 ) \ - X( int, efc_J_colind, njmax, MJ_M(nv) ) \ - X( int, efc_JT_rownnz, nv, 1 ) \ - X( int, efc_JT_rowadr, nv, 1 ) \ - X( int, efc_JT_rowsuper, nv, 1 ) \ - X( int, efc_JT_colind, nv, MJ_M(njmax) ) \ - X( mjtNum, efc_J, njmax, MJ_M(nv) ) \ - X( mjtNum, efc_JT, nv, MJ_M(njmax) ) \ - X( mjtNum, efc_pos, njmax, 1 ) \ - X( mjtNum, efc_margin, njmax, 1 ) \ - X( mjtNum, efc_frictionloss, njmax, 1 ) \ - X( mjtNum, efc_diagApprox, njmax, 1 ) \ - X( mjtNum, efc_KBIP, njmax, 4 ) \ - X( mjtNum, efc_D, njmax, 1 ) \ - X( mjtNum, efc_R, njmax, 1 ) \ - X( int, efc_AR_rownnz, njmax, 1 ) \ - X( int, efc_AR_rowadr, njmax, 1 ) \ - X( int, efc_AR_colind, njmax, MJ_M(njmax) ) \ - X( mjtNum, efc_AR, njmax, MJ_M(njmax) ) \ X( mjtNum, ten_velocity, ntendon, 1 ) \ X( mjtNum, actuator_velocity, nu, 1 ) \ X( mjtNum, cvel, nbody, 6 ) \ X( mjtNum, cdof_dot, nv, 6 ) \ X( mjtNum, qfrc_bias, nv, 1 ) \ X( mjtNum, qfrc_passive, nv, 1 ) \ - X( mjtNum, efc_vel, njmax, 1 ) \ - X( mjtNum, efc_aref, njmax, 1 ) \ X( mjtNum, subtree_linvel, nbody, 3 ) \ X( mjtNum, subtree_angmom, nbody, 3 ) \ X( mjtNum, qH, nM, 1 ) \ @@ -535,9 +508,6 @@ X( mjtNum, qfrc_actuator, nv, 1 ) \ X( mjtNum, qfrc_smooth, nv, 1 ) \ X( mjtNum, qacc_smooth, nv, 1 ) \ - X( mjtNum, efc_b, njmax, 1 ) \ - X( mjtNum, efc_force, njmax, 1 ) \ - X( int, efc_state, njmax, 1 ) \ X( mjtNum, qfrc_constraint, nv, 1 ) \ X( mjtNum, qfrc_inverse, nv, 1 ) \ X( mjtNum, cacc, nbody, 6 ) \ @@ -545,6 +515,55 @@ X( mjtNum, cfrc_ext, nbody, 6 ) +// macro for annotating that an array size in an X macro is a member of mjData +// by default this macro does nothing, but users can redefine it as necessary +#define MJ_D(n) n + +// array of contacts +#define MJDATA_ARENA_POINTERS_CONTACT \ + X( mjContact, contact, MJ_D(ncon), 1 ) + +// array fields of mjData that are used in the primal problem +#define MJDATA_ARENA_POINTERS_PRIMAL \ + X( int, efc_type, MJ_D(nefc), 1 ) \ + X( int, efc_id, MJ_D(nefc), 1 ) \ + X( int, efc_J_rownnz, MJ_D(nefc), 1 ) \ + X( int, efc_J_rowadr, MJ_D(nefc), 1 ) \ + X( int, efc_J_rowsuper, MJ_D(nefc), 1 ) \ + X( int, efc_J_colind, MJ_D(nefc), MJ_M(nv) ) \ + X( int, efc_JT_rownnz, MJ_M(nv), 1 ) \ + X( int, efc_JT_rowadr, MJ_M(nv), 1 ) \ + X( int, efc_JT_rowsuper, MJ_M(nv), 1 ) \ + X( int, efc_JT_colind, MJ_M(nv), MJ_D(nefc) ) \ + X( mjtNum, efc_J, MJ_D(nefc), MJ_M(nv) ) \ + X( mjtNum, efc_JT, MJ_M(nv), MJ_D(nefc) ) \ + X( mjtNum, efc_pos, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_margin, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_frictionloss, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_diagApprox, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_KBIP, MJ_D(nefc), 4 ) \ + X( mjtNum, efc_D, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_R, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_vel, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_aref, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_b, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_force, MJ_D(nefc), 1 ) \ + X( int, efc_state, MJ_D(nefc), 1 ) \ + +// array fields of mjData that are used in the dual problem +#define MJDATA_ARENA_POINTERS_DUAL \ + X( int, efc_AR_rownnz, MJ_D(nefc), 1 ) \ + X( int, efc_AR_rowadr, MJ_D(nefc), 1 ) \ + X( int, efc_AR_colind, MJ_D(nefc), MJ_D(nefc) ) \ + X( mjtNum, efc_AR, MJ_D(nefc), MJ_D(nefc) ) + +// array fields of mjData that live in d->arena +#define MJDATA_ARENA_POINTERS \ + MJDATA_ARENA_POINTERS_CONTACT \ + MJDATA_ARENA_POINTERS_PRIMAL \ + MJDATA_ARENA_POINTERS_DUAL + + // scalar fields of mjData #define MJDATA_SCALAR \ X( int, nstack ) \ diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 6abed1ed..90b1270c 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -457,18 +457,22 @@ class MuJoCoBindingsTest(parameterized.TestCase): # Grab a reference to the contacts upfront so that we know that they're # a view into mjData rather than a copy. - contact = self.data.contact[:4] + contact = self.data.contact self.model.opt.timestep = 2**-9 # 0.001953125; allows exact comparisons self.assertEqual(self.data.time, 0) while self.data.time < expected_contact_time: self.assertEqual(self.data.ncon, 0) + self.assertEmpty(self.data.efc_type) + self.assertTrue(self.data.efc_type.flags['OWNDATA']) prev_time = self.data.time mujoco.mj_step(self.model, self.data) self.assertEqual(self.data.time, prev_time + self.model.opt.timestep) mujoco.mj_forward(self.model, self.data) self.assertEqual(self.data.ncon, 4) + self.assertLen(self.data.efc_type, 16) + self.assertFalse(self.data.efc_type.flags['OWNDATA']) # Sort contacts in anticlockwise order sorted_contact = sorted( @@ -478,6 +482,11 @@ class MuJoCoBindingsTest(parameterized.TestCase): np.testing.assert_allclose(sorted_contact[2].pos[:2], [0.1, 0.1]) np.testing.assert_allclose(sorted_contact[3].pos[:2], [-0.1, 0.1]) + mujoco.mj_resetData(self.model, self.data) + self.assertEqual(self.data.ncon, 0) + self.assertEmpty(self.data.efc_type) + self.assertTrue(self.data.efc_type.flags['OWNDATA']) + def test_mj_step_multiple(self): self.model.opt.timestep = 2**-9 # 0.001953125; allows exact comparisons self.assertEqual(self.data.time, 0) @@ -488,24 +497,31 @@ class MuJoCoBindingsTest(parameterized.TestCase): self.assertIn('Optionally, repeat nstep times.', mujoco.mj_step.__doc__) def test_mj_contact_list(self): - self.assertLen(self.data.contact, self.model.nconmax) + self.assertEmpty(self.data.contact) + + expected_ncon = 1234 + self.data.ncon = expected_ncon + self.assertLen(self.data.contact, expected_ncon) expected_pos = [] for contact in self.data.contact: expected_pos.append(np.random.uniform(size=3)) contact.pos = expected_pos[-1] + self.assertLen(expected_pos, expected_ncon) np.testing.assert_array_equal(self.data.contact.pos, expected_pos) expected_friction = [] for contact in self.data.contact: expected_friction.append(np.random.uniform(size=5)) contact.friction = expected_friction[-1] + self.assertLen(expected_friction, expected_ncon) np.testing.assert_array_equal(self.data.contact.friction, expected_friction) expected_H = [] # pylint: disable=invalid-name for contact in self.data.contact: expected_H.append(np.random.uniform(size=36)) contact.H = expected_H[-1] + self.assertLen(expected_H, expected_ncon) np.testing.assert_array_equal(self.data.contact.H, expected_H) def test_mj_struct_list_equality(self): @@ -516,16 +532,16 @@ class MuJoCoBindingsTest(parameterized.TestCase): self.assertEqual(self.data.ncon, 4) mujoco.mj_forward(model2, data2) self.assertEqual(data2.ncon, 4) - self.assertEqual(data2.contact[:4], self.data.contact[:4]) + self.assertEqual(data2.contact, self.data.contact) self.data.qpos[3:7] = [np.cos(np.pi/8), np.sin(np.pi/8), 0, 0] self.data.qpos[2] *= (np.sqrt(2) - 1) * 0.1 - 1e-6 mujoco.mj_forward(self.model, self.data) self.assertEqual(self.data.ncon, 2) - self.assertNotEqual(data2.contact[:2], self.data.contact[:2]) + self.assertNotEqual(data2.contact, self.data.contact) # Check that we can compare slices of different lengths - self.assertNotEqual(data2.contact[:2], self.data.contact[:4]) + self.assertNotEqual(data2.contact, self.data.contact) # Check that comparing things of different types do not raise an error self.assertNotEqual(self.data.contact, self.data.warning) @@ -856,7 +872,7 @@ Euler integrator, semi-implicit in velocity. def test_can_raise_error(self): self.data.pstack = self.data.nstack - with self.assertRaisesWithLiteralMatch(mujoco.FatalError, 'Stack overflow'): + with self.assertRaisesRegex(mujoco.FatalError, r'\Astack overflow'): mujoco.mj_forward(self.model, self.data) def test_mjcb_time(self): diff --git a/python/mujoco/mjdata_meta.h b/python/mujoco/mjdata_meta.h index f3ee806c..c432c4df 100644 --- a/python/mujoco/mjdata_meta.h +++ b/python/mujoco/mjdata_meta.h @@ -15,6 +15,7 @@ #ifndef MUJOCO_PYTHON_MJDATA_META_H_ #define MUJOCO_PYTHON_MJDATA_META_H_ +#include #include #include "raw.h" #include "util/crossplatform.h" @@ -74,6 +75,8 @@ struct MjDataMetadata { MJDATA_METADATA #undef X + bool is_dual; + private: MjDataMetadata() = default; MjDataMetadata(const MjDataMetadata& other) = default; @@ -94,10 +97,7 @@ struct MjDataMetadata { MJDATA_METADATA #undef X - dummy_() {} - - // Dummy variable to terminate X macro sequences. - MUJOCO_MAYBE_UNUSED bool dummy_; + is_dual(mj_isDual(m)) {} }; } // namespace mujoco::python diff --git a/python/mujoco/serialization.h b/python/mujoco/serialization.h index b669d4da..9fb6a1b3 100644 --- a/python/mujoco/serialization.h +++ b/python/mujoco/serialization.h @@ -41,25 +41,26 @@ inline char ReadChar(std::istream& input) { return c; } -inline void WriteInt(std::ostream& output, int i) { - output.write(reinterpret_cast(&i), sizeof(int)); +inline void WriteInt(std::ostream& output, std::size_t i) { + output.write(reinterpret_cast(&i), sizeof(std::size_t)); } -inline int ReadInt(std::istream& input) { - int i = 0; - input.read(reinterpret_cast(&i), sizeof(int)); +inline std::size_t ReadInt(std::istream& input) { + std::size_t i = 0; + input.read(reinterpret_cast(&i), sizeof(std::size_t)); return i; } -inline void WriteBytes(std::ostream& output, const void* src, size_t nbytes) { +inline void WriteBytes(std::ostream& output, const void* src, + std::size_t nbytes) { // Start by writing nbytes itself, so it can be validated at the time of // reading. WriteInt(output, nbytes); output.write(reinterpret_cast(src), nbytes); } -inline void ReadBytes(std::istream& input, void* dest, size_t nbytes) { - size_t actual_nbytes = ReadInt(input); +inline void ReadBytes(std::istream& input, void* dest, std::size_t nbytes) { + std::size_t actual_nbytes = ReadInt(input); if (actual_nbytes != nbytes) { input.setstate(input.rdstate() | std::ios_base::failbit); return; diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index ecaf305a..04e18bbf 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -52,6 +52,9 @@ namespace mujoco::python::_impl { namespace py = ::pybind11; namespace { +#define PTRDIFF(x, y) \ + reinterpret_cast(x) - reinterpret_cast(y) + // Returns the shape of a NumPy array given the dimensions from an X Macro. // If dim1 is a _literal_ constant 1, the resulting array is 1-dimensional of // length dim0, otherwise the resulting array is 2-dimensional of shape @@ -74,6 +77,9 @@ constexpr auto XArrayShapeImpl(const std::string_view dim1_str) { } } +inline std::size_t NConMax(const mjData* d) { + return d->nstack * sizeof(mjtNum) / sizeof(mjContact); +} } // namespace // ==================== MJOPTION =============================================== @@ -498,51 +504,15 @@ MjContactWrapper::MjWrapper(const MjContactWrapper& other) *this->ptr_ = *other.ptr_; } -#define X(type, var) \ - var(std::vector{num}, std::vector{sizeof(raw::MjContact)}, \ - &ptr->var, owner) -#define XN(type, var) \ - var(std::vector{num, sizeof(raw::MjContact::var) / sizeof(type)}, \ - std::vector{sizeof(raw::MjContact), sizeof(type)}, &ptr->var[0], \ - owner) -MjContactList::MjStructList(raw::MjContact* ptr, int num, py::handle owner) - : StructListBase(ptr, num, owner), - X(mjtNum, dist), - XN(mjtNum, pos), - XN(mjtNum, frame), - X(mjtNum, includemargin), - XN(mjtNum, friction), - XN(mjtNum, solref), - XN(mjtNum, solimp), - X(mjtNum, mu), - XN(mjtNum, H), - X(int, dim), - X(int, geom1), - X(int, geom2), - X(int, exclude), - X(int, efc_address) {} -#undef X -#undef XN +MjContactList::MjStructList(raw::MjContact* ptr, int nconmax, + int* ncon, py::handle owner) + : StructListBase(ptr, nconmax, owner, /* lazy = */ true), + ncon_(ncon) {} // Slicing -#define X(type, var) var(other.var[slice]) MjContactList::MjStructList(MjContactList& other, py::slice slice) : StructListBase(other, slice), - X(mjtNum, dist), - X(mjtNum, pos), - X(mjtNum, frame), - X(mjtNum, includemargin), - X(mjtNum, friction), - X(mjtNum, solref), - X(mjtNum, solimp), - X(mjtNum, mu), - X(mjtNum, H), - X(int, dim), - X(int, geom1), - X(int, geom2), - X(int, exclude), - X(int, efc_address) {} -#undef X + ncon_(other.ncon_) {} // ==================== MJDATA ================================================= static void MjDataCapsuleDestructor(PyObject* pyobj) { @@ -579,9 +549,11 @@ MjDataWrapper::MjWrapper(const MjModelWrapper& model) var(InitPyArray(X_ARRAY_SHAPE(model.get()->dim0, dim1), ptr_->var, owner_)), MJDATA_POINTERS #undef MJ_M -#define MJ_M(x) x +#define MJ_M(x) (x) #undef X + contact(MjContactList(ptr_->contact, NConMax(ptr_), &ptr_->ncon, owner_)), + #define X(dtype, var, dim0, dim1) var(InitPyArray(ptr_->var, owner_)), MJDATA_VECTOR #undef X @@ -607,9 +579,11 @@ MjDataWrapper::MjWrapper(const MjDataWrapper& other) owner_)), MJDATA_POINTERS #undef MJ_M -#define MJ_M(x) x +#define MJ_M(x) (x) #undef X + contact(MjContactList(ptr_->contact, NConMax(ptr_), &ptr_->ncon, owner_)), + #define X(dtype, var, dim0, dim1) var(InitPyArray(ptr_->var, owner_)), MJDATA_VECTOR #undef X @@ -635,9 +609,11 @@ MjDataWrapper::MjWrapper(MjDataWrapper&& other) owner_)), MJDATA_POINTERS #undef MJ_M -#define MJ_M(x) x +#define MJ_M(x) (x) #undef X + contact(MjContactList(ptr_->contact, NConMax(ptr_), &ptr_->ncon,owner_)), + #define X(dtype, var, dim0, dim1) var(InitPyArray(ptr_->var, owner_)), MJDATA_VECTOR #undef X @@ -664,9 +640,11 @@ MjDataWrapper::MjWrapper(MjDataMetadata&& metadata, raw::MjData* d) var(InitPyArray(X_ARRAY_SHAPE(metadata.dim0, dim1), ptr_->var, owner_)), MJDATA_POINTERS #undef MJ_M -#define MJ_M(x) x +#define MJ_M(x) (x) #undef X + contact(MjContactList(ptr_->contact, NConMax(ptr_), &ptr_->ncon, owner_)), + #define X(dtype, var, dim0, dim1) var(InitPyArray(ptr_->var, owner_)), MJDATA_VECTOR #undef X @@ -707,6 +685,8 @@ void MjDataWrapper::Serialize(std::ostream& output) const { MJMODEL_INTS #undef X + WriteInt(output, this->metadata_.is_dual); + #define X(dtype, var, n) \ WriteBytes(output, this->metadata_.var.get(), \ this->metadata_.n * sizeof(dtype)); @@ -716,9 +696,17 @@ void MjDataWrapper::Serialize(std::ostream& output) const { // Write struct and scalar fields #define X(var) WriteBytes(output, &ptr_->var, sizeof(ptr_->var)) + X(parena); + X(maxuse_stack); + X(maxuse_arena); + X(maxuse_con); + X(maxuse_efc); X(solver); X(timer); X(warning); + X(ne); + X(nf); + X(nefc); X(ncon); X(time); X(energy); @@ -727,9 +715,32 @@ void MjDataWrapper::Serialize(std::ostream& output) const { // Write buffer contents { MJDATA_POINTERS_PREAMBLE((&this->metadata_)) + #define X(type, name, nr, nc) \ WriteBytes(output, ptr_->name, sizeof(type)*(this->metadata_.nr)*(nc)); MJDATA_POINTERS +#undef X + +#undef MJ_M +#define MJ_M(x) this->metadata_.x +#undef MJ_D +#define MJ_D(x) this->ptr_->x +#define X(type, name, nr, nc) \ + if ((nr) * (nc)) { \ + WriteInt(output, PTRDIFF(ptr_->name, ptr_->arena)); \ + WriteBytes(output, ptr_->name, sizeof(type) * (nr) * (nc)); \ + } + + MJDATA_ARENA_POINTERS_CONTACT + MJDATA_ARENA_POINTERS_PRIMAL + + if (this->metadata_.is_dual) { + MJDATA_ARENA_POINTERS_DUAL + } +#undef MJ_M +#define MJ_M(x) x +#undef MJ_D +#define MJ_D(x) x #undef X } } @@ -754,6 +765,8 @@ MjDataWrapper MjDataWrapper::Deserialize(std::istream& input) { MJMODEL_INTS #undef X + metadata.is_dual = ReadInt(input); + #define X(dtype, var, n) \ metadata.var.reset(new dtype[metadata.n]); \ ReadBytes(input, metadata.var.get(), metadata.n * sizeof(dtype)); \ @@ -773,9 +786,17 @@ MjDataWrapper MjDataWrapper::Deserialize(std::istream& input) { ReadBytes(input, (void*) &d->var, sizeof(d->var)); \ CheckInput(input, "mjData"); + X(parena); + X(maxuse_stack); + X(maxuse_arena); + X(maxuse_con); + X(maxuse_efc); X(solver); X(timer); X(warning); + X(ne); + X(nf); + X(nefc); X(ncon); X(time); X(energy); @@ -784,9 +805,33 @@ MjDataWrapper MjDataWrapper::Deserialize(std::istream& input) { // Read buffer contents { MJDATA_POINTERS_PREAMBLE((&m)) + #define X(type, name, nr, nc) \ ReadBytes(input, d->name, sizeof(type)*(m.nr)*(nc)); MJDATA_POINTERS +#undef X + +#undef MJ_M +#define MJ_M(x) m.x +#undef MJ_D +#define MJ_D(x) d->x +#define X(type, name, nr, nc) \ + if ((nr) * (nc)) { \ + d->name = reinterpret_castname)>( \ + static_cast(d->arena) + ReadInt(input)); \ + ReadBytes(input, d->name, sizeof(type) * (nr) * (nc)); \ + } + + MJDATA_ARENA_POINTERS_CONTACT + MJDATA_ARENA_POINTERS_PRIMAL + + if (metadata.is_dual) { + MJDATA_ARENA_POINTERS_DUAL + } +#undef MJ_M +#define MJ_M(x) x +#undef MJ_D +#define MJ_D(x) x #undef X } CheckInput(input, "mjData"); @@ -1697,22 +1742,36 @@ This is useful for example when the MJB is not available as a file on disk.)")); mjContactList.def("__len__", &MjContactList::size); DefineStructFunctions(mjContactList); -#define X(type, var) mjContactList.def_readonly(#var, &MjContactList::var) +#define X(type, var) \ + mjContactList.def_property_readonly(#var, [](const MjContactList& c) { \ + return py::array_t(std::vector{c.size()}, \ + std::vector{sizeof(raw::MjContact)}, \ + &c.get()->var, c.owner()); \ + }); +#define XN(type, var) \ + mjContactList.def_property_readonly(#var, [](const MjContactList& c) { \ + return py::array_t( \ + std::vector{c.size(), \ + sizeof(raw::MjContact::var) / sizeof(type)}, \ + std::vector{sizeof(raw::MjContact), sizeof(type)}, \ + &c.get()->var[0], c.owner()); \ + }); X(mjtNum, dist); - X(mjtNum, pos); - X(mjtNum, frame); + XN(mjtNum, pos); + XN(mjtNum, frame); X(mjtNum, includemargin); - X(mjtNum, friction); - X(mjtNum, solref); - X(mjtNum, solimp); + XN(mjtNum, friction); + XN(mjtNum, solref); + XN(mjtNum, solimp); X(mjtNum, mu); - X(mjtNum, H); + XN(mjtNum, H); X(int, dim); X(int, geom1); X(int, geom2); X(int, exclude); X(int, efc_address); #undef X +#undef XN // ==================== MJDATA =============================================== py::class_ mjData(m, "MjData"); @@ -1749,6 +1808,29 @@ This is useful for example when the MJB is not available as a file on disk.)")); #define X(dtype, var, dim0, dim1) \ DefinePyArray(mjData, #var, &MjDataWrapper::var); MJDATA_POINTERS + MJDATA_ARENA_POINTERS_CONTACT +#undef X + +#undef MJ_M +#define MJ_M(x) d.metadata().x +#undef MJ_D +#define MJ_D(x) d.get()->x +#define X(dtype, var, dim0, dim1) \ + mjData.def_property_readonly(#var, [](const MjDataWrapper& d) { \ + return InitPyArray(X_ARRAY_SHAPE(dim0, dim1), d.get()->var, d.owner()); \ + }); + + MJDATA_ARENA_POINTERS_PRIMAL + MJDATA_ARENA_POINTERS_DUAL + +#undef MJ_M +#define MJ_M(x) (x) +#undef MJ_D +#define MJ_D(x) (x) +#undef X + +#define X(dtype, var, dim0, dim1) \ + DefinePyArray(mjData, #var, &MjDataWrapper::var); MJDATA_VECTOR #undef X diff --git a/python/mujoco/structs.h b/python/mujoco/structs.h index aa2e8f1c..93f1df9f 100644 --- a/python/mujoco/structs.h +++ b/python/mujoco/structs.h @@ -92,38 +92,59 @@ class MjWrapper {}; template class StructListBase { public: - StructListBase(T* ptr, int num, pybind11::handle owner) : ptr_(ptr) { - for (int i = 0; i < num; ++i) { - wrappers_.push_back(std::make_shared>(&ptr[i], owner)); + StructListBase(T* ptr, int num, pybind11::handle owner, bool lazy = false) + : ptr_(ptr), num_(num), owner_(owner) { + if (!lazy) { + PopulateUpTo(size()); } } StructListBase(const StructListBase& other) = delete; StructListBase(StructListBase&& other) = default; + virtual ~StructListBase() = default; + MjWrapper& operator[](int i) { - if (i < 0 || i >= wrappers_.size()) { + if (i < 0 || i >= size()) { throw pybind11::index_error(); } + PopulateUpTo(i); return *wrappers_[i]; } - int size() const { - return wrappers_.size(); + virtual int size() const { + return num_; } + T* get() const { return ptr_; } + pybind11::handle owner() const { return owner_; } + protected: + void PopulateUpTo(int n) { + while (wrappers_.size() <= n) { + wrappers_.push_back( + std::make_shared>(&ptr_[wrappers_.size()], owner_)); + } + } + // Slicing - StructListBase(StructListBase& other, pybind11::slice slice) { + StructListBase(StructListBase& other, pybind11::slice slice) + : owner_(other.owner_) { pybind11::size_t start, stop, step, slicelength; - slice.compute(other.size(), &start, &stop, &step, &slicelength); + if (!slice.compute(other.size(), &start, &stop, &step, &slicelength)) { + throw pybind11::index_error(); + } + other.PopulateUpTo(stop); ptr_ = &other.ptr_[start]; for (int i = start; i < stop; i += step) { wrappers_.push_back(other.wrappers_[i]); } + num_ = wrappers_.size(); } T* ptr_; + int num_; + pybind11::handle owner_; // Using shared_ptr here so that we get identical Python objects when slicing. std::vector>> wrappers_; @@ -277,6 +298,8 @@ class MjStructList : public StructListBase { public: MjStructList(raw::MjWarningStat* ptr, int num, pybind11::handle owner); + MjStructList(MjStructList&&) = default; + ~MjStructList() override = default; using StructListBase::operator[]; using StructListBase::size; @@ -325,6 +348,8 @@ template <> class MjStructList : public StructListBase { public: MjStructList(raw::MjTimerStat* ptr, int num, pybind11::handle owner); + MjStructList(MjStructList&&) = default; + ~MjStructList() override = default; using StructListBase::operator[]; using StructListBase::size; @@ -374,6 +399,8 @@ class MjStructList : public StructListBase { public: MjStructList(raw::MjSolverStat* ptr, int num, pybind11::handle owner); + MjStructList(MjStructList&&) = default; + ~MjStructList() override = default; using StructListBase::operator[]; using StructListBase::size; @@ -489,33 +516,28 @@ struct enable_if_mj_struct { using type = void; }; template <> class MjStructList : public StructListBase { public: - MjStructList(raw::MjContact* ptr, int num, pybind11::handle owner); + MjStructList(raw::MjContact* ptr, int nconmax, + int* ncon, pybind11::handle owner); + MjStructList(MjStructList&&) = default; + ~MjStructList() override = default; using StructListBase::operator[]; - using StructListBase::size; + + int size() const override { + if (ncon_) { + return *ncon_; + } else { + return StructListBase::size(); + } + } + MjStructList Slice(pybind11::slice slice) { return MjStructList(*this, slice); } -#define X(type, var) pybind11::array_t var - X(mjtNum, dist); - X(mjtNum, pos); - X(mjtNum, frame); - X(mjtNum, includemargin); - X(mjtNum, friction); - X(mjtNum, solref); - X(mjtNum, solimp); - X(mjtNum, mu); - X(mjtNum, H); - X(int, dim); - X(int, geom1); - X(int, geom2); - X(int, exclude); - X(int, efc_address); -#undef X - protected: MjStructList(MjStructList& other, pybind11::slice slice); + int* ncon_ = nullptr; }; using MjContactList = MjStructList; @@ -539,6 +561,7 @@ class MjWrapper: public WrapperBase { MjWrapper(MjWrapper&&); ~MjWrapper(); + const MjDataMetadata& metadata() const { return metadata_; } MjDataIndexer& indexer() { return indexer_; } void Serialize(std::ostream& output) const; @@ -548,10 +571,13 @@ class MjWrapper: public WrapperBase { "__MUJOCO_STRUCTS_MJDATAWRAPPER_LOOKUP"; static MjWrapper* FromRawPointer(raw::MjData* m) noexcept; + #define X(dtype, var, dim0, dim1) py_array_or_tuple_t var; MJDATA_POINTERS #undef X + py_array_or_tuple_t contact; + py_array_or_tuple_t warning; py_array_or_tuple_t timer; py_array_or_tuple_t solver; diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 5f13406e..210253fc 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -26,6 +26,7 @@ #include #include "lodepng.h" #include +#include #include #include #include "glfw_dispatch.h" @@ -475,9 +476,9 @@ void infotext(mj::Simulate* sim, solerr = mju_log10(mju_max(mjMINVAL, solerr)); // prepare info text - mju::strcpy_arr(title, "Time\nSize\nCPU\nSolver \nFPS\nstack\nconbuf\nefcbuf"); + mju::strcpy_arr(title, "Time\nSize\nCPU\nSolver \nFPS\nMemory"); mju::sprintf_arr(content, - "%-9.3f\n%d (%d con)\n%.3f\n%.1f (%d it)\n%.0f\n%.3f\n%.3f\n%.3f", + "%-9.3f\n%d (%d con)\n%.3f\n%.1f (%d it)\n%.0f\n%.3f", d->time, d->nefc, d->ncon, sim->run ? @@ -485,9 +486,7 @@ void infotext(mj::Simulate* sim, d->timer[mjTIMER_FORWARD].duration / mjMAX(1, d->timer[mjTIMER_FORWARD].number), solerr, d->solver_iter, 1/interval, - d->maxuse_stack/(double)d->nstack, - d->maxuse_con/(double)m->nconmax, - d->maxuse_efc/(double)m->njmax); + d->maxuse_arena/(double)(d->nstack * sizeof(mjtNum))); // add Energy if enabled { diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 6e101d29..3c5610d4 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -19,6 +19,7 @@ #include #include +#include #include "engine/engine_callback.h" #include "engine/engine_collision_convex.h" #include "engine/engine_collision_primitive.h" @@ -55,7 +56,7 @@ void mj_collision(const mjModel* m, mjData* d) { int *broadphasepair = 0; mjMARKSTACK; - // clear size + // reset the size of the contact array d->ncon = 0; // return if disabled diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 59b2ba23..7ccf6ae5 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -13,13 +13,15 @@ // limitations under the License. #include "engine/engine_core_constraint.h" +#include #include #include #include #include -#include "engine/engine_collision_driver.h" +#include +#include "engine/engine_array_safety.h" #include "engine/engine_core_smooth.h" #include "engine/engine_io.h" #include "engine/engine_macro.h" @@ -101,14 +103,27 @@ mjtNum mj_assignMargin(const mjModel* m, mjtNum source) { // add contact to d->contact list; return 0 if success; 1 if buffer full int mj_addContact(const mjModel* m, mjData* d, const mjContact* con) { - // if out of space, warn and return error - if (d->ncon >= m->nconmax) { - mj_warning(d, mjWARN_CONTACTFULL, m->nconmax); + // if nconmax is specified and ncon >= nconmax, warn and return error + if (m->nconmax != -1 && d->ncon >= m->nconmax) { + mj_warning(d, mjWARN_CONTACTFULL, d->ncon); return 1; } + // move arena pointer back to the end of the existing contact array and invalidate efc_ arrays + d->parena = d->ncon * sizeof(mjContact); + d->nefc = 0; +#define X(type, name, nr, nc) d->name = NULL; + MJDATA_ARENA_POINTERS +#undef X + d->contact = d->arena; + // copy contact - d->contact[d->ncon] = *con; + mjContact* dst = mj_arenaAlloc(d, sizeof(mjContact), _Alignof(mjContact)); + if (!dst) { + mj_warning(d, mjWARN_CONTACTFULL, d->ncon); + return 1; + } + *dst = *con; // increase counter, return success d->ncon++; @@ -127,12 +142,6 @@ int mj_addConstraint(const mjModel* m, mjData* d, int *nnz = d->efc_J_rownnz, *adr = d->efc_J_rowadr, *ind = d->efc_J_colind; mjtNum *J = d->efc_J; - // if out of space, warn and return error - if (nefc+size > m->njmax) { - mj_warning(d, mjWARN_CNSTRFULL, m->njmax); - return 1; - } - // init empty guard for constraints other than contact if (type==mjCNSTR_CONTACT_FRICTIONLESS || type==mjCNSTR_CONTACT_PYRAMIDAL || @@ -348,7 +357,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mjtNum *jac[2], *jacdif, *data, *sparse_buf = NULL; mjMARKSTACK; - // disabled or no equality contraints: return + // disabled or no equality constraints: return if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax==0) { return; } @@ -774,12 +783,6 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { b1 = m->geom_bodyid[con->geom1]; b2 = m->geom_bodyid[con->geom2]; - // check size here, because pyramid rows are added incrementally - if (d->nefc + (dim==1 ? 1 : (ispyramid ? 2*(dim-1) : dim)) > m->njmax) { - mj_warning(d, mjWARN_CNSTRFULL, m->njmax); - break; - } - // save efc_address con->efc_address = d->nefc; @@ -1238,6 +1241,164 @@ void mj_makeImpedance(const mjModel* m, mjData* d) { +//------------------------------------- constraint counting ---------------------------------------- + +// count equality constraints +static inline int mj_ne(const mjModel* m, const mjData* d) { + // disabled or no equality constraints: return + if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax==0) { + return 0; + } + + int ne = 0; + + for (int i=0; ineq; i++) { + if (!m->eq_active[i]) { + continue; + } + + // process according to type + switch (m->eq_type[i]) { + case mjEQ_CONNECT: + ne += 3; + break; + + case mjEQ_WELD: + ne += 6; + break; + + case mjEQ_JOINT: + case mjEQ_TENDON: + ne++; + break; + + default: // SHOULD NOT OCCUR + mju_error_i("Invalid equality constraint type %d", m->eq_type[i]); + } + } + + return ne; +} + + + +// count frictional constraints +static inline int mj_nf(const mjModel* m, const mjData* d) { + // disabled: return + if (mjDISABLED(mjDSBL_FRICTIONLOSS)) { + return 0; + } + + int nf = 0; + const int nv = m->nv; + const int ntendon = m->ntendon; + + // count frictional dofs + for (int i=0; idof_frictionloss[i] > 0); + } + + // count frictional tendons + for (int i=0; itendon_frictionloss[i] > 0); + } + + return nf; +} + + + +// count limit constraints +static inline int mj_nl(const mjModel* m, const mjData* d) { + // disabled: return + if (mjDISABLED(mjDSBL_LIMIT)) { + return 0; + } + + int nl = 0; + const int njnt = m->njnt; + const int ntendon = m->ntendon; + + // count limited joints + for (int i=0; ijnt_limited[i]) { + continue; + } + + // slides and hinges can have active limits on two sides, check both + if (m->jnt_type[i]==mjJNT_SLIDE || m->jnt_type[i]==mjJNT_HINGE) { + // get margin + mjtNum margin = m->jnt_margin[i]; + + // get joint value + mjtNum value = d->qpos[m->jnt_qposadr[i]]; + + // check lower and upper limits + for (int side=-1; side<=1; side+=2) { + // compute distance (negative: penetration) + mjtNum dist = side * (m->jnt_range[2*i+(side+1)/2] - value); + + // detect joint limit + if (disttendon_limited[i]; + } + + return nl; +} + + + +// count contact constraints +static inline int mj_nc(const mjModel* m, const mjData* d) { + // disabled or no contacts: return + int ncon = d->ncon; + if (mjDISABLED(mjDSBL_CONTACT) || ncon==0) { + return 0; + } + + int nc = 0; + int ispyramid = mj_isPyramidal(m); + + // find contacts to be counted + for (int i=0; icontact + i; + if (con->exclude) { + continue; + } + + int dim = con->dim; + + // dim 1: single constraint + if (dim==1) { + nc++; + } + + // dim > 1: depends on cone type + else { + nc += (ispyramid ? 2*(dim-1) : dim); + } + } + + return nc; +} + + + +// count all constraints +static inline int mj_nefc(const mjModel* m, const mjData* d) { + return mj_ne(m, d) + mj_nf(m, d) + mj_nl(m, d) + mj_nc(m, d); +} + //---------------------------- top-level API for constraint construction --------------------------- // driver: call all functions above @@ -1246,16 +1407,56 @@ void mj_makeConstraint(const mjModel* m, mjData* d) { d->ne = d->nf = d->nefc = 0; // disabled or Jacobian not allocated: return - if (mjDISABLED(mjDSBL_CONSTRAINT) || m->njmax==0) { + if (mjDISABLED(mjDSBL_CONSTRAINT)) { return; } + int nefc_allocated = mj_nefc(m, d); + d->nefc = nefc_allocated; + +#undef MJ_M +#define MJ_M(n) m->n +#undef MJ_D +#define MJ_D(n) d->n + + // move arena pointer to end of contact array + d->parena = d->ncon * sizeof(mjContact); + +#define X(type, name, nr, nc) \ + d->name = mj_arenaAlloc(d, sizeof(type) * (nr) * (nc), _Alignof(type)); \ + if (!d->name) { \ + mj_warning(d, mjWARN_CNSTRFULL, d->nstack * sizeof(mjtNum)); \ + d->nefc = 0; \ + return; \ + } + + MJDATA_ARENA_POINTERS_PRIMAL + if (mj_isDual(m)) { + MJDATA_ARENA_POINTERS_DUAL + } + +#undef X + +#undef MJ_M +#define MJ_M(n) n +#undef MJ_D +#define MJ_D(n) n + + d->nefc = 0; + // instantiate all elements of Jacobian mj_instantiateEquality(m, d); mj_instantiateFriction(m, d); mj_instantiateLimit(m, d); mj_instantiateContact(m, d); + if (d->nefc > nefc_allocated) { + char msg[1024]; + mjSNPRINTF( + msg, "nefc under-allocation: found nefc=%d but allocated only %d", d->nefc, nefc_allocated); + mju_error(msg); + } + // collect memory use statistics d->maxuse_con = mjMAX(d->maxuse_con, d->ncon); d->maxuse_efc = mjMAX(d->maxuse_efc, d->nefc); diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index b110a6bc..bfa34dbd 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -24,37 +24,17 @@ #include #include #include +#include "engine/engine_array_safety.h" #include "engine/engine_macro.h" #include "engine/engine_plugin.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_vfs.h" -#ifdef ADDRESS_SANITIZER - #include -#elif defined(_MSC_VER) - #define ASAN_POISON_MEMORY_REGION(addr, size) - #define ASAN_UNPOISON_MEMORY_REGION(addr, size) -#else - #define ASAN_POISON_MEMORY_REGION(addr, size) ((void)(addr), (void)(size)) - #define ASAN_UNPOISON_MEMORY_REGION(addr, size) ((void)(addr), (void)(size)) -#endif - -#ifdef MEMORY_SANITIZER - #include -#endif - #ifdef _MSC_VER #pragma warning (disable: 4305) // disable MSVC warning: truncation from 'double' to 'float' #endif -#ifndef __has_builtin -#define __has_builtin(x) 0 -#endif - -#define PTRDIFF(x, y) ((void*)(x) - (void*)(y)) - - //------------------------------ mjLROpt ----------------------------------------------------------- // set default options for length range computation @@ -260,14 +240,6 @@ void mj_defaultStatistic(mjStatistic* stat) { static const int ID = 54321; -// number of bytes to be skipped to achieve 64-byte alignment -static unsigned int SKIP(intptr_t offset) { - const unsigned int align = 64; - // compute skipped bytes - return (align - (offset % align)) % align; -} - - // count ints in mjModel static int getnint(void) { @@ -840,6 +812,13 @@ static void mj_setPtrData(const mjModel* m, mjData* d) { if (d->nbuffer != sz) { mju_error("mjData buffer size mismatch"); } + + // zero-initialize arena pointers +#define X(type, name, nr, nc) d->name = NULL; + MJDATA_ARENA_POINTERS +#undef X + + d->contact = d->arena; } @@ -859,7 +838,7 @@ static mjData* _makeData(const mjModel* m) { // compute buffer size d->nbuffer = 0; - d->buffer = d->stack = NULL; + d->buffer = d->arena = NULL; #define X(type, name, nr, nc) \ if (!safeAddToBufferSize(&offset, &d->nbuffer, sizeof(type), m->nr, nc)) { \ mju_free(d); \ @@ -880,12 +859,12 @@ static mjData* _makeData(const mjModel* m) { mju_error("Could not allocate mjData buffer"); } - // allocate stack - d->stack = (mjtNum*) mju_malloc(d->nstack * sizeof(mjtNum)); - if (!d->stack) { + // allocate arena + d->arena = mju_malloc(d->nstack * sizeof(mjtNum)); + if (!d->arena) { mju_free(d->buffer); mju_free(d); - mju_error("Could not allocate mjData stack"); + mju_error("Could not allocate mjData arena"); } // set pointers into buffer, reset data @@ -917,7 +896,7 @@ mjData* mj_makeData(const mjModel* m) { // copy mjData, if dest==NULL create new data mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) { void* save_buffer; - mjtNum* save_stack; + void* save_arena; // allocate new data if needed if (!dest) { @@ -939,10 +918,10 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) { // save pointers, copy everything, restore pointers save_buffer = dest->buffer; - save_stack = dest->stack; + save_arena = dest->arena; *dest = *src; dest->buffer = save_buffer; - dest->stack = save_stack; + dest->arena = save_arena; mj_setPtrData(m, dest); // save plugin_data, since the X macro copying block below will override it @@ -965,6 +944,16 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) { #undef X } + // copy arena memory + memcpy(dest->arena, src->arena, src->nstack * sizeof(mjtNum)); +#define X(type, name, nr, nc) \ + dest->name = src->name ? (type*)((char*)dest->arena + PTRDIFF(src->name, src->arena)) : NULL; + MJDATA_ARENA_POINTERS +#undef X + + // restore contact pointer + dest->contact = dest->arena; + // restore plugin_data if (plugin_data_size) { memcpy(dest->plugin_data, save_plugin_data, plugin_data_size); @@ -986,25 +975,60 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) { +// allocate memory from the mjData arena +void* mj_arenaAlloc(mjData* d, int bytes, int alignment) { + int misalignment = d->parena % alignment; + int padding = misalignment ? alignment - misalignment : 0; + + // check size + size_t bytes_available = (d->nstack - d->pstack) * sizeof(mjtNum); + if (d->parena + padding + bytes > bytes_available) { + return NULL; + } + + // allocate, update max, return pointer to buffer + void* result = (char*)d->arena + d->parena + padding; + d->parena += padding + bytes; + d->maxuse_arena = mjMAX(d->maxuse_arena, d->pstack*sizeof(mjtNum) + d->parena); + return result; +} + + + // allocate size mjtNums on the mjData stack mjtNum* mj_stackAlloc(mjData* d, int size) { - mjtNum* result; - // return NULL if empty if (!size) { return 0; } // check size - if (d->pstack + size > d->nstack) { - mju_error("Stack overflow"); + size_t stack_available_bytes = d->nstack * sizeof(mjtNum) - d->parena; + size_t stack_required_bytes = (d->pstack + size) * sizeof(mjtNum); + if (stack_required_bytes > stack_available_bytes) { + char err[256]; + mjSNPRINTF(err, "stack overflow: max = %zu, available = %zu, requested = %zu " + "(ne = %d, nf = %d, nefc = %d, ncon = %d)", + d->nstack * sizeof(mjtNum), stack_available_bytes, stack_required_bytes, + d->ne, d->nf, d->nefc, d->ncon); + mju_error(err); } - // allocate, update max, return pointer to buffer - result = (mjtNum*)d->stack + d->pstack; + // allocate at end of arena + char* end_ptr = (char*)d->arena + d->nstack * sizeof(mjtNum); + char* result = end_ptr - (d->pstack + size + 1) * sizeof(mjtNum); + +#ifdef ADDRESS_SANITIZER + if ((uintptr_t)result % sizeof(mjtNum)) { + mju_error("mj_stackAlloc fails to align to sizeof(mjtNum)"); + } +#endif + + // update max, return pointer to buffer d->pstack += size; d->maxuse_stack = mjMAX(d->maxuse_stack, d->pstack); - return result; + d->maxuse_arena = mjMAX(d->maxuse_arena, d->pstack*sizeof(mjtNum) + d->parena); + return (mjtNum*)result; } @@ -1022,8 +1046,16 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { // clear stack pointer d->pstack = 0; + // clear arena pointers + d->parena = 0; +#define X(type, name, nr, nc) d->name = NULL; + MJDATA_ARENA_POINTERS +#undef X + d->contact = d->arena; + // clear memory utilization stats d->maxuse_stack = 0; + d->maxuse_arena = 0; d->maxuse_con = 0; d->maxuse_efc = 0; @@ -1162,7 +1194,7 @@ void mj_deleteData(mjData* d) { } } mju_free(d->buffer); - mju_free(d->stack); + mju_free(d->arena); mju_free(d); } } diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index c0d18e2f..de0c73c5 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -96,6 +96,9 @@ MJAPI void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_va // reset data, set fields from specified keyframe MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key); +// mjData arena allocate +void* mj_arenaAlloc(mjData* d, int bytes, int alignment); + // mjData stack allocate MJAPI mjtNum* mj_stackAlloc(mjData* d, int size); diff --git a/src/engine/engine_macro.h b/src/engine/engine_macro.h index 4e48ca89..38996305 100644 --- a/src/engine/engine_macro.h +++ b/src/engine/engine_macro.h @@ -15,6 +15,8 @@ #ifndef MUJOCO_SRC_ENGINE_ENGINE_MACRO_H_ #define MUJOCO_SRC_ENGINE_ENGINE_MACRO_H_ +#include + #include "engine/engine_callback.h" // IWYU pragma: export //-------------------------------- utility macros -------------------------------------------------- @@ -40,4 +42,35 @@ #define TM_START1 mjtNum _tm1 = (mjcb_time ? mjcb_time() : 0); #define TM_END1(i) {d->timer[i].duration += ((mjcb_time ? mjcb_time() : 0) - _tm1); d->timer[i].number++;} +//-------------------------- sanitizer macros ------------------------------------------------------ + +#ifdef ADDRESS_SANITIZER + #include +#elif defined(_MSC_VER) + #define ASAN_POISON_MEMORY_REGION(addr, size) + #define ASAN_UNPOISON_MEMORY_REGION(addr, size) +#else + #define ASAN_POISON_MEMORY_REGION(addr, size) ((void)(addr), (void)(size)) + #define ASAN_UNPOISON_MEMORY_REGION(addr, size) ((void)(addr), (void)(size)) +#endif + +#ifdef MEMORY_SANITIZER + #include +#endif + +#ifndef __has_builtin +#define __has_builtin(x) 0 +#endif + +//-------------------------- pointer arithmetic ---------------------------------------------------- + +#define PTRDIFF(x, y) ((char*)(x) - (char*)(y)) + +// number of bytes to be skipped to achieve 64-byte alignment +static inline unsigned int SKIP(intptr_t offset) { + const unsigned int align = 64; + // compute skipped bytes + return (align - (offset % align)) % align; +} + #endif // MUJOCO_SRC_ENGINE_ENGINE_MACRO_H_ diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 4517a3ce..6af93e73 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -37,6 +37,7 @@ #define FLOAT_FORMAT "% -9.2g" #define FLOAT_FORMAT_MAX_LEN 20 #define INT_FORMAT " %d" +#define SIZE_T_FORMAT " %zu" #define NAME_FORMAT "%-21s" @@ -46,6 +47,9 @@ // print 2D array of mjtNum into file static void printArray(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; rname, int : INT_FORMAT, default : NULL); \ + 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); \ diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index ecb728d2..df7c71ec 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -956,11 +956,16 @@ const char* mju_warningText(int warning, int info) { break; case mjWARN_CONTACTFULL: - mjSNPRINTF(str, "Pre-allocated contact buffer is full. Increase nconmax above %d.", info); + mjSNPRINTF(str, + "Too many contacts. Either the arena memory is full, or nconmax is specified and is " + "exceeded. Increase arena memory allocation, or increase/remove nconmax. " + "(ncon = %d)", info); break; case mjWARN_CNSTRFULL: - mjSNPRINTF(str, "Pre-allocated constraint buffer is full. Increase njmax above %d.", info); + mjSNPRINTF(str, + "Insufficient arena memory for the number of constraints generated. " + "Increase arena memory allocation above %d bytes.", info); break; case mjWARN_VGEOMFULL: diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 2eb0881f..ed18a43e 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -118,6 +118,7 @@ mjCModel::mjCModel() { modelname = "MuJoCo Model"; mj_defaultOption(&option); mj_defaultVisual(&visual); + memory = -1; nemax = 0; njmax = -1; nconmax = -1; @@ -274,6 +275,8 @@ void mjCModel::Clear(void) { ntupledata = 0; npluginattr = 0; nnames = 0; + memory = -1; + nstack = -1; nemax = 0; nM = 0; nD = 0; @@ -1012,16 +1015,6 @@ void mjCModel::SetSizes(void) { } else { nemax += 1; } - - // nconmax - if (nconmax<0) { - nconmax = 100; - } - - // njmax - if (njmax<0) { - njmax = 500; - } } @@ -1604,7 +1597,6 @@ void mjCModel::CopyObjects(mjModel* m) { m->nemax = nemax; m->njmax = njmax; m->nconmax = nconmax; - m->nstack = nstack; m->nsensordata = nsensordata; m->nuserdata = nuserdata; @@ -2661,15 +2653,33 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { mj_setTotalmass(m, settotalmass); } - // set stack size: user-specified or conservative heuristic - if (nstack>0) { - m->nstack = nstack; + // set arena size into m->nstack + if (memory != -1) { + // memory size is user-specified in bytes, round down to nearest sizeof(mjtNum) + m->nstack = memory / sizeof(mjtNum); } else { - m->nstack = mjMAX( - 1000, - 5*(m->njmax + m->neq + m->nv)*(m->njmax + m->neq + m->nv) + - 20*(m->nq + m->nv + m->nu + m->na + m->nbody + m->njnt + - m->ngeom + m->nsite + m->neq + m->ntendon + m->nwrap)); + const int nconmax = m->nconmax == -1 ? 100 : m->nconmax; + const int njmax = m->njmax == -1 ? 500 : m->njmax; + if (nstack != -1) { + // (legacy) stack size is user-specified, already as multiple of sizeof(mjtNum) + m->nstack = nstack; + } else { + // use a conservative heuristic if neither memory nor nstack is specified in XML + m->nstack = mjMAX( + 1000, + 5*(njmax + m->neq + m->nv)*(njmax + m->neq + m->nv) + + 20*(m->nq + m->nv + m->nu + m->na + m->nbody + m->njnt + + m->ngeom + m->nsite + m->neq + m->ntendon + m->nwrap)); + } + + // add an arena space equal to memory footprint prior to the introduction of the arena + const std::size_t arena_bytes = ( + nconmax * sizeof(mjContact) + + njmax * (8 * sizeof(int) + 14 * sizeof(mjtNum)) + + m->nv * (3 * sizeof(int)) + + njmax * m->nv * (2 * sizeof(int) + 2 * sizeof(mjtNum)) + + njmax * njmax * (sizeof(int) + sizeof(mjtNum))); + m->nstack += (arena_bytes / sizeof(mjtNum)) + (arena_bytes % sizeof(mjtNum) ? 1 : 0); } // create data diff --git a/src/user/user_model.h b/src/user/user_model.h index 46c50c55..6eb6c2a5 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -148,10 +148,11 @@ class mjCModel { std::string modelname; // model name mjOption option; // options mjVisual visual; // visual options + std::size_t memory; // size of arena+stack memory in bytes int nemax; // max number of equality constraints int njmax; // max number of constraints (Jacobian rows) int nconmax; // max number of detected contacts (mjContact array size) - int nstack; // number of fields in mjData stack + int nstack; // (deprecated) number of fields in mjData stack int nuserdata; // number extra fields in mjData int nuser_body; // number of mjtNums in body_user int nuser_jnt; // number of mjtNums in jnt_user diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 6171edf4..9287ea13 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -15,11 +15,15 @@ #include "xml/xml_native_reader.h" #include +#include +#include #include #include #include #include +#include #include +#include #include #include #include @@ -103,7 +107,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "override", "energy", "fwdinv", "sensornoise", "multiccd"}, {">"}, - {"size", "*", "13", "njmax", "nconmax", "nstack", "nuserdata", "nkey", + {"size", "*", "14", "memory", "njmax", "nconmax", "nstack", "nuserdata", "nkey", "nuser_body", "nuser_jnt", "nuser_geom", "nuser_site", "nuser_cam", "nuser_tendon", "nuser_actuator", "nuser_sensor"}, @@ -978,13 +982,134 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { // size section parser void mjXReader::Size(XMLElement* section, mjCModel* mod) { + // read memory bytes + { + constexpr char err_msg[] = + "unsigned integer with an optional suffix {K,M,G,T,P,E} is expected in " + "attribute 'memory' (or the size specified is too big)"; + + auto memory = [&]() -> std::optional { + const char* pstr = section->Attribute("memory"); + if (!pstr) { + return std::nullopt; + } + + // trim entire string + std::string trimmed; + { + std::istringstream strm((std::string(pstr))); + strm >> trimmed; + std::string trailing; + strm >> trailing; + if (!trailing.empty() || !strm.eof()) { + throw mjXError(section, err_msg); + } + + // allow explicit specification of the default "-1" value + if (trimmed == "-1") { + return std::nullopt; + } + } + + std::istringstream strm(trimmed); + + // check that the number is not negative + if (strm.peek() == '-') { + throw mjXError(section, err_msg); + } + + std::size_t base_size; + strm >> base_size; + if (strm.fail()) { + // either not an integer or the number without the suffix is already bigger than size_t + throw mjXError(section, err_msg); + } + + // parse the multiplier suffix + int multiplier_bit = 0; + if (!strm.eof()) { + char suffix = strm.get(); + if (suffix == 'K' || suffix == 'k') { + multiplier_bit = 10; + } else if (suffix == 'M' || suffix == 'm') { + multiplier_bit = 20; + } else if (suffix == 'G' || suffix == 'g') { + multiplier_bit = 30; + } else if (suffix == 'T' || suffix == 't') { + multiplier_bit = 40; + } else if (suffix == 'P' || suffix == 'p') { + multiplier_bit = 50; + } else if (suffix == 'E' || suffix == 'e') { + multiplier_bit = 60; + } + + // check for invalid suffix, or suffix longer than one character + strm.get(); + if (!multiplier_bit || !strm.eof()) { + throw mjXError(section, err_msg); + } + } + + // check that the specified suffix isn't bigger than size_t + if (multiplier_bit + 1 > std::numeric_limits::digits) { + throw mjXError(section, err_msg); + } + + // check that the suffix won't take the total size beyond size_t + const std::size_t max_base_size = + (std::numeric_limits::max() << multiplier_bit) >> multiplier_bit; + if (base_size > max_base_size) { + throw mjXError(section, err_msg); + } + + const std::size_t total_size = base_size << multiplier_bit; + return total_size; + }(); + + if (memory.has_value()) { + if (*memory / sizeof(mjtNum) > std::numeric_limits::max()) { + throw mjXError(section, err_msg); + } + mod->memory = static_cast(*memory); + } + } + // read sizes - ReadAttrInt(section, "njmax", &mod->njmax); - ReadAttrInt(section, "nconmax", &mod->nconmax); - ReadAttrInt(section, "nstack", &mod->nstack); ReadAttrInt(section, "nuserdata", &mod->nuserdata); ReadAttrInt(section, "nkey", &mod->nkey); + ReadAttrInt(section, "nconmax", &mod->nconmax); + if (mod->nconmax < -1) throw mjXError(section, "nconmax must be >= -1"); + + { + int nstack = -1; + const bool has_nstack = ReadAttrInt(section, "nstack", &nstack); + if (has_nstack) { + if (mod->nstack < -1) { + throw mjXError(section, "nstack must be >= -1"); + } + if (mod->memory != -1 && nstack != -1) { + throw mjXError(section, + "either 'memory' and 'nstack' attribute can be specified, not both"); + } + mod->nstack = nstack; + } + } + { + int njmax = -1; + const bool has_njmax = ReadAttrInt(section, "njmax", &njmax); + if (has_njmax) { + if (mod->njmax < -1) { + throw mjXError(section, "njmax must be >= -1"); + } + if (mod->memory != -1 && njmax != -1) { + throw mjXError(section, + "either 'memory' and 'njmax' attribute can be specified, not both"); + } + mod->njmax = njmax; + } + } + ReadAttrInt(section, "nuser_body", &mod->nuser_body); if (mod->nuser_body < -1) throw mjXError(section, "nuser_body must be >= -1"); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 265f2595..aa89eec4 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include #include @@ -818,6 +820,32 @@ void mjXWriter::Option(XMLElement* root) { void mjXWriter::Size(XMLElement* root) { XMLElement* section = InsertEnd(root, "size"); + // write memory + if (model->memory != -1) { + const std::size_t memory = static_cast(model->memory); + const std::vector> kSuffix = { + {60, 'E'}, {50, 'P'}, {40, 'T'}, {30, 'G'}, {20, 'M'}, {10, 'K'}, + }; + + std::ostringstream strm; + + // check for divisibility by each suffixed size + for (const auto& [multiplier_bit, suffix_char] : kSuffix) { + const std::size_t multiplier = static_cast(1) << multiplier_bit; + if (memory >= multiplier && !(memory & (multiplier - 1))) { + strm << (memory >> multiplier_bit) << suffix_char; + break; + } + } + + // doesn't match any suffix, just write the number out as-is + if (!strm.tellp()) { + strm << memory; + } + + WriteAttrTxt(section, "memory", strm.str()); + } + // write sizes WriteAttrInt(section, "njmax", model->njmax, -1); WriteAttrInt(section, "nconmax", model->nconmax, -1); diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 59abbcb3..26bcee1e 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -27,7 +27,7 @@ namespace mujoco { namespace { using ::testing::ElementsAre; -using UserDataTest = MujocoTest; +using XMLReaderTest = MujocoTest; static std::vector GetRow(const mjtNum* array, int ncolumn, int row) { return std::vector(array + ncolumn * row, @@ -36,7 +36,7 @@ static std::vector GetRow(const mjtNum* array, int ncolumn, int row) { // ------------- test automatic inference of nuser_xxx ------------------------- -TEST_F(UserDataTest, AutoNUserBody) { +TEST_F(XMLReaderTest, AutoNUserBody) { static constexpr char xml[] = R"( @@ -52,7 +52,7 @@ TEST_F(UserDataTest, AutoNUserBody) { mj_deleteModel(m); } -TEST_F(UserDataTest, AutoNUserJoint) { +TEST_F(XMLReaderTest, AutoNUserJoint) { static constexpr char xml[] = R"( @@ -71,7 +71,7 @@ TEST_F(UserDataTest, AutoNUserJoint) { mj_deleteModel(m); } -TEST_F(UserDataTest, AutoNUserGeom) { +TEST_F(XMLReaderTest, AutoNUserGeom) { static constexpr char xml[] = R"( @@ -87,7 +87,7 @@ TEST_F(UserDataTest, AutoNUserGeom) { mj_deleteModel(m); } -TEST_F(UserDataTest, AutoNUserSite) { +TEST_F(XMLReaderTest, AutoNUserSite) { static constexpr char xml[] = R"( @@ -103,7 +103,7 @@ TEST_F(UserDataTest, AutoNUserSite) { mj_deleteModel(m); } -TEST_F(UserDataTest, AutoNUserCamera) { +TEST_F(XMLReaderTest, AutoNUserCamera) { static constexpr char xml[] = R"( @@ -119,7 +119,7 @@ TEST_F(UserDataTest, AutoNUserCamera) { mj_deleteModel(m); } -TEST_F(UserDataTest, AutoNUserTendon) { +TEST_F(XMLReaderTest, AutoNUserTendon) { static constexpr char xml[] = R"( @@ -145,7 +145,7 @@ TEST_F(UserDataTest, AutoNUserTendon) { mj_deleteModel(m); } -TEST_F(UserDataTest, AutoNUserActuator) { +TEST_F(XMLReaderTest, AutoNUserActuator) { static constexpr char xml[] = R"( @@ -169,7 +169,7 @@ TEST_F(UserDataTest, AutoNUserActuator) { mj_deleteModel(m); } -TEST_F(UserDataTest, AutoNUserSensor) { +TEST_F(XMLReaderTest, AutoNUserSensor) { static constexpr char xml[] = R"( diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 5acbdfc5..53d64359 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -459,9 +459,9 @@ TEST_F(ActRangeTest, ActRangeDefaultsPropagate) { // ------------- test nuser_xxx fields ----------------------------------------- -using UserDataTest = MujocoTest; +using XMLReaderTest = MujocoTest; -TEST_F(UserDataTest, NBodyTooSmall) { +TEST_F(XMLReaderTest, NBodyTooSmall) { static constexpr char xml[] = R"( @@ -476,7 +476,7 @@ TEST_F(UserDataTest, NBodyTooSmall) { EXPECT_THAT(error.data(), HasSubstr("nuser_body")); } -TEST_F(UserDataTest, NJointTooSmall) { +TEST_F(XMLReaderTest, NJointTooSmall) { static constexpr char xml[] = R"( @@ -494,7 +494,7 @@ TEST_F(UserDataTest, NJointTooSmall) { EXPECT_THAT(error.data(), HasSubstr("nuser_jnt")); } -TEST_F(UserDataTest, NGeomTooSmall) { +TEST_F(XMLReaderTest, NGeomTooSmall) { static constexpr char xml[] = R"( @@ -509,7 +509,7 @@ TEST_F(UserDataTest, NGeomTooSmall) { EXPECT_THAT(error.data(), HasSubstr("nuser_geom")); } -TEST_F(UserDataTest, NSiteTooSmall) { +TEST_F(XMLReaderTest, NSiteTooSmall) { static constexpr char xml[] = R"( @@ -524,7 +524,7 @@ TEST_F(UserDataTest, NSiteTooSmall) { EXPECT_THAT(error.data(), HasSubstr("nuser_site")); } -TEST_F(UserDataTest, NCameraTooSmall) { +TEST_F(XMLReaderTest, NCameraTooSmall) { static constexpr char xml[] = R"( @@ -539,7 +539,7 @@ TEST_F(UserDataTest, NCameraTooSmall) { EXPECT_THAT(error.data(), HasSubstr("nuser_cam")); } -TEST_F(UserDataTest, NTendonTooSmall) { +TEST_F(XMLReaderTest, NTendonTooSmall) { static constexpr char xml[] = R"( @@ -561,7 +561,7 @@ TEST_F(UserDataTest, NTendonTooSmall) { EXPECT_THAT(error.data(), HasSubstr("nuser_tendon")); } -TEST_F(UserDataTest, NActuatorTooSmall) { +TEST_F(XMLReaderTest, NActuatorTooSmall) { static constexpr char xml[] = R"( @@ -582,7 +582,7 @@ TEST_F(UserDataTest, NActuatorTooSmall) { EXPECT_THAT(error.data(), HasSubstr("nuser_actuator")); } -TEST_F(UserDataTest, NSensorTooSmall) { +TEST_F(XMLReaderTest, NSensorTooSmall) { static constexpr char xml[] = R"( diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index f88e37bb..7f2eedec 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -37,9 +37,108 @@ using ::testing::IsNan; using ::testing::IsNull; using ::testing::NotNull; -using UserDataTest = MujocoTest; +using XMLReaderTest = MujocoTest; -TEST_F(UserDataTest, InvalidNUserBody) { +TEST_F(XMLReaderTest, MemorySize) { + std::array error; + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()); + EXPECT_EQ(model->nstack, 128 / sizeof(mjtNum)); + mj_deleteModel(model); + } + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()); + EXPECT_EQ(model->nstack, 1024 / sizeof(mjtNum)); + mj_deleteModel(model); + } + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()); + EXPECT_EQ(model->nstack, 10240 / sizeof(mjtNum)); + mj_deleteModel(model); + } + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()); + EXPECT_EQ(model->nstack, 4*1024*1024 / sizeof(mjtNum)); + mj_deleteModel(model); + } + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()); + EXPECT_EQ(model->nstack, 1024*1024*1024 / sizeof(mjtNum)); + mj_deleteModel(model); + } +} + +TEST_F(XMLReaderTest, InvalidMemorySize) { + std::array error; + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()); + } + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()); + } + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()); + } + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()); + } +} + +TEST_F(XMLReaderTest, InvalidNUserBody) { static constexpr char xml[] = R"( @@ -51,7 +150,7 @@ TEST_F(UserDataTest, InvalidNUserBody) { EXPECT_THAT(error.data(), HasSubstr("nuser_body")); } -TEST_F(UserDataTest, InvalidNUserJoint) { +TEST_F(XMLReaderTest, InvalidNUserJoint) { static constexpr char xml[] = R"( @@ -63,7 +162,7 @@ TEST_F(UserDataTest, InvalidNUserJoint) { EXPECT_THAT(error.data(), HasSubstr("nuser_jnt")); } -TEST_F(UserDataTest, InvalidNUserGeom) { +TEST_F(XMLReaderTest, InvalidNUserGeom) { static constexpr char xml[] = R"( @@ -75,7 +174,7 @@ TEST_F(UserDataTest, InvalidNUserGeom) { EXPECT_THAT(error.data(), HasSubstr("nuser_geom")); } -TEST_F(UserDataTest, InvalidNUserSite) { +TEST_F(XMLReaderTest, InvalidNUserSite) { static constexpr char xml[] = R"( @@ -87,7 +186,7 @@ TEST_F(UserDataTest, InvalidNUserSite) { EXPECT_THAT(error.data(), HasSubstr("nuser_site")); } -TEST_F(UserDataTest, InvalidNUserCamera) { +TEST_F(XMLReaderTest, InvalidNUserCamera) { static constexpr char xml[] = R"( @@ -99,7 +198,7 @@ TEST_F(UserDataTest, InvalidNUserCamera) { EXPECT_THAT(error.data(), HasSubstr("nuser_cam")); } -TEST_F(UserDataTest, InvalidNUserTendon) { +TEST_F(XMLReaderTest, InvalidNUserTendon) { static constexpr char xml[] = R"( @@ -111,7 +210,7 @@ TEST_F(UserDataTest, InvalidNUserTendon) { EXPECT_THAT(error.data(), HasSubstr("nuser_tendon")); } -TEST_F(UserDataTest, InvalidNUserActuator) { +TEST_F(XMLReaderTest, InvalidNUserActuator) { static constexpr char xml[] = R"( @@ -123,7 +222,7 @@ TEST_F(UserDataTest, InvalidNUserActuator) { EXPECT_THAT(error.data(), HasSubstr("nuser_actuator")); } -TEST_F(UserDataTest, InvalidNUserSensor) { +TEST_F(XMLReaderTest, InvalidNUserSensor) { static constexpr char xml[] = R"( @@ -135,7 +234,7 @@ TEST_F(UserDataTest, InvalidNUserSensor) { EXPECT_THAT(error.data(), HasSubstr("nuser_sensor")); } -TEST_F(UserDataTest, CanParseInf) { +TEST_F(XMLReaderTest, CanParseInf) { static constexpr char xml[] = R"( @@ -158,7 +257,7 @@ TEST_F(UserDataTest, CanParseInf) { mj_deleteModel(model); } -TEST_F(UserDataTest, CanParseNanAndRaisesWarning) { +TEST_F(XMLReaderTest, CanParseNanAndRaisesWarning) { static constexpr char xml[] = R"( @@ -187,7 +286,7 @@ TEST_F(UserDataTest, CanParseNanAndRaisesWarning) { mj_deleteModel(model); } -TEST_F(UserDataTest, InvalidArrayElement) { +TEST_F(XMLReaderTest, InvalidArrayElement) { static constexpr char xml[] = R"( @@ -203,7 +302,7 @@ TEST_F(UserDataTest, InvalidArrayElement) { EXPECT_THAT(error.data(), HasSubstr("problem reading attribute 'axisangle'")); } -TEST_F(UserDataTest, InvalidArrayLength) { +TEST_F(XMLReaderTest, InvalidArrayLength) { static constexpr char xml[] = R"( @@ -219,7 +318,7 @@ TEST_F(UserDataTest, InvalidArrayLength) { EXPECT_THAT(error.data(), HasSubstr("has too much data")); } -TEST_F(UserDataTest, InvalidNumber) { +TEST_F(XMLReaderTest, InvalidNumber) { static constexpr char xml[] = R"( @@ -235,7 +334,7 @@ TEST_F(UserDataTest, InvalidNumber) { EXPECT_THAT(error.data(), HasSubstr("problem reading attribute")); } -TEST_F(UserDataTest, AllowsSpaces) { +TEST_F(XMLReaderTest, AllowsSpaces) { static constexpr char xml[] = R"( @@ -251,7 +350,7 @@ TEST_F(UserDataTest, AllowsSpaces) { mj_deleteModel(model); } -TEST_F(UserDataTest, InvalidDoubleOrientation) { +TEST_F(XMLReaderTest, InvalidDoubleOrientation) { std::string prefix = "<"; std::string suffix = "/>"; std::vector orientations = { @@ -270,15 +369,18 @@ TEST_F(UserDataTest, InvalidDoubleOrientation) { if (orient1 == orient2) continue; std::string xml = prefix + field + orient1 + orient2 + suffix; std::array error; - mjModel* model = LoadModelFromString(xml.c_str(), error.data(), error.size()); + mjModel* model = + LoadModelFromString(xml.c_str(), error.data(), error.size()); ASSERT_THAT(model, IsNull()); - EXPECT_THAT(error.data(), HasSubstr("multiple orientation specifiers for the same field")); + EXPECT_THAT( + error.data(), + HasSubstr("multiple orientation specifiers for the same field")); } } } } -TEST_F(UserDataTest, InvalidInertialOrientation) { +TEST_F(XMLReaderTest, InvalidInertialOrientation) { static constexpr char xml[] = R"( @@ -291,10 +393,11 @@ TEST_F(UserDataTest, InvalidInertialOrientation) { std::array error; mjModel* model = LoadModelFromString(xml, error.data(), error.size()); ASSERT_THAT(model, IsNull()); - EXPECT_THAT(error.data(), HasSubstr("multiple orientation specifiers for the same field")); + EXPECT_THAT(error.data(), + HasSubstr("multiple orientation specifiers for the same field")); } -TEST_F(UserDataTest, ReadShellParameter) { +TEST_F(XMLReaderTest, ReadShellParameter) { static constexpr char xml[] = R"( @@ -313,7 +416,7 @@ TEST_F(UserDataTest, ReadShellParameter) { mj_deleteModel(model); } -TEST_F(UserDataTest, ReadsDamper) { +TEST_F(XMLReaderTest, ReadsDamper) { static constexpr char xml[] = R"( @@ -339,7 +442,7 @@ TEST_F(UserDataTest, ReadsDamper) { mj_deleteModel(model); } -TEST_F(UserDataTest, RequiresPoisitiveDamping) { +TEST_F(XMLReaderTest, RequiresPoisitiveDamping) { static constexpr char xml[] = R"( @@ -356,10 +459,11 @@ TEST_F(UserDataTest, RequiresPoisitiveDamping) { std::array error; mjModel* model = LoadModelFromString(xml, error.data(), error.size()); ASSERT_THAT(model, IsNull()); - EXPECT_THAT(error.data(), HasSubstr("damping coefficient cannot be negative")); + EXPECT_THAT(error.data(), + HasSubstr("damping coefficient cannot be negative")); } -TEST_F(UserDataTest, RequiresControlRange) { +TEST_F(XMLReaderTest, RequiresControlRange) { static constexpr char xml[] = R"( @@ -379,7 +483,7 @@ TEST_F(UserDataTest, RequiresControlRange) { EXPECT_THAT(error.data(), HasSubstr("invalid control range")); } -TEST_F(UserDataTest, PositiveControlRange) { +TEST_F(XMLReaderTest, PositiveControlRange) { static constexpr char xml[] = R"( @@ -399,7 +503,7 @@ TEST_F(UserDataTest, PositiveControlRange) { EXPECT_THAT(error.data(), HasSubstr("control range cannot be negative")); } -TEST_F(UserDataTest, ReadsSkinGroups) { +TEST_F(XMLReaderTest, ReadsSkinGroups) { static constexpr char xml[] = R"( @@ -430,7 +534,7 @@ TEST_F(UserDataTest, ReadsSkinGroups) { mj_deleteModel(model); } -TEST_F(UserDataTest, InvalidSkinGroup) { +TEST_F(XMLReaderTest, InvalidSkinGroup) { static constexpr char xml[] = R"( @@ -446,7 +550,9 @@ TEST_F(UserDataTest, InvalidSkinGroup) { std::array error; mjModel* model = LoadModelFromString(xml, error.data(), error.size()); ASSERT_THAT(model, IsNull()); - EXPECT_THAT(error.data(), HasSubstr("skin group must be between 0 and 5\nElement 'skin', line 7")); + EXPECT_THAT( + error.data(), + HasSubstr("skin group must be between 0 and 5\nElement 'skin', line 7")); mj_deleteModel(model); } @@ -529,7 +635,8 @@ TEST_F(ActuatorTest, IncompleteActlimited) { std::array error; mjModel* model = LoadModelFromString(xml, error.data(), error.size()); ASSERT_THAT(model, IsNull()); - EXPECT_THAT(error.data(), HasSubstr("attribute 'actrange' does not have enough data")); + EXPECT_THAT(error.data(), + HasSubstr("attribute 'actrange' does not have enough data")); } TEST_F(ActuatorTest, ReadsByte) { @@ -762,7 +869,7 @@ TEST_F(ActuatorParseTest, DampersDontRequireRange) { mj_deleteModel(model); } -TEST_F(UserDataTest, ZnearZeroNotAllowed) { +TEST_F(XMLReaderTest, ZnearZeroNotAllowed) { static constexpr char xml[] = R"( @@ -776,7 +883,7 @@ TEST_F(UserDataTest, ZnearZeroNotAllowed) { EXPECT_THAT(error.data(), HasSubstr("znear must be strictly positive")); } -TEST_F(UserDataTest, ZnearNegativeNotAllowed) { +TEST_F(XMLReaderTest, ZnearNegativeNotAllowed) { static constexpr char xml[] = R"( @@ -790,7 +897,7 @@ TEST_F(UserDataTest, ZnearNegativeNotAllowed) { EXPECT_THAT(error.data(), HasSubstr("znear must be strictly positive")); } -TEST_F(UserDataTest, ExtentZeroNotAllowed) { +TEST_F(XMLReaderTest, ExtentZeroNotAllowed) { static constexpr char xml[] = R"( @@ -802,7 +909,7 @@ TEST_F(UserDataTest, ExtentZeroNotAllowed) { EXPECT_THAT(error.data(), HasSubstr("extent must be strictly positive")); } -TEST_F(UserDataTest, ExtentNegativeNotAllowed) { +TEST_F(XMLReaderTest, ExtentNegativeNotAllowed) { static constexpr char xml[] = R"( diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 258c48e6..75c7e742 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -52,6 +52,69 @@ using ::testing::NotNull; using XMLWriterTest = MujocoTest; +TEST_F(XMLWriterTest, SavesMemory) { + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("memory=\"1023\"")); + mj_deleteModel(model); + } + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("memory=\"1K\"")); + mj_deleteModel(model); + } + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("memory=\"4K\"")); + mj_deleteModel(model); + } + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("memory=\"1M\"")); + mj_deleteModel(model); + } + { + static constexpr char xml[] = R"( + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("memory=\"1023K\"")); + mj_deleteModel(model); + } +} + TEST_F(XMLWriterTest, SavesDisableSensor) { static constexpr char xml[] = R"( @@ -118,6 +181,7 @@ TEST_F(XMLWriterTest, NotAddsInertial) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, Not(HasSubstr("inertial"))); mj_deleteModel(model); @@ -136,6 +200,7 @@ TEST_F(XMLWriterTest, DropsInertialIfFromGeom) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, Not(HasSubstr("inertial"))); mj_deleteModel(model); @@ -154,6 +219,7 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredJointLimited) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("range=\"-1 1\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("limited=\"true\""))); @@ -173,6 +239,7 @@ TEST_F(XMLWriterTest, DoesNotKeepExplicitJointLimitedIfAutoLimits) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("autolimits=\"true\"")); EXPECT_THAT(saved_xml, HasSubstr("range=\"-1 1\"")); @@ -193,6 +260,7 @@ TEST_F(XMLWriterTest, KeepsJointLimitedFalseIfAutoLimits) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("limited=\"false\" range=\"-1 1\"")); mj_deleteModel(model); @@ -219,6 +287,7 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredTendonLimited) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("autolimits=\"true\"")); EXPECT_THAT(saved_xml, HasSubstr("range=\"-1 1\"")); @@ -247,6 +316,7 @@ TEST_F(XMLWriterTest, DoesNotKeepExplicitTendonLimitedIfAutoLimits) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("autolimits=\"true\"")); EXPECT_THAT(saved_xml, HasSubstr("range=\"-1 1\"")); @@ -275,6 +345,7 @@ TEST_F(XMLWriterTest, KeepsTendonLimitedFalseIfAutoLimits) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("limited=\"false\" range=\"-1 1\"")); mj_deleteModel(model); @@ -296,6 +367,7 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredActlimited) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("autolimits=\"true\"")); EXPECT_THAT(saved_xml, HasSubstr("actrange=\"-1 1\"")); @@ -319,6 +391,7 @@ TEST_F(XMLWriterTest, DoesNotKeepExplicitActlimitedIfAutoLimits) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("autolimits=\"true\"")); EXPECT_THAT(saved_xml, HasSubstr("actrange=\"-1 1\"")); @@ -342,6 +415,7 @@ TEST_F(XMLWriterTest, KeepsActlimitedFalse) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("actlimited=\"false\" actrange=\"-1 1\"")); mj_deleteModel(model); @@ -363,6 +437,7 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredCtrllimited) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("ctrlrange=\"-1 1\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("ctrllimited=\"true\""))); @@ -385,6 +460,7 @@ TEST_F(XMLWriterTest, DoesNotKeepExplicitCtrllimitedIfAutoLimits) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("ctrlrange=\"-1 1\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("ctrllimited=\"true\""))); @@ -406,6 +482,7 @@ TEST_F(XMLWriterTest, KeepsCtrllimitedFalse) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("ctrllimited=\"false\" ctrlrange=\"-1 1\"")); mj_deleteModel(model); @@ -427,6 +504,7 @@ TEST_F(XMLWriterTest, DoesNotKeepInferredForcelimited) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("autolimits=\"true\"")); EXPECT_THAT(saved_xml, HasSubstr("forcerange=\"-1 1\"")); @@ -449,6 +527,7 @@ TEST_F(XMLWriterTest, DoesNotKeepExplicitForcelimited) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("autolimits=\"true\"")); EXPECT_THAT(saved_xml, HasSubstr("forcerange=\"-1 1\"")); @@ -471,8 +550,10 @@ TEST_F(XMLWriterTest, KeepsForcelimitedFalse) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); - EXPECT_THAT(saved_xml, HasSubstr("forcelimited=\"false\" forcerange=\"-1 1\"")); + EXPECT_THAT(saved_xml, + HasSubstr("forcelimited=\"false\" forcerange=\"-1 1\"")); mj_deleteModel(model); } @@ -487,6 +568,7 @@ TEST_F(XMLWriterTest, UndefinedMassDensity) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, Not(HasSubstr("density"))); EXPECT_THAT(saved_xml, Not(HasSubstr("mass"))); @@ -507,6 +589,7 @@ TEST_F(XMLWriterTest, WritesDefaults) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, Not(HasSubstr("mass"))); EXPECT_THAT(saved_xml, HasSubstr("")); @@ -524,6 +607,7 @@ TEST_F(XMLWriterTest, WritesDensity) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("density=\"100\"")); EXPECT_THAT(saved_xml, Not(HasSubstr("mass"))); @@ -541,6 +625,7 @@ TEST_F(XMLWriterTest, WritesMass) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, Not(HasSubstr("density"))); EXPECT_THAT(saved_xml, HasSubstr("mass=\"0.1\"")); @@ -558,6 +643,7 @@ TEST_F(XMLWriterTest, ZeroMass) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, Not(HasSubstr("density"))); EXPECT_THAT(saved_xml, HasSubstr("mass=\"0\"")); @@ -575,6 +661,7 @@ TEST_F(XMLWriterTest, OverwritesDensity) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, Not(HasSubstr("density"))); EXPECT_THAT(saved_xml, HasSubstr("mass=\"100\"")); @@ -589,6 +676,7 @@ TEST_F(XMLWriterTest, UsesTwoSpaces) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr(" ")); EXPECT_THAT(saved_xml, Not(HasSubstr(" "))); @@ -609,9 +697,13 @@ TEST_F(XMLWriterTest, WritesSkin) { )"; mjModel* model = LoadModelFromString(xml); - mjModel* mtemp = LoadModelFromString(SaveAndReadXml(model)); + ASSERT_THAT(model, NotNull()); EXPECT_THAT(model->nskin, 1); + + mjModel* mtemp = LoadModelFromString(SaveAndReadXml(model)); + ASSERT_THAT(mtemp, NotNull()); EXPECT_THAT(mtemp->nskin, 1); + mj_deleteModel(model); mj_deleteModel(mtemp); } @@ -626,12 +718,17 @@ TEST_F(XMLWriterTest, SetPrecision) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + // save to XML and re-load, expect to lose precision mjModel* model_lo = LoadModelFromString(SaveAndReadXml(model)); + ASSERT_THAT(model_lo, NotNull()); + EXPECT_EQ(model->geom_size[1], model_lo->geom_size[1]); EXPECT_NE(model->geom_size[2], model_lo->geom_size[2]); { - // save to XML and re-load with FullFloatPrecision, expect to maintain precision + // save to XML and re-load with FullFloatPrecision + // expect to maintain precision FullFloatPrecision increase_precision; mjModel* model_hi = LoadModelFromString(SaveAndReadXml(model)); EXPECT_EQ(model->geom_size[2], model_hi->geom_size[2]); @@ -664,6 +761,7 @@ TEST_F(XMLWriterLocaleTest, IgnoresLocale) { )"; mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); std::string saved_xml = SaveAndReadXml(model); EXPECT_THAT(saved_xml, HasSubstr("0.1 1.23 2.345")); mj_deleteModel(model); @@ -675,7 +773,7 @@ TEST_F(XMLWriterLocaleTest, IgnoresLocale) { } -// ------------------------ test loading and saving multiple files --------------------------------- +// ------------------- test loading and saving multiple files ------------------ namespace mju = ::mujoco::util; static constexpr int kFieldSize = 500; @@ -695,10 +793,12 @@ template T Compare(T val1, T val2) { return error < 2*10*std::numeric_limits::epsilon() ? 0 : error; } -mjtNum CompareModel(const mjModel* m1, const mjModel* m2, char (&field)[kFieldSize]) { +mjtNum CompareModel(const mjModel* m1, const mjModel* m2, + char (&field)[kFieldSize]) { mjtNum dif, maxdif = 0.0; - // define symbols corresponding to number of columns (needed in MJMODEL_POINTERS) + // define symbols corresponding to number of columns + // (needed in MJMODEL_POINTERS) MJMODEL_POINTERS_PREAMBLE(m1); // compare ints @@ -739,7 +839,8 @@ mjtNum CompareModel(const mjModel* m1, const mjModel* m2, char (&field)[kFieldSi TEST_F(XMLWriterTest, WriteReadCompare) { FullFloatPrecision increase_precision; // Loop over all xml files in data - std::vector paths = {GetTestDataFilePath("."), GetModelPath(".")}; + std::vector paths = {GetTestDataFilePath("."), + GetModelPath(".")}; std::string ext(".xml"); for (auto const& path : paths) { for (auto &p : std::filesystem::recursive_directory_iterator(path)) { @@ -753,15 +854,18 @@ TEST_F(XMLWriterTest, WriteReadCompare) { // load model std::array error; - mjModel* m = mj_loadXML(xml.c_str(), nullptr, error.data(), error.size()); - ASSERT_THAT(m, NotNull()) << "Failed to load " << xml.c_str() << ": " << error.data(); + mjModel* m = mj_loadXML( + xml.c_str(), nullptr, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) + << "Failed to load " << xml.c_str() << ": " << error.data(); // make data mjData* d = mj_makeData(m); - ASSERT_THAT(d, testing::NotNull()) << "Failed to create data" << std::endl; + ASSERT_THAT(d, testing::NotNull()) << "Failed to create data\n"; // save and load back - mjModel* mtemp = LoadModelFromString(SaveAndReadXml(m), error.data(), error.size()); + mjModel* mtemp = + LoadModelFromString(SaveAndReadXml(m), error.data(), error.size()); if (!mtemp) { // if failing because assets are missing, accept the test @@ -770,9 +874,9 @@ TEST_F(XMLWriterTest, WriteReadCompare) { // compare and delete char field[kFieldSize] = ""; mjtNum result = CompareModel(m, mtemp, field); - EXPECT_LE(result, 0) << "Loaded and saved models are different!" << std::endl - << "Affected file " << p.path().string() << std::endl - << "Different field: " << field << std::endl; + EXPECT_LE(result, 0) << "Loaded and saved models are different!\n" + << "Affected file " << p.path().string() << '\n' + << "Different field: " << field << '\n'; mj_deleteModel(mtemp); } diff --git a/unity/Runtime/Bindings/MujocoBindings.cs b/unity/Runtime/Bindings/MujocoBindings.cs index d117fc05..d581ea32 100644 --- a/unity/Runtime/Bindings/MujocoBindings.cs +++ b/unity/Runtime/Bindings/MujocoBindings.cs @@ -549,8 +549,10 @@ public unsafe struct mjData_ { public int nstack; public int nbuffer; public int nplugin; - public int pstack; + public UIntPtr pstack; + public UIntPtr parena; public int maxuse_stack; + public UIntPtr maxuse_arena; public int maxuse_con; public int maxuse_efc; public mjWarningStat_ warning0; @@ -1584,7 +1586,7 @@ public unsafe struct mjData_ { public double time; public fixed double energy[2]; public void* buffer; - public double* stack; + public void* arena; public double* qpos; public double* qvel; public double* act; @@ -1635,30 +1637,6 @@ public unsafe struct mjData_ { public double* qLD; public double* qLDiagInv; public double* qLDiagSqrtInv; - public mjContact_* contact; - public int* efc_type; - public int* efc_id; - public int* efc_J_rownnz; - public int* efc_J_rowadr; - public int* efc_J_rowsuper; - public int* efc_J_colind; - public int* efc_JT_rownnz; - public int* efc_JT_rowadr; - public int* efc_JT_rowsuper; - public int* efc_JT_colind; - public double* efc_J; - public double* efc_JT; - public double* efc_pos; - public double* efc_margin; - public double* efc_frictionloss; - public double* efc_diagApprox; - public double* efc_KBIP; - public double* efc_D; - public double* efc_R; - public int* efc_AR_rownnz; - public int* efc_AR_rowadr; - public int* efc_AR_colind; - public double* efc_AR; public double* ten_velocity; public double* actuator_velocity; public double* cvel; @@ -1680,14 +1658,38 @@ public unsafe struct mjData_ { public double* qfrc_actuator; public double* qfrc_smooth; public double* qacc_smooth; - public double* efc_b; - public double* efc_force; - public int* efc_state; public double* qfrc_constraint; public double* qfrc_inverse; public double* cacc; public double* cfrc_int; public double* cfrc_ext; + public mjContact_* contact; + public int* efc_type; + public int* efc_id; + public int* efc_J_rownnz; + public int* efc_J_rowadr; + public int* efc_J_rowsuper; + public int* efc_J_colind; + public int* efc_JT_rownnz; + public int* efc_JT_rowadr; + public int* efc_JT_rowsuper; + public int* efc_JT_colind; + public double* efc_J; + public double* efc_JT; + public double* efc_pos; + public double* efc_margin; + public double* efc_frictionloss; + public double* efc_diagApprox; + public double* efc_KBIP; + public double* efc_D; + public double* efc_R; + public double* efc_b; + public double* efc_force; + public int* efc_state; + public int* efc_AR_rownnz; + public int* efc_AR_rowadr; + public int* efc_AR_colind; + public double* efc_AR; } [StructLayout(LayoutKind.Sequential)] @@ -3252,7 +3254,7 @@ public static unsafe extern void mju_warning_s([MarshalAs(UnmanagedType.LPStr)]s public static unsafe extern void mju_clearHandlers(); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern void* mju_malloc(uint size); +public static unsafe extern void* mju_malloc(UIntPtr size); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_free(void* ptr);