diff --git a/doc/APIreference/APIfunctions.rst b/doc/APIreference/APIfunctions.rst index ce8309ea..48464ae2 100644 --- a/doc/APIreference/APIfunctions.rst +++ b/doc/APIreference/APIfunctions.rst @@ -43,39 +43,6 @@ API function can be classified as: Macros ^^^^^^ - -.. _mjMARKSTACK: - -mjMARKSTACK -~~~~~~~~~~~ - -.. code-block:: C - - #define mjMARKSTACK int _mark = d->pstack; - -This macro is helpful when using the MuJoCo stack in custom computations. It works together with the next macro and the -:ref:`mj_stackAlloc` family of functions, and assumes that mjData\* d is defined. The use pattern is this: - -.. code-block:: C - - mjMARKSTACK; - mjtNum* temp = mj_stackAllocNum(d, 100); - // ... use temp as needed - mjFREESTACK; - - -.. _mjFREESTACK: - -mjFREESTACK -~~~~~~~~~~~ - -.. code-block:: C - - #define mjFREESTACK d->pstack = _mark; - -Reset the MuJoCo stack pointer to the variable \_mark, normally saved by mjMARKSTACK. - - .. _mjDISABLED: mjDISABLED diff --git a/doc/changelog.rst b/doc/changelog.rst index 2a98a3be..068db417 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,58 +5,80 @@ Changelog Upcoming version (not yet released) ----------------------------------- -General -^^^^^^^ +New features +^^^^^^^^^^^^ + +.. youtube:: Vc1tq0fFvQA + :align: right + :width: 240px + +1. Added constraint island discovery in :ref:`mj_island`. Constraint islands are disjoint sets of constraints + and degrees-of-freedom that do not interact. In a future release the constraint solver will be refactored to + exploit the disjoint structure. Island discovery can be activated using a new :ref:`enable flag` + which will be removed after the refactor. If island discovery is enabled, geoms, contacts and + tendons will be colored according to the corresponding island, see video. .. youtube:: QewlEqIZi1o :align: right :width: 240px -1. Added new signed distance field (SDF) collision primitive. SDFs can take any shape and are not constrained to be +2. Added new signed distance field (SDF) collision primitive. SDFs can take any shape and are not constrained to be convex. Collision points are found by minimizing the maximum of the two colliding SDFs via gradient descent. - Added new SDF plugin for defining implicit geometries. The plugin must define methods computing an SDF and its gradient at query points See the :ref:`documentation` for more details. - .. youtube:: Vc1tq0fFvQA - :align: right - :width: 240px +3. Added :ref:`mjThreadPool` and :ref:`mjTask` which allow for multi-threaded operations within MuJoCo engine pipeline. -#. Added constraint island discovery in :ref:`mj_island`. Constraint islands are disjoint sets of constraints - and degrees-of-freedom that do not interact. In a future release the constraint solver will be refactored to - exploit the disjoint structure. Island discovery can be activated using a new :ref:`enable flag` - which will be removed after the refactor. If island discovery is enabled, geoms, contacts and - tendons will be colored according to the corresponding island, see video. -#. Added a new :ref:`dyntype`, ``filterexact``, which updates first-order filter states with +General +^^^^^^^ + +.. admonition:: Breaking API changes + :class: attention + + 4. Removed macros ``mjMARKSTACK`` and ``mjFREESTACK``. + + .. admonition:: Migration note + :class: note + + These macros have been replaced by new functions :ref:`mj_markStack` and :ref:`mj_freeStack`. These functions + manages ``mjData`` stack frames in a fully encapsulated way (i.e. without having to introduce a local variable + at the call site). + + 5. Changed the function :ref:`mj_stackAlloc` to allocate an arbitrary number of bytes, rather than in multiples of + ``sizeof(mjtNum)``, and add an additional argument for specifying the alignment of the returned pointer. + + .. admonition:: Migration note + :class: note + + The old functionality for allocating ``mjtNum`` arrays is still available through a new function + :ref:`mj_stackAllocNum`. + + 6. Renamed the ``nstack`` field in :ref:`mjModel` and :ref:`mjData` to ``narena``. Changed ``narena``, ``pstack``, + and ``maxuse_stack`` to count number of bytes rather than number of :ref:`mjtNum` |-| s. + +7. Added a new :ref:`dyntype`, ``filterexact``, which updates first-order filter states with the exact formula rather than with Euler integration. -#. Added an actuator attribute, :ref:`actearly`, which uses semi-implicit integration for +8. Added an actuator attribute, :ref:`actearly`, which uses semi-implicit integration for actuator forces: using the next step's actuator state to compute the current actuator forces at the current timestep. -#. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to +9. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to :ref:`actuatorfrcrange` and :ref:`actuatorfrclimited`, respectively. -#. Added the flag :ref:`eulerdamp`, which disables implicit integration of joint damping in the - Euler integrator. See the :ref:`Numerical Integration` section for more details. -#. Added the flag :ref:`invdiscrete`, which enables discrete-time inverse dynamics for all - :ref:`integrators` other than ``RK4``. See the flag documentation for more details. -#. Changed the function :ref:`mj_stackAlloc` to allocate an arbitrary number of bytes, rather than in multiples of - ``sizeof(mjtNum)``, and add an additional argument for specifying the alignment of the returned pointer. The existing - functionality of allocating ``mjtNum`` arrays is still available through the new function :ref:`mj_stackAllocNum`. -#. Renamed the ``nstack`` field in :ref:`mjModel` and :ref:`mjData` to ``narena``. Changed ``narena``, ``pstack``, and - ``maxuse_stack`` to count number of bytes rather than number of :ref:`mjtNum` |-| s. -#. Added new functions ``mj_markStack`` and ``mj_freeStack`` that manages ``mjData`` stack frames in a fully - encapsulated way (i.e. without having to introduce a local variable at the call site). -#. Added :ref:`mjThreadPool` and :ref:`mjTask` which allow for multi-threaded operations within MuJoCo engine pipeline. +10. Added the flag :ref:`eulerdamp`, which disables implicit integration of joint damping in the + Euler integrator. See the :ref:`Numerical Integration` section for more details. +11. Added the flag :ref:`invdiscrete`, which enables discrete-time inverse dynamics for all + :ref:`integrators` other than ``RK4``. See the flag documentation for more details. Python bindings ^^^^^^^^^^^^^^^ -10. Fixed `#870 `__ where calling ``update_scene`` with an invalid +12. Fixed `#870 `__ where calling ``update_scene`` with an invalid camera name used the default camera. Bug fixes ^^^^^^^^^ -11. Fixed a bug that was causing the geom margins to be ignored during the midphase. +13. Fixed a bug that was causing the geom margins to be ignored during the midphase. Version 2.3.7 (July 20, 2023) diff --git a/doc/includes/references.h b/doc/includes/references.h index da9f1560..bf956b54 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2456,7 +2456,7 @@ void mju_addTo3(mjtNum res[3], const mjtNum vec[3]); void mju_subFrom3(mjtNum res[3], const mjtNum vec[3]); void mju_addToScl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl); void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtNum scl); -mjtNum mju_normalize3(mjtNum res[3]); +mjtNum mju_normalize3(mjtNum vec[3]); mjtNum mju_norm3(const mjtNum vec[3]); mjtNum mju_dot3(const mjtNum vec1[3], const mjtNum vec2[3]); mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]); @@ -2466,10 +2466,10 @@ void mju_cross(mjtNum res[3], const mjtNum a[3], const mjtNum b[3]); void mju_zero4(mjtNum res[4]); void mju_unit4(mjtNum res[4]); void mju_copy4(mjtNum res[4], const mjtNum data[4]); -mjtNum mju_normalize4(mjtNum res[4]); +mjtNum mju_normalize4(mjtNum vec[4]); void mju_zero(mjtNum* res, int n); void mju_fill(mjtNum* res, mjtNum val, int n); -void mju_copy(mjtNum* res, const mjtNum* data, int n); +void mju_copy(mjtNum* res, const mjtNum* vec, int n); mjtNum mju_sum(const mjtNum* vec, int n); mjtNum mju_L1(const mjtNum* vec, int n); void mju_scl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n); diff --git a/doc/programming/simulation.rst b/doc/programming/simulation.rst index e3a58e8b..b63ab55b 100644 --- a/doc/programming/simulation.rst +++ b/doc/programming/simulation.rst @@ -703,21 +703,19 @@ internally when an instability is detected in :ref:`mj_step`, :ref:`mj_step1` an take advantage of the custom stack, this needs to be done in-between MuJoCo calls that have the potential to reset the simulation. -Below is the general template for using the custom stack in user code. This assumes that ``mjData\* d`` is defined in -the scope. If not, saving and restoring the stack pointer should be done manually instead of using the -:ref:`mjMARKSTACK` and :ref:`mjFREESTACK` macros. +Below is the general template for using the custom stack in user code. .. code-block:: C - // save stack pointer in the "hidden" variable _mark - mjMARKSTACK; + // mark an mjData stack frame + mj_markStack(d); // allocate space mjtNum* myqpos = mj_stackAllocNum(d, m->nq); mjtNum* myqvel = mj_stackAllocNum(d, m->nv); - // restore stack from _mark - mjFREESTACK; + // restore the mjData stack frame + mj_freeStack(d); The function :ref:`mj_stackAllocNum` checks if there is enough space, and if so it advances the stack pointer, otherwise it triggers an error. It also keeps track of the maximum stack allocation; diff --git a/include/mujoco/mjmacro.h b/include/mujoco/mjmacro.h index e3972015..9ccf3042 100644 --- a/include/mujoco/mjmacro.h +++ b/include/mujoco/mjmacro.h @@ -30,10 +30,6 @@ #define mjMAX(a, b) (((a) > (b)) ? (a) : (b)) #define mjMIN(a, b) (((a) < (b)) ? (a) : (b)) -// mjData stack frame management -#define mjMARKSTACK mj_markStack(d); -#define mjFREESTACK mj_freeStack(d); - // return current value of mjOption enable/disable flags #define mjDISABLED(x) (m->opt.disableflags & (x)) #define mjENABLED(x) (m->opt.enableflags & (x)) diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index df70de12..8abb1e1f 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -895,7 +895,7 @@ MJAPI void mju_addToScl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl); MJAPI void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtNum scl); // Normalize vector, return length before normalization. -MJAPI mjtNum mju_normalize3(mjtNum res[3]); +MJAPI mjtNum mju_normalize3(mjtNum vec[3]); // Return vector length (without normalizing the vector). MJAPI mjtNum mju_norm3(const mjtNum vec[3]); @@ -925,7 +925,7 @@ MJAPI void mju_unit4(mjtNum res[4]); MJAPI void mju_copy4(mjtNum res[4], const mjtNum data[4]); // Normalize vector, return length before normalization. -MJAPI mjtNum mju_normalize4(mjtNum res[4]); +MJAPI mjtNum mju_normalize4(mjtNum vec[4]); // Set res = 0. MJAPI void mju_zero(mjtNum* res, int n); @@ -934,7 +934,7 @@ MJAPI void mju_zero(mjtNum* res, int n); MJAPI void mju_fill(mjtNum* res, mjtNum val, int n); // Set res = vec. -MJAPI void mju_copy(mjtNum* res, const mjtNum* data, int n); +MJAPI void mju_copy(mjtNum* res, const mjtNum* vec, int n); // Return sum(vec). MJAPI mjtNum mju_sum(const mjtNum* vec, int n); diff --git a/introspect/functions.py b/introspect/functions.py index cf2417d2..e5323e4c 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -5558,7 +5558,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ return_type=ValueType(name='mjtNum'), parameters=( FunctionParameterDecl( - name='res', + name='vec', type=ArrayType( inner_type=ValueType(name='mjtNum'), extents=(3,), @@ -5771,7 +5771,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ return_type=ValueType(name='mjtNum'), parameters=( FunctionParameterDecl( - name='res', + name='vec', type=ArrayType( inner_type=ValueType(name='mjtNum'), extents=(4,), @@ -5832,7 +5832,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), FunctionParameterDecl( - name='data', + name='vec', type=PointerType( inner_type=ValueType(name='mjtNum', is_const=True), ), diff --git a/plugin/sensor/touch_grid.cc b/plugin/sensor/touch_grid.cc index da318f81..4e58fee3 100644 --- a/plugin/sensor/touch_grid.cc +++ b/plugin/sensor/touch_grid.cc @@ -19,10 +19,8 @@ #include #include #include -#include -#include #include -#include +#include #include #include @@ -249,7 +247,7 @@ TouchGrid::TouchGrid(const mjModel* m, mjData* d, int instance, int nchannel, void TouchGrid::Reset(const mjModel* m, int instance) {} void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) { - mjMARKSTACK; + mj_markStack(d); // Get sensor id. int id; @@ -283,7 +281,7 @@ void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) { // No contacts, return. if (!ncon) { - mjFREESTACK; + mj_freeStack(d); return; } @@ -372,7 +370,7 @@ void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) { } } - mjFREESTACK; + mj_freeStack(d); } // Thickness of taxel-visualization boxes relative to contact distance. @@ -380,7 +378,7 @@ static const mjtNum kRelativeThickness = 0.02; void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, mjvScene* scn, int instance) { - mjMARKSTACK; + mj_markStack(d); // Get sensor id. int id; @@ -403,7 +401,7 @@ void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, // If no normal force readings, quick return. if (!maxval) { - mjFREESTACK; + mj_freeStack(d); return; } @@ -430,7 +428,7 @@ void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, } if (scn->ngeom >= scn->maxgeom) { mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom); - mjFREESTACK; + mj_freeStack(d); return; } else { // size @@ -478,7 +476,7 @@ void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, } } - mjFREESTACK; + mj_freeStack(d); } diff --git a/sample/derivative.cc b/sample/derivative.cc index f15556da..e3fcd94f 100644 --- a/sample/derivative.cc +++ b/sample/derivative.cc @@ -61,7 +61,7 @@ void worker(const mjModel* m, const mjData* dmain, mjData* d, int id) { int nv = m->nv; // allocate stack space for result at center - mjMARKSTACK; + mj_markStack(d); mjtNum* center = mj_stackAllocNum(d, nv); mjtNum* warmstart = mj_stackAllocNum(d, nv); @@ -188,7 +188,7 @@ void worker(const mjModel* m, const mjData* dmain, mjData* d, int id) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -222,7 +222,7 @@ void checkderiv(const mjModel* m, mjData* d, mjtNum error[7]) { int nv = m->nv; // allocate space - mjMARKSTACK; + mj_markStack(d); mjtNum* mat = mj_stackAllocNum(d, nv*nv); // get pointers to derivative matrices @@ -275,7 +275,7 @@ void checkderiv(const mjModel* m, mjData* d, mjtNum error[7]) { mju_addTo(mat, F0, nv*nv); error[7] = relnorm(mat, F0, nv*nv); - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index ede3a6bf..3a887c7f 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -240,7 +240,7 @@ static void collideTree(const mjModel* m, mjData* d, int b1, int b2, mjtNum offset[12]; // 2 bb x 2 bb x 3 axes (world) mjtByte initialize = 1; - mjMARKSTACK; + mj_markStack(d); // TODO(b/273737633): Store bvh max depths to make this bound tighter. const int max_stack = m->body_bvhnum[b1] + m->body_bvhnum[b2]; mjCollisionTree* stack = mj_stackAllocTree(d, max_stack); @@ -345,7 +345,7 @@ static void collideTree(const mjModel* m, mjData* d, int b1, int b2, } } } - mjFREESTACK; + mj_freeStack(d); } @@ -408,7 +408,7 @@ void mj_collision(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // predefined only; ignore exclude if (m->opt.collision == mjCOL_PAIR) { @@ -503,7 +503,7 @@ void mj_collision(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -750,7 +750,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) { mju_eig3(eigval, frame, quat, cov); // allocate AABB; clear world entry (not used) - mjMARKSTACK; + mj_markStack(d); aabb = mj_stackAllocNum(d, 6*nbody); mju_zero(aabb, 6); @@ -859,7 +859,7 @@ endbroad: mjQUICKSORT(pair, npair, sizeof(int), paircompare, 0); } - mjFREESTACK; + mj_freeStack(d); return npair; } diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index a97af9f6..65006785 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -14,6 +14,7 @@ #include "engine/engine_collision_sdf.h" +#include #include #include @@ -449,7 +450,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g, const int* child = m->bvh_child + 2*bvhadr; mjtByte* visited = d->bvh_active + bvhadr; - mjMARKSTACK; + mj_markStack(d); // TODO(quaglino): Store bvh max depths to make this bound tighter. int max_stack = m->mesh_bvhnum[m->geom_dataid[g]]; struct CollideTreeArgs_ { @@ -479,7 +480,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g, faces[*npoints] = faceid[node]; if (++(*npoints)==MAXSDFFACE) { mju_warning("mjc_MeshSDF: too many bounding volumes, some contacts may be missed"); - mjFREESTACK; + mj_freeStack(d); return; } visited[node] = 1; @@ -504,7 +505,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g, } } - mjFREESTACK; + mj_freeStack(d); } //------------------------------ collision functions ----------------------------------------------- diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index b44ae2b3..1ffe5b99 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -21,8 +21,6 @@ #include #include #include -#include "engine/engine_array_safety.h" -#include "engine/engine_crossplatform.h" #include "engine/engine_core_smooth.h" #include "engine/engine_io.h" #include "engine/engine_support.h" @@ -508,7 +506,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // allocate space jac[0] = mj_stackAllocNum(d, 6*nv); @@ -702,7 +700,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -717,7 +715,7 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // allocate Jacobian jac = mj_stackAllocNum(d, nv); @@ -763,7 +761,7 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -779,7 +777,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // allocate Jacobian jac = mj_stackAllocNum(d, nv); @@ -909,7 +907,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -925,7 +923,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // allocate Jacobian jac = mj_stackAllocNum(d, 6*NV); @@ -1019,7 +1017,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -1474,7 +1472,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) { return 0; } - mjMARKSTACK; + mj_markStack(d); if (nnz) { chain = mj_stackAllocInt(d, nv); @@ -1555,7 +1553,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) { *nnz += nnze; } - mjFREESTACK; + mj_freeStack(d); return ne; } @@ -1670,7 +1668,7 @@ static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) { return 0; } - mjMARKSTACK; + mj_markStack(d); int *chain = mj_stackAllocInt(d, m->nv); for (int i=0; i < ncon; i++) { @@ -1703,7 +1701,7 @@ static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) { *nnz += nnzc; } - mjFREESTACK; + mj_freeStack(d); return nc; } @@ -1838,7 +1836,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // space for backsubM2(J')' and its traspose mjtNum* JM2 = mj_stackAllocNum(d, nefc*nv); @@ -1978,7 +1976,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index ebd342a4..a56665c6 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -178,7 +178,7 @@ void mj_kinematics(const mjModel* m, mjData* d) { // map inertias and motion dofs to global frame centered at subtree-CoM void mj_comPos(const mjModel* m, mjData* d) { mjtNum offset[3], axis[3]; - mjMARKSTACK; + mj_markStack(d); mjtNum* mass_subtree = mj_stackAllocNum(d, m->nbody); // clear subtree @@ -261,7 +261,7 @@ void mj_comPos(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -400,7 +400,7 @@ void mj_tendon(const mjModel* m, mjData* d) { } // allocate space - mjMARKSTACK; + mj_markStack(d); jac1 = mj_stackAllocNum(d, 3*nv); jac2 = mj_stackAllocNum(d, 3*nv); jacdif = mj_stackAllocNum(d, 3*nv); @@ -609,7 +609,7 @@ void mj_tendon(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -628,7 +628,7 @@ void mj_transmission(const mjModel* m, mjData* d) { } // allocate space, clear moments - mjMARKSTACK; + mj_markStack(d); jac = mj_stackAllocNum(d, 3*nv); jacA = mj_stackAllocNum(d, 3*nv); jacS = mj_stackAllocNum(d, 3*nv); @@ -954,7 +954,7 @@ void mj_transmission(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -1383,7 +1383,7 @@ void mj_comVel(const mjModel* m, mjData* d) { // subtree linear velocity and angular momentum void mj_subtreeVel(const mjModel* m, mjData* d) { mjtNum dx[3], dv[3], dp[3], dL[3]; - mjMARKSTACK; + mj_markStack(d); mjtNum* body_vel = mj_stackAllocNum(d, 6*m->nbody); // bodywise quantities @@ -1440,7 +1440,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) { mju_addTo3(d->subtree_angmom+3*parent, dL); } - mjFREESTACK; + mj_freeStack(d); } @@ -1449,7 +1449,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) { // RNE: compute M(qpos)*qacc + C(qpos,qvel); flg_acc=0 removes inertial term void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) { mjtNum tmp[6], tmp1[6]; - mjMARKSTACK; + mj_markStack(d); mjtNum* loc_cacc = mj_stackAllocNum(d, m->nbody*6); mjtNum* loc_cfrc_body = mj_stackAllocNum(d, m->nbody*6); @@ -1495,7 +1495,7 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) { result[i] = mju_dot(d->cdof+6*i, loc_cfrc_body+6*m->dof_bodyid[i], 6); } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 932fb8a5..76a56439 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -399,7 +399,7 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) { int nv = m->nv, nbody = m->nbody; mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6]; - mjMARKSTACK; + mj_markStack(d); mjtNum* Dcvel = mj_stackAllocNum(d, nbody*6*nv); mjtNum* Dcdofdot = mj_stackAllocNum(d, nv*6*nv); mjtNum* Dcacc = mj_stackAllocNum(d, nbody*6*nv); @@ -470,7 +470,7 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -614,7 +614,7 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) { mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6]; - mjMARKSTACK; + mj_markStack(d); mjtNum* Dcdofdot = mj_stackAllocNum(d, 6*m->nD); mjtNum* Dcvel = mj_stackAllocNum(d, 6*m->nB); mjtNum* Dcacc = mj_stackAllocNum(d, 6*m->nB); @@ -687,7 +687,7 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) { mju_subFrom(d->qDeriv + Dadr[j], row, Bnnz[i]); } - mjFREESTACK; + mj_freeStack(d); } @@ -734,7 +734,7 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum* int nv = m->nv; // allocate dense row - mjMARKSTACK; + mj_markStack(d); mjtNum* row = mj_stackAllocNum(d, nv); // process non-zero elements of B @@ -760,7 +760,7 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum* } } - mjFREESTACK; + mj_freeStack(d); } @@ -773,7 +773,7 @@ static void addJTBJSparse( int nv = m->nv; // allocate row - mjMARKSTACK; + mj_markStack(d); mjtNum* row = mj_stackAllocNum(d, nv); // compute qDeriv(k,p) += sum_{i,j} ( J(i,k)*B(i,j)*J(j,p) ) @@ -808,7 +808,7 @@ static void addJTBJSparse( } // free space - mjFREESTACK; + mj_freeStack(d); } @@ -1226,7 +1226,7 @@ static inline void mjd_magnus_force( // fluid forces based on ellipsoid approximation void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) { - mjMARKSTACK; + mj_markStack(d); int nv = m->nv; int nnz = nv; @@ -1331,14 +1331,14 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) { } } - mjFREESTACK; + mj_freeStack(d); } // fluid forces based on inertia-box approximation void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) { - mjMARKSTACK; + mj_markStack(d); int nv = m->nv; int rownnz[6], rowadr[6]; @@ -1488,7 +1488,7 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) } } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_derivative_fd.c b/src/engine/engine_derivative_fd.c index e095d9de..9281955f 100644 --- a/src/engine/engine_derivative_fd.c +++ b/src/engine/engine_derivative_fd.c @@ -197,7 +197,7 @@ static void inverseSkip(const mjModel* m, mjData* d, mjtStage stage, int skipsen void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { int nv = m->nv; - mjMARKSTACK; + mj_markStack(d); mjtNum* qfrc_passive = mj_stackAllocNum(d, nv); mjtNum* fd = mj_stackAllocNum(d, nv); int* cnt = mj_stackAllocInt(d, nv); @@ -237,7 +237,7 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { // restore mj_fwdVelocity(m, d); - mjFREESTACK; + mj_freeStack(d); } @@ -248,7 +248,7 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { int nv = m->nv; - mjMARKSTACK; + mj_markStack(d); mjtNum* plus = mj_stackAllocNum(d, nv); mjtNum* minus = mj_stackAllocNum(d, nv); mjtNum* fd = mj_stackAllocNum(d, nv); @@ -303,7 +303,7 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { mj_fwdVelocity(m, d); mj_fwdActuation(m, d); - mjFREESTACK; + mj_freeStack(d); } @@ -330,7 +330,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, mjtNum* DsDq, mjtNum* DsDv, mjtNum* DsDa, mjtNum* DsDu) { int nq = m->nq, nv = m->nv, na = m->na, nu = m->nu, ns = m->nsensordata; int ndx = 2*nv+na; // row length of Dy Jacobians - mjMARKSTACK; + mj_markStack(d); // states mjtNum *state = mj_stackAllocNum(d, nq+nv+na); // current state @@ -558,7 +558,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, } } - mjFREESTACK; + mj_freeStack(d); } @@ -580,7 +580,7 @@ void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_cente mjtNum *DyDq, *DyDv, *DyDa, *DsDq, *DsDv, *DsDa; DyDq = DyDv = DyDa = DsDq = DsDv = DsDa = NULL; - mjMARKSTACK; + mj_markStack(d); // allocate transposed matrices mjtNum *AT = A ? mj_stackAllocNum(d, ndx*ndx) : NULL; // state-transition matrix (transposed) @@ -611,7 +611,7 @@ void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_cente if (C) mju_transpose(C, CT, ndx, ns); if (D) mju_transpose(D, DT, nu, ns); - mjFREESTACK; + mj_freeStack(d); } // finite differenced Jacobians of (force, sensors) = mj_inverse(state, acceleration) @@ -648,7 +648,7 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio int skipsensor = !DsDq && !DsDv && !DsDa; // local vectors - mjMARKSTACK; + mj_markStack(d); mjtNum *pos = mj_stackAllocNum(d, nq); // position mjtNum *force = mj_stackAllocNum(d, nv); // force mjtNum *force_plus = mj_stackAllocNum(d, nv); // nudged force @@ -731,5 +731,5 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio } } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 0ebe6a72..5a38c92e 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -199,7 +199,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // local, clamped copy of ctrl - mjMARKSTACK; + mj_markStack(d); mjtNum *ctrl = mj_stackAllocNum(d, nu); if (mjDISABLED(mjDSBL_CLAMPCTRL)) { mju_copy(ctrl, d->ctrl, nu); @@ -392,7 +392,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); TM_END(mjTIMER_ACTUATION); } @@ -401,7 +401,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // add up all non-constraint forces, compute qacc_smooth void mj_fwdAcceleration(const mjModel* m, mjData* d) { TM_START; - mjMARKSTACK; + mj_markStack(d); int nv = m->nv; // qforce = sum of all non-constraint forces @@ -413,7 +413,7 @@ void mj_fwdAcceleration(const mjModel* m, mjData* d) { // qacc_smooth = M \ qfr_smooth mj_solveM(m, d, d->qacc_smooth, d->qfrc_smooth, 1); - mjFREESTACK; + mj_freeStack(d); TM_END(mjTIMER_ACCELERATION); } @@ -425,7 +425,7 @@ static void warmstart(const mjModel* m, mjData* d) { // warmstart with best of (qacc_warmstart, qacc_smooth) if (!mjDISABLED(mjDSBL_WARMSTART)) { - mjMARKSTACK; + mj_markStack(d); mjtNum* jar = mj_stackAllocNum(d, nefc); // start with qacc = qacc_warmstart @@ -479,7 +479,7 @@ static void warmstart(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } // coldstart with qacc = qacc_smooth, efc_force = 0 @@ -589,7 +589,7 @@ static void mj_advance(const mjModel* m, mjData* d, // Euler integrator, semi-implicit in velocity, possibly skipping factorisation void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { int nv = m->nv, nM = m->nM; - mjMARKSTACK; + mj_markStack(d); mjtNum* qfrc = mj_stackAllocNum(d, nv); mjtNum* qacc = mj_stackAllocNum(d, nv); @@ -633,7 +633,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { // advance state and time mj_advance(m, d, d->act_dot, qacc, NULL); - mjFREESTACK; + mj_freeStack(d); } @@ -672,7 +672,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { } // allocate space for intermediate solutions - mjMARKSTACK; + mj_markStack(d); dX = mj_stackAllocNum(d, 2*nv+na); for (int i=0; i < N; i++) { X[i] = mj_stackAllocNum(d, nq+nv+na); @@ -746,7 +746,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { // advance state and time mj_advance(m, d, dX+2*nv, dX+nv, dX); - mjFREESTACK; + mj_freeStack(d); } @@ -755,7 +755,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { int nv = m->nv; - mjMARKSTACK; + mj_markStack(d); mjtNum* qfrc = mj_stackAllocNum(d, nv); mjtNum* qacc = mj_stackAllocNum(d, nv); @@ -810,7 +810,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { // advance state and time mj_advance(m, d, d->act_dot, qacc, NULL); - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index 1e1fa50c..d81b8746 100644 --- a/src/engine/engine_inverse.c +++ b/src/engine/engine_inverse.c @@ -95,7 +95,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { int nv = m->nv, dof_damping; mjtNum *qacc = d->qacc; - mjMARKSTACK; + mj_markStack(d); mjtNum* qfrc = mj_stackAllocNum(d, nv); // use selected integrator @@ -119,7 +119,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { // if disabled or no dof damping, nothing to do if (!dof_damping) { - mjFREESTACK; + mj_freeStack(d); return; } @@ -169,7 +169,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { // solve for qacc: qfrc = M * qacc mj_solveM(m, d, qacc, qfrc, 1); - mjFREESTACK; + mj_freeStack(d); } @@ -186,7 +186,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); mjtNum* jar = mj_stackAllocNum(d, nefc); // compute jar = Jac*qacc - aref @@ -196,7 +196,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) { // call update function mj_constraintUpdate(m, d, jar, NULL, 0); - mjFREESTACK; + mj_freeStack(d); TM_END(mjTIMER_CONSTRAINT); } @@ -206,7 +206,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) { void mj_inverseSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) { TM_START; - mjMARKSTACK; + mj_markStack(d); mjtNum* qacc; int nv = m->nv; @@ -259,7 +259,7 @@ void mj_inverseSkip(const mjModel* m, mjData* d, mju_copy(d->qacc, qacc, nv); } - mjFREESTACK; + mj_freeStack(d); TM_END(mjTIMER_INVERSE); } @@ -286,7 +286,7 @@ void mj_compareFwdInv(const mjModel* m, mjData* d) { } // allocate - mjMARKSTACK; + mj_markStack(d); qforce = mj_stackAllocNum(d, nv); dif = mj_stackAllocNum(d, nv); save_qfrc_constraint = mj_stackAllocNum(d, nv); @@ -314,5 +314,5 @@ void mj_compareFwdInv(const mjModel* m, mjData* d) { mju_copy(d->qfrc_constraint, save_qfrc_constraint, nv); mju_copy(d->efc_force, save_efc_force, nefc); - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index e78b4a37..21324fde 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -850,7 +850,7 @@ static void makeDSparse(const mjModel* m, mjData* d) { int* rowadr = d->D_rowadr; int* colind = d->D_colind; - mjMARKSTACK; + mj_markStack(d); int* remaining = mj_stackAllocInt(d, nv); // compute rownnz @@ -898,7 +898,7 @@ static void makeDSparse(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -943,7 +943,7 @@ static void makeBSparse(const mjModel* m, mjData* d) { } // allocate and clear incremental row counts - mjMARKSTACK; + mj_markStack(d); int* cnt = mj_stackAllocInt(d, nbody); mju_zeroInt(cnt, nbody); @@ -991,7 +991,7 @@ static void makeBSparse(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 0c6771a8..c7bba3b9 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -18,11 +18,9 @@ #include #include -#include #include #include #include "engine/engine_core_constraint.h" -#include "engine/engine_crossplatform.h" #include "engine/engine_io.h" #include "engine/engine_support.h" #include "engine/engine_util_errmem.h" @@ -414,7 +412,7 @@ void mj_island(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // allocate edge array int nedge_max = countMaxEdge(m, d); @@ -448,7 +446,7 @@ void mj_island(const mjModel* m, mjData* d) { // allocate island arrays on arena if (!arenaAllocIsland(m, d)) { - mjFREESTACK; + mj_freeStack(d); return; } @@ -527,5 +525,5 @@ void mj_island(const mjModel* m, mjData* d) { d->island_efcind[d->island_efcadr[island] + (d->island_efcnum[island]++)] = i; } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_island.h b/src/engine/engine_island.h index a724ac33..140bcdf6 100644 --- a/src/engine/engine_island.h +++ b/src/engine/engine_island.h @@ -26,7 +26,7 @@ extern "C" { // find disjoint subgraphs ("islands") given sparse symmetric adjacency matrix MJAPI int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, const int* colind, - int* scratch); + int* stack); //-------------------------- top-level API for island construction --------------------------------- diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 6e215a9c..7504f3f8 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -736,7 +736,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, } mjtNum *M; - mjMARKSTACK; + mj_markStack(d); // check format string if (!validateFloatFormat(float_format)) { @@ -755,7 +755,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, // check for nullptr if (!fp) { mju_warning("Could not open file '%s' for writing mjModel", filename); - mjFREESTACK; + mj_freeStack(d); return; } @@ -1145,7 +1145,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, fclose(fp); } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index 462ba914..9904ee7a 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -15,6 +15,7 @@ #include "engine/engine_ray.h" +#include #include #include @@ -1158,7 +1159,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, int* geomid, mjtNum* dist, int nray, mjtNum cutoff) { - mjMARKSTACK; + mj_markStack(d); // allocate source mjtNum* geom_ba = mj_stackAllocNum(d, 4*m->ngeom); @@ -1173,5 +1174,5 @@ void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* dist[i] = mju_singleRay(m, d, pnt, vec+3*i, geom_eliminate, geom_ba, geomid+i); } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 45400bbf..6be75fc9 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -860,12 +860,12 @@ void mj_energyVel(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); vec = mj_stackAllocNum(d, m->nv); // kinetic energy: 0.5 * qvel' * M * qvel mj_mulM(m, d, vec, d->qvel); d->energy[1] = 0.5*mju_dot(vec, d->qvel, m->nv); - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 7313d029..397ce15f 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -61,7 +61,7 @@ static void mj_setM0(mjModel* m, mjData* d) { static void set0(mjModel* m, mjData* d) { int id, id1, id2, dnum, nv = m->nv; mjtNum A[36] = {0}, pos[3], quat[4]; - mjMARKSTACK; + mj_markStack(d); mjtNum* jac = mj_stackAllocNum(d, 6*nv); mjtNum* tmp = mj_stackAllocNum(d, 6*nv); int* cammode = 0; @@ -264,7 +264,7 @@ static void set0(mjModel* m, mjData* d) { mju_copy3(m->light_dir0+3*i, d->light_xdir+3*i); } - mjFREESTACK; + mj_freeStack(d); } @@ -283,7 +283,7 @@ static void setStat(mjModel* m, mjData* d) { mjtNum xmin[3] = {1E+10, 1E+10, 1E+10}; mjtNum xmax[3] = {-1E+10, -1E+10, -1E+10}; mjtNum rbound; - mjMARKSTACK; + mj_markStack(d); mjtNum* body = mj_stackAllocNum(d, m->nbody); // compute bounding box of bodies, joint centers, geoms and sites @@ -383,7 +383,7 @@ static void setStat(mjModel* m, mjData* d) { m->stat.meaninertia /= m->nv; } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 8e4ab123..c0f1e313 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -309,7 +309,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { mjtNum *mu, x, denom, improvement; mjtNum v[6], v1[6], Athis[36], Ac[25], bc[5], res[6], oldforce[6]; mjContact* con; - mjMARKSTACK; + mj_markStack(d); mjtNum* ARinv = mj_stackAllocNum(d, nefc); int* oldstate = mj_stackAllocInt(d, nefc); @@ -497,7 +497,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { // map to joint space dualFinish(m, d); - mjFREESTACK; + mj_freeStack(d); } @@ -511,7 +511,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { mjtNum *mu, improvement; mjtNum v[5], Ac[25], bc[5], res[5], oldforce[5], delta[5], mid, y, K0, K1; mjContact* con; - mjMARKSTACK; + mj_markStack(d); mjtNum* ARinv = mj_stackAllocNum(d, nefc); int* oldstate = mj_stackAllocInt(d, nefc); @@ -703,7 +703,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { // map to joint space dualFinish(m, d); - mjFREESTACK; + mj_freeStack(d); } @@ -1277,7 +1277,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) { static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) { int nv = m->nv, nefc = d->nefc; mjtNum local[36]; - mjMARKSTACK; + mj_markStack(d); // storage for L'*J mjtNum* LTJ = mj_stackAllocNum(d, 6*nv); @@ -1347,7 +1347,7 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -1355,7 +1355,7 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) { // compute and factorize Hessian: direct method static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) { int nv = m->nv, nefc = d->nefc; - mjMARKSTACK; + mj_markStack(d); // compute D corresponding to quad states mjtNum* D = mj_stackAllocNum(d, nefc); @@ -1427,7 +1427,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) { ctx->nnz = nv*nv; } - mjFREESTACK; + mj_freeStack(d); // add cones if present if (ctx->ncone) { @@ -1444,7 +1444,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) { static void HessianIncremental(const mjModel* m, mjData* d, mjCGContext* ctx, const int* oldstate) { int rank, nv = m->nv, nefc = d->nefc; - mjMARKSTACK; + mj_markStack(d); // local space mjtNum* vec = mj_stackAllocNum(d, nv); @@ -1490,7 +1490,7 @@ static void HessianIncremental(const mjModel* m, mjData* d, // recompute H directly if accuracy lost if (rank < nv) { - mjFREESTACK; + mj_freeStack(d); HessianDirect(m, d, ctx); // nothing else to do @@ -1504,7 +1504,7 @@ static void HessianIncremental(const mjModel* m, mjData* d, HessianCone(m, d, ctx); } - mjFREESTACK; + mj_freeStack(d); } @@ -1515,7 +1515,7 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New mjtNum alpha, beta; mjtNum *gradold = NULL, *Mgradold = NULL, *Mgraddif = NULL; mjCGContext ctx; - mjMARKSTACK; + mj_markStack(d); // allocate context CGallocate(m, d, &ctx, flg_Newton); @@ -1625,7 +1625,7 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New d->solver_nnz = 0; } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index cf7456c3..d502a614 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -279,7 +279,7 @@ void mj_jacBodyCom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr // compute subtree-com Jacobian void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) { int nv = m->nv; - mjMARKSTACK; + mj_markStack(d); mjtNum* jacp_b = mj_stackAllocNum(d, 3*nv); // clear output @@ -300,7 +300,7 @@ void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) { // normalize by subtree mass mju_scl(jacp, jacp, 1/m->body_subtreemass[body], 3*nv); - mjFREESTACK; + mj_freeStack(d); } @@ -325,7 +325,7 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA int nv = m->nv; // get full Jacobian of point - mjMARKSTACK; + mj_markStack(d); mjtNum* jacp = (jacPoint ? jacPoint : mj_stackAllocNum(d, 3*nv)); mjtNum* jacr = mj_stackAllocNum(d, 3*nv); mj_jac(m, d, jacp, jacr, point, body); @@ -339,7 +339,7 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA } } - mjFREESTACK; + mj_freeStack(d); } @@ -1014,7 +1014,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, // sparse if (rownnz && rowadr && colind) { int nv = m->nv; - mjMARKSTACK; + mj_markStack(d); // create sparse inertia matrix M int nnz = m->nD; // use sparse dof-dof matrix int* M_rownnz = mj_stackAllocInt(d, nv); // actual nnz count @@ -1024,7 +1024,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, mj_makeMSparse(m, d, M, M_rownnz, NULL, M_colind); mj_addMSparse(m, d, dst, rownnz, rowadr, colind, M, M_rownnz, NULL, M_colind); - mjFREESTACK; + mj_freeStack(d); } // dense @@ -1111,7 +1111,7 @@ void mj_addMSparse(const mjModel* m, mjData* d, mjtNum* dst, M_rowadr = d->D_rowadr; } - mjMARKSTACK; + mj_markStack(d); int* buf_ind = mj_stackAllocInt(d, nv); mjtNum* sparse_buf = mj_stackAllocNum(d, nv); @@ -1121,7 +1121,7 @@ void mj_addMSparse(const mjModel* m, mjData* d, mjtNum* dst, rownnz[i], M_rownnz[i], colind + rowadr[i], M_colind + M_rowadr[i], sparse_buf, buf_ind); } - mjFREESTACK; + mj_freeStack(d); } @@ -1158,7 +1158,7 @@ void mj_addMDense(const mjModel* m, mjData* d, mjtNum* dst) { // dst[D] = src[M], handle different sparsity representations void mj_copyM2DSparse(const mjModel* m, mjData* d, mjtNum* dst, const mjtNum* src) { int nv = m->nv; - mjMARKSTACK; + mj_markStack(d); // init remaining int* remaining = mj_stackAllocInt(d, nv); @@ -1185,7 +1185,7 @@ void mj_copyM2DSparse(const mjModel* m, mjData* d, mjtNum* dst, const mjtNum* sr } } - mjFREESTACK; + mj_freeStack(d); } @@ -1223,7 +1223,7 @@ void mj_applyFT(const mjModel* m, mjData* d, int nv = m->nv; // allocate local variables - mjMARKSTACK; + mj_markStack(d); mjtNum* jacp = mj_stackAllocNum(d, 3*nv); mjtNum* jacr = mj_stackAllocNum(d, 3*nv); mjtNum* qforce = mj_stackAllocNum(d, nv); @@ -1246,7 +1246,7 @@ void mj_applyFT(const mjModel* m, mjData* d, mju_addTo(qfrc_target, qforce, nv); } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index 31a77a58..bb7d16f5 100644 --- a/src/engine/engine_util_blas.c +++ b/src/engine/engine_util_blas.c @@ -16,7 +16,6 @@ #include -#include #include #ifdef mjUSEPLATFORMSIMD diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index d5dafbb5..f82055a1 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -147,7 +147,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, mjData* d) { int rank = n; - mjMARKSTACK; + mj_markStack(d); int* buf_ind = mj_stackAllocInt(d, n); mjtNum* sparse_buf = mj_stackAllocNum(d, n); @@ -198,7 +198,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, } } - mjFREESTACK; + mj_freeStack(d); return rank; } @@ -253,7 +253,7 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, int* rownnz, int* rowadr, int* colind, int x_nnz, int* x_ind, mjData* d) { - mjMARKSTACK; + mj_markStack(d); int* buf_ind = mj_stackAllocInt(d, n); mjtNum* sparse_buf = mj_stackAllocNum(d, n); @@ -294,7 +294,7 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, i = i - 1 + (new_x_nnz - i); } - mjFREESTACK; + mj_freeStack(d); return rank; } diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index cde53635..193a1cb0 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -13,12 +13,11 @@ // limitations under the License. #include "engine/engine_util_sparse.h" -#include "engine/engine_util_sparse_avx.h" +#include "engine/engine_util_sparse_avx.h" // IWYU pragma: keep #include #include -#include #include #include "engine/engine_io.h" #include "engine/engine_util_blas.h" @@ -474,13 +473,12 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, mjData* d) { - mjMARKSTACK; + mj_markStack(d); int* chain = mj_stackAllocInt(d, 2*nc); int nchain = 0; int* res_colind = NULL; for (int r=0; r < nc; r++) { - // supernode; copy everything to next row if (rowsuperT && r > 0 && rowsuperT[r-1] > 0) { res_rownnz[r] = res_rownnz[r - 1]; @@ -556,7 +554,7 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, res_rowadr[r] = res_rowadr[r-1] + res_rownnz[r-1]; } - mjFREESTACK; + mj_freeStack(d); } @@ -580,7 +578,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, const int* colindT, const int* rowsuperT, mjData* d) { // allocate space for accumulation buffer and matT - mjMARKSTACK; + mj_markStack(d); // a dense row buffer that stores the current row in the resulting matrix mjtNum* buffer = mj_stackAllocNum(d, nc); @@ -689,5 +687,5 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, } } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 069dbdba..98885330 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -19,7 +19,6 @@ #include #include -#include #include #include #include "engine/engine_core_smooth.h" @@ -518,7 +517,7 @@ void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, // copy perturb pos,quat from selected body; set scale for perturbation void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPerturb* pert) { - mjMARKSTACK; + mj_markStack(d); int nv = m->nv; int sel = pert->select; @@ -529,7 +528,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur // invalid selected body: return if (sel <= 0 || sel >= m->nbody) { - mjFREESTACK; + mj_freeStack(d); return; } @@ -558,7 +557,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur mju_sub3(dif, pert->refselpos, headpos); pert->scale = mjv_frustumHeight(scn) * mju_dot3(dif, forward); - mjFREESTACK; + mj_freeStack(d); } diff --git a/test/benchmark/engine_core_smooth_benchmark_test.cc b/test/benchmark/engine_core_smooth_benchmark_test.cc index 4decaa13..80f3a474 100644 --- a/test/benchmark/engine_core_smooth_benchmark_test.cc +++ b/test/benchmark/engine_core_smooth_benchmark_test.cc @@ -105,13 +105,13 @@ static void BM_solveLD(benchmark::State& state, bool new_function) { static mjModel* m = LoadModelFromPath("composite/cloth.xml"); mjData* d = mj_makeData(m); - // warm-up rollout to get a typcal state + // warm-up rollout to get a typical state for (int i=0; i < kNumWarmupSteps; i++) { mj_step(m, d); } - // allocate gadient - mjMARKSTACK; + // allocate gradient + mj_markStack(d); mjtNum *grad = mj_stackAllocNum(d, m->nv); mjtNum *Ma = mj_stackAllocNum(d, m->nv); mjtNum *res = mj_stackAllocNum(d, m->nv); @@ -145,7 +145,7 @@ static void BM_solveLD(benchmark::State& state, bool new_function) { } // finalize - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index f3880aa1..ee95b134 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -15,6 +15,9 @@ // A benchmark for comparing different implementations of mj_solveLD. #include +#include +#include + #include #include #include @@ -47,7 +50,7 @@ void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline( const int* rownnz, const int* rowadr, const int* colind, const int* rowsuper, const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, mjData* d) { - mjMARKSTACK; + mj_markStack(d); int* chain = mj_stackAllocInt(d, 2 * nc); mjtNum* buffer = mj_stackAllocNum(d, nc); @@ -149,7 +152,7 @@ void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline( } } - mjFREESTACK; + mj_freeStack(d); } // transpose sparse matrix (uncompressed) @@ -354,7 +357,7 @@ static void BM_MatVecSparse(benchmark::State& state, int unroll) { } // allocate gradient - mjMARKSTACK; + mj_markStack(d); mjtNum *Ma = mj_stackAllocNum(d, m->nv); mjtNum *vec = mj_stackAllocNum(d, m->nv); mjtNum *res = mj_stackAllocNum(d, d->nefc); @@ -395,7 +398,7 @@ static void BM_MatVecSparse(benchmark::State& state, int unroll) { } // finalize - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } @@ -433,7 +436,7 @@ static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { } // allocate - mjMARKSTACK; + mj_markStack(d); mjtNum* H = mj_stackAllocNum(d, m->nv*m->nv); int* rownnz = mj_stackAllocInt(d, m->nv); int* rowadr = mj_stackAllocInt(d, m->nv); @@ -478,7 +481,7 @@ static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { } // finalize - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } @@ -510,7 +513,7 @@ static void BM_transposeSparse(benchmark::State& state, TransposeFuncPtr func) { mj_step(m, d); } - mjMARKSTACK; + mj_markStack(d); // need uncompressed layout mjtNum* res = mj_stackAllocNum(d, m->nv * d->nefc); @@ -524,7 +527,7 @@ static void BM_transposeSparse(benchmark::State& state, TransposeFuncPtr func) { d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); } - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } @@ -556,7 +559,7 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { } // allocate - mjMARKSTACK; + mj_markStack(d); mjtNum* H = mj_stackAllocNum(d, m->nv * m->nv); int* rownnz = mj_stackAllocInt(d, m->nv); int* rowadr = mj_stackAllocInt(d, m->nv); @@ -598,7 +601,7 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { } // finalize - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 10af8856..2b9a4702 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -14,7 +14,10 @@ // Tests for engine/engine_derivative.c. +#include +#include #include +#include #include #include @@ -287,7 +290,7 @@ TEST_F(DerivativeTest, StepSkip) { static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) { int nv = m->nv, nu = m->nu; mjtNum dt = m->opt.timestep; - mjMARKSTACK; + mj_markStack(d); // === state-transition matrix A if (A) { @@ -330,7 +333,7 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) { mju_scl(B+nu*nv, BcT, dt, nu*nv); } - mjFREESTACK; + mj_freeStack(d); } // compare FD derivatives to analytic derivatives of linear dynamical system diff --git a/test/engine/engine_util_container_test.cc b/test/engine/engine_util_container_test.cc index 42408739..26466e43 100644 --- a/test/engine/engine_util_container_test.cc +++ b/test/engine/engine_util_container_test.cc @@ -50,7 +50,7 @@ TEST(TestMjArrayList, TestMjArrayListSingleThreaded) { mjModel* m = LoadModelFromString("", error.data(), error.size()); ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error.data(); mjData* d = mj_makeData(m); - mjMARKSTACK; + mj_markStack(d); using DataType = int; constexpr int kInitialCapacity = 10; @@ -76,7 +76,7 @@ TEST(TestMjArrayList, TestMjArrayListSingleThreaded) { EXPECT_EQ(mju_arrayListAt(array_list, kNumElements), nullptr); EXPECT_EQ(mju_arrayListAt(array_list, 100), nullptr); - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); mj_deleteModel(m); } @@ -85,7 +85,7 @@ TEST(TestMjArrayList, ZeroInitialCapacity) { mjModel* m = LoadModelFromString("", nullptr, 0); ASSERT_THAT(m, NotNull()) << "Failed to load model"; mjData* d = mj_makeData(m); - mjMARKSTACK; + mj_markStack(d); mjArrayList* array_list = mju_arrayListCreate(d, sizeof(double), /*initial_capacity=*/0); EXPECT_EQ(mju_arrayListSize(array_list), 0); @@ -101,7 +101,7 @@ TEST(TestMjArrayList, ZeroInitialCapacity) { } EXPECT_EQ(mju_arrayListAt(array_list, 35), nullptr); - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); mj_deleteModel(m); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 4891f9f8..475a894d 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -3659,7 +3659,7 @@ public static unsafe extern void mju_addToScl3(double* res, double* vec, double public static unsafe extern void mju_addScl3(double* res, double* vec1, double* vec2, double scl); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern double mju_normalize3(double* res); +public static unsafe extern double mju_normalize3(double* vec); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern double mju_norm3(double* vec); @@ -3689,7 +3689,7 @@ public static unsafe extern void mju_unit4(double* res); public static unsafe extern void mju_copy4(double* res, double* data); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern double mju_normalize4(double* res); +public static unsafe extern double mju_normalize4(double* vec); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_zero(double* res, int n); @@ -3698,7 +3698,7 @@ public static unsafe extern void mju_zero(double* res, int n); public static unsafe extern void mju_fill(double* res, double val, int n); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern void mju_copy(double* res, double* data, int n); +public static unsafe extern void mju_copy(double* res, double* vec, int n); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern double mju_sum(double* vec, int n);