diff --git a/doc/APIreference/APIfunctions.rst b/doc/APIreference/APIfunctions.rst index acf5f274..fca904c7 100644 --- a/doc/APIreference/APIfunctions.rst +++ b/doc/APIreference/APIfunctions.rst @@ -53,12 +53,12 @@ mjMARKSTACK #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` function, and assumes that mjData\* d is defined. The use pattern is this: +: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_stackAlloc(d, 100); + mjtNum* temp = mj_stackAllocNum(d, 100); // ... use temp as needed mjFREESTACK; diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index a80982ab..897a343f 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -1232,12 +1232,12 @@ mj_resetDataKeyframe Reset data, set fields from specified keyframe. -.. _mj_stackAlloc: +.. _mj_stackAllocNum: -mj_stackAlloc -~~~~~~~~~~~~~ +mj_stackAllocNum +~~~~~~~~~~~~~~~~ -.. mujoco-include:: mj_stackAlloc +.. mujoco-include:: mj_stackAllocNum Allocate array of mjtNums on :ref:`mjData` stack. Call mju_error on stack overflow. diff --git a/doc/changelog.rst b/doc/changelog.rst index f9d0e343..7e2408d7 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -38,17 +38,18 @@ General 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. +#. Renamed the function ``mj_stackAlloc`` to ``mj_stackAllocNum``. Python bindings ^^^^^^^^^^^^^^^ -8. Fixed `#870 `__ where calling ``update_scene`` with an invalid +9. Fixed `#870 `__ where calling ``update_scene`` with an invalid camera name used the default camera. Bug fixes ^^^^^^^^^ -9. Fixed a bug that was causing the geom margins to be ignored during the midphase. +10. 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 0139c349..25f21323 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2212,7 +2212,7 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src); void mj_resetData(const mjModel* m, mjData* d); void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value); void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key); -mjtNum* mj_stackAlloc(mjData* d, int size); +mjtNum* mj_stackAllocNum(mjData* d, int size); int* mj_stackAllocInt(mjData* d, int size); void mj_deleteData(mjData* d); void mj_resetCallbacks(void); diff --git a/doc/programming/simulation.rst b/doc/programming/simulation.rst index 78d2bda5..be65bfd5 100644 --- a/doc/programming/simulation.rst +++ b/doc/programming/simulation.rst @@ -713,16 +713,17 @@ the scope. If not, saving and restoring the stack pointer should be done manuall mjMARKSTACK; // allocate space - mjtNum* myqpos = mj_stackAlloc(d, m->nq); - mjtNum* myqvel = mj_stackAlloc(d, m->nv); + mjtNum* myqpos = mj_stackAllocNum(d, m->nq); + mjtNum* myqvel = mj_stackAllocNum(d, m->nv); // restore stack from _mark mjFREESTACK; -The function :ref:`mj_stackAlloc` 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; see :ref:`diagnostics ` below. -Note that :ref:`mj_stackAlloc` is only used for allocating ``mjtNum`` arrays, the most common type of array. -:ref:`mj_stackAllocInt` is provided for integer array allocation. Allocators for other types are also possible, as in +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; +see :ref:`diagnostics ` below. Note that :ref:`mj_stackAllocNum` is only used for allocating +``mjtNum`` arrays, the most common type of array. :ref:`mj_stackAllocInt` is provided for integer array allocation. +Allocators for other types are also possible, as in `engine_collision_driver.c `__. .. _siError: diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 1ef8bb63..c3713271 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -189,7 +189,7 @@ MJAPI void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_va MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key); // Allocate array of mjtNums on mjData stack. Call mju_error on stack overflow. -MJAPI mjtNum* mj_stackAlloc(mjData* d, int size); +MJAPI mjtNum* mj_stackAllocNum(mjData* d, int size); // Allocate array of ints on mjData stack. Call mju_error on stack overflow. MJAPI int* mj_stackAllocInt(mjData* d, int size); diff --git a/introspect/functions.py b/introspect/functions.py index 15f02142..39b31343 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -677,9 +677,9 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Reset data, set fields from specified keyframe.', )), - ('mj_stackAlloc', + ('mj_stackAllocNum', FunctionDecl( - name='mj_stackAlloc', + name='mj_stackAllocNum', return_type=PointerType( inner_type=ValueType(name='mjtNum'), ), diff --git a/plugin/sensor/touch_grid.cc b/plugin/sensor/touch_grid.cc index 27bf08b0..da318f81 100644 --- a/plugin/sensor/touch_grid.cc +++ b/plugin/sensor/touch_grid.cc @@ -292,8 +292,8 @@ void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) { mjtNum* site_mat = d->site_xmat + 9*site_id; // allocate contact forces and positions - mjtNum* forces = mj_stackAlloc(d, ncon*6); - mjtNum* positions = mj_stackAlloc(d, ncon*3); + mjtNum* forces = mj_stackAllocNum(d, ncon*6); + mjtNum* positions = mj_stackAllocNum(d, ncon*3); // Get forces and positions in spherical coordinates. int contact = 0; @@ -338,12 +338,12 @@ void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) { } // Transpose forces. - mjtNum* forcesT = mj_stackAlloc(d, ncon*6); + mjtNum* forcesT = mj_stackAllocNum(d, ncon*6); mju_transpose(forcesT, forces, ncon, 6); // Allocate bin edges. - mjtNum* x_edges = mj_stackAlloc(d, size_[0] + 1); - mjtNum* y_edges = mj_stackAlloc(d, size_[1] + 1); + mjtNum* x_edges = mj_stackAllocNum(d, size_[0] + 1); + mjtNum* y_edges = mj_stackAllocNum(d, size_[1] + 1); // Make bin edges. BinEdges(x_edges, y_edges, size_, fov_, gamma_); @@ -415,8 +415,8 @@ void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, mju_mat2Quat(site_quat, site_mat); // Allocate bin edges. - mjtNum* x_edges = mj_stackAlloc(d, size_[0] + 1); - mjtNum* y_edges = mj_stackAlloc(d, size_[1] + 1); + mjtNum* x_edges = mj_stackAllocNum(d, size_[0] + 1); + mjtNum* y_edges = mj_stackAllocNum(d, size_[1] + 1); // Make bin edges. BinEdges(x_edges, y_edges, size_, fov_, gamma_); diff --git a/sample/derivative.cc b/sample/derivative.cc index ff4a3468..f15556da 100644 --- a/sample/derivative.cc +++ b/sample/derivative.cc @@ -62,8 +62,8 @@ void worker(const mjModel* m, const mjData* dmain, mjData* d, int id) { // allocate stack space for result at center mjMARKSTACK; - mjtNum* center = mj_stackAlloc(d, nv); - mjtNum* warmstart = mj_stackAlloc(d, nv); + mjtNum* center = mj_stackAllocNum(d, nv); + mjtNum* warmstart = mj_stackAllocNum(d, nv); // prepare static schedule: range of derivative columns to be computed by this thread int chunk = (m->nv + nthread-1) / nthread; @@ -223,7 +223,7 @@ void checkderiv(const mjModel* m, mjData* d, mjtNum error[7]) { // allocate space mjMARKSTACK; - mjtNum* mat = mj_stackAlloc(d, nv*nv); + mjtNum* mat = mj_stackAllocNum(d, nv*nv); // get pointers to derivative matrices mjtNum* G0 = deriv; // dinv/dpos diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 27182277..696414bf 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -732,7 +732,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) - aabb = mj_stackAlloc(d, 6*nbody); + aabb = mj_stackAllocNum(d, 6*nbody); mju_zero(aabb, 6); // construct body AABB for the aligned frame, count collidable diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index b8e010b4..7282a644 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -433,14 +433,14 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { } // allocate space - jac[0] = mj_stackAlloc(d, 6*nv); - jac[1] = mj_stackAlloc(d, 6*nv); - jacdif = mj_stackAlloc(d, 6*nv); + jac[0] = mj_stackAllocNum(d, 6*nv); + jac[1] = mj_stackAllocNum(d, 6*nv); + jacdif = mj_stackAllocNum(d, 6*nv); if (issparse) { chain = mj_stackAllocInt(d, nv); chain2 = mj_stackAllocInt(d, nv); buf_ind = mj_stackAllocInt(d, nv); - sparse_buf = mj_stackAlloc(d, nv); + sparse_buf = mj_stackAllocNum(d, nv); } // find active equality constraints @@ -641,7 +641,7 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) { } // allocate Jacobian - jac = mj_stackAlloc(d, nv); + jac = mj_stackAllocNum(d, nv); // find frictional dofs for (int i=0; i < nv; i++) { @@ -702,7 +702,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { } // allocate Jacobian - jac = mj_stackAlloc(d, nv); + jac = mj_stackAllocNum(d, nv); // find joint limits for (int i=0; i < m->njnt; i++) { @@ -847,13 +847,13 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { } // allocate Jacobian - jac = mj_stackAlloc(d, 6*NV); - jacdifp = mj_stackAlloc(d, 3*NV); - jacdifr = mj_stackAlloc(d, 3*NV); - jac1p = mj_stackAlloc(d, 3*NV); - jac2p = mj_stackAlloc(d, 3*NV); - jac1r = mj_stackAlloc(d, 3*NV); - jac2r = mj_stackAlloc(d, 3*NV); + jac = mj_stackAllocNum(d, 6*NV); + jacdifp = mj_stackAllocNum(d, 3*NV); + jacdifr = mj_stackAllocNum(d, 3*NV); + jac1p = mj_stackAllocNum(d, 3*NV); + jac2p = mj_stackAllocNum(d, 3*NV); + jac1r = mj_stackAllocNum(d, 3*NV); + jac2r = mj_stackAllocNum(d, 3*NV); if (issparse) { chain = mj_stackAllocInt(d, NV); } @@ -1755,8 +1755,8 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { } // space for backsubM2(J')' and its traspose - mjtNum* JM2 = mj_stackAlloc(d, nefc*nv); - mjtNum* JM2T = mj_stackAlloc(d, nv*nefc); + mjtNum* JM2 = mj_stackAllocNum(d, nefc*nv); + mjtNum* JM2T = mj_stackAllocNum(d, nv*nefc); // sparse if (mj_isSparse(m)) { diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 1609ab07..9b4affe7 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -179,7 +179,7 @@ void mj_kinematics(const mjModel* m, mjData* d) { void mj_comPos(const mjModel* m, mjData* d) { mjtNum offset[3], axis[3]; mjMARKSTACK; - mjtNum* mass_subtree = mj_stackAlloc(d, m->nbody); + mjtNum* mass_subtree = mj_stackAllocNum(d, m->nbody); // clear subtree mju_zero(mass_subtree, m->nbody); @@ -401,14 +401,14 @@ void mj_tendon(const mjModel* m, mjData* d) { } // allocate space - jac1 = mj_stackAlloc(d, 3*nv); - jac2 = mj_stackAlloc(d, 3*nv); - jacdif = mj_stackAlloc(d, 3*nv); - tmp = mj_stackAlloc(d, nv); + jac1 = mj_stackAllocNum(d, 3*nv); + jac2 = mj_stackAllocNum(d, 3*nv); + jacdif = mj_stackAllocNum(d, 3*nv); + tmp = mj_stackAllocNum(d, nv); if (issparse) { chain = mj_stackAllocInt(d, nv); buf_ind = mj_stackAllocInt(d, nv); - sparse_buf = mj_stackAlloc(d, nv); + sparse_buf = mj_stackAllocNum(d, nv); } // clear results @@ -629,9 +629,9 @@ void mj_transmission(const mjModel* m, mjData* d) { } // allocate space, clear moments - jac = mj_stackAlloc(d, 3*nv); - jacA = mj_stackAlloc(d, 3*nv); - jacS = mj_stackAlloc(d, 3*nv); + jac = mj_stackAllocNum(d, 3*nv); + jacA = mj_stackAllocNum(d, 3*nv); + jacS = mj_stackAllocNum(d, 3*nv); mju_zero(moment, nu*nv); // define variables required for body transmission, don't allocate @@ -804,7 +804,7 @@ void mj_transmission(const mjModel* m, mjData* d) { // reference site defined else { int refid = m->actuator_trnid[2*i+1]; - if (!jacref) jacref = mj_stackAlloc(d, 3*nv); + if (!jacref) jacref = mj_stackAllocNum(d, 3*nv); // clear length length[i] = 0; @@ -855,7 +855,7 @@ void mj_transmission(const mjModel* m, mjData* d) { mju_rotVecMat(wrench, gear+3, d->site_xmat+9*refid); // moment_tmp: global Jacobian projected on wrench, add to moment - if (!moment_tmp) moment_tmp = mj_stackAlloc(d, nv); + if (!moment_tmp) moment_tmp = mj_stackAllocNum(d, nv); mju_mulMatTVec(moment_tmp, jacS, wrench, 3, nv); mju_addTo(moment+i*nv, moment_tmp, nv); } @@ -871,11 +871,11 @@ void mj_transmission(const mjModel* m, mjData* d) { { // allocate stack variables for the first mjTRN_BODY if (!efc_force) { - efc_force = mj_stackAlloc(d, d->nefc); - moment_exclude = mj_stackAlloc(d, nv); - jacdifp = mj_stackAlloc(d, 3*nv); - jac1p = mj_stackAlloc(d, 3*nv); - jac2p = mj_stackAlloc(d, 3*nv); + efc_force = mj_stackAllocNum(d, d->nefc); + moment_exclude = mj_stackAllocNum(d, nv); + jacdifp = mj_stackAllocNum(d, 3*nv); + jac1p = mj_stackAllocNum(d, 3*nv); + jac2p = mj_stackAllocNum(d, 3*nv); chain = issparse ? mj_stackAllocInt(d, nv) : NULL; } @@ -1324,7 +1324,7 @@ void mj_comVel(const mjModel* m, mjData* d) { void mj_subtreeVel(const mjModel* m, mjData* d) { mjtNum dx[3], dv[3], dp[3], dL[3]; mjMARKSTACK; - mjtNum* body_vel = mj_stackAlloc(d, 6*m->nbody); + mjtNum* body_vel = mj_stackAllocNum(d, 6*m->nbody); // bodywise quantities for (int i=0; i < m->nbody; i++) { @@ -1390,8 +1390,8 @@ void mj_subtreeVel(const mjModel* m, mjData* d) { void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) { mjtNum tmp[6], tmp1[6]; mjMARKSTACK; - mjtNum* loc_cacc = mj_stackAlloc(d, m->nbody*6); - mjtNum* loc_cfrc_body = mj_stackAlloc(d, m->nbody*6); + mjtNum* loc_cacc = mj_stackAllocNum(d, m->nbody*6); + mjtNum* loc_cfrc_body = mj_stackAllocNum(d, m->nbody*6); // set world acceleration to -gravity mju_zero(loc_cacc, 6); diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 1710bdb8..932fb8a5 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -400,11 +400,11 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) { mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6]; mjMARKSTACK; - mjtNum* Dcvel = mj_stackAlloc(d, nbody*6*nv); - mjtNum* Dcdofdot = mj_stackAlloc(d, nv*6*nv); - mjtNum* Dcacc = mj_stackAlloc(d, nbody*6*nv); - mjtNum* Dcfrcbody = mj_stackAlloc(d, nbody*6*nv); - mjtNum* row = mj_stackAlloc(d, nv); + mjtNum* Dcvel = mj_stackAllocNum(d, nbody*6*nv); + mjtNum* Dcdofdot = mj_stackAllocNum(d, nv*6*nv); + mjtNum* Dcacc = mj_stackAllocNum(d, nbody*6*nv); + mjtNum* Dcfrcbody = mj_stackAllocNum(d, nbody*6*nv); + mjtNum* row = mj_stackAllocNum(d, nv); // compute Dcvel and Dcdofdot mjd_comVel_vel_dense(m, d, Dcvel, Dcdofdot); @@ -615,11 +615,11 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) { mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6]; mjMARKSTACK; - mjtNum* Dcdofdot = mj_stackAlloc(d, 6*m->nD); - mjtNum* Dcvel = mj_stackAlloc(d, 6*m->nB); - mjtNum* Dcacc = mj_stackAlloc(d, 6*m->nB); - mjtNum* Dcfrcbody = mj_stackAlloc(d, 6*m->nB); - mjtNum* row = mj_stackAlloc(d, nv); + mjtNum* Dcdofdot = mj_stackAllocNum(d, 6*m->nD); + mjtNum* Dcvel = mj_stackAllocNum(d, 6*m->nB); + mjtNum* Dcacc = mj_stackAllocNum(d, 6*m->nB); + mjtNum* Dcfrcbody = mj_stackAllocNum(d, 6*m->nB); + mjtNum* row = mj_stackAllocNum(d, nv); // clear mju_zero(Dcdofdot, 6*m->nD); @@ -735,7 +735,7 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum* // allocate dense row mjMARKSTACK; - mjtNum* row = mj_stackAlloc(d, nv); + mjtNum* row = mj_stackAllocNum(d, nv); // process non-zero elements of B for (int i=0; i < n; i++) { @@ -774,7 +774,7 @@ static void addJTBJSparse( // allocate row mjMARKSTACK; - mjtNum* row = mj_stackAlloc(d, nv); + mjtNum* row = mj_stackAllocNum(d, nv); // compute qDeriv(k,p) += sum_{i,j} ( J(i,k)*B(i,j)*J(j,p) ) for (int i = 0; i < n; i++) { @@ -1231,8 +1231,8 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) { int nv = m->nv; int nnz = nv; int rownnz[6], rowadr[6]; - mjtNum* J = mj_stackAlloc(d, 6*nv); - mjtNum* tmp = mj_stackAlloc(d, 3*nv); + mjtNum* J = mj_stackAllocNum(d, 6*nv); + mjtNum* tmp = mj_stackAllocNum(d, 3*nv); int* colind = mj_stackAllocInt(d, 6*nv); int* colind_compressed = mj_stackAllocInt(d, 6*nv); @@ -1342,8 +1342,8 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) int nv = m->nv; int rownnz[6], rowadr[6]; - mjtNum* J = mj_stackAlloc(d, 6*nv); - mjtNum* tmp = mj_stackAlloc(d, 3*nv); + mjtNum* J = mj_stackAllocNum(d, 6*nv); + mjtNum* tmp = mj_stackAllocNum(d, 3*nv); int* colind = mj_stackAllocInt(d, 6*nv); mjtNum lvel[6], wind[6], lwind[6], box[3], B; diff --git a/src/engine/engine_derivative_fd.c b/src/engine/engine_derivative_fd.c index cb451b43..3c874207 100644 --- a/src/engine/engine_derivative_fd.c +++ b/src/engine/engine_derivative_fd.c @@ -197,8 +197,8 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { int nv = m->nv; mjMARKSTACK; - mjtNum* qfrc_passive = mj_stackAlloc(d, nv); - mjtNum* fd = mj_stackAlloc(d, nv); + mjtNum* qfrc_passive = mj_stackAllocNum(d, nv); + mjtNum* fd = mj_stackAllocNum(d, nv); int* cnt = mj_stackAllocInt(d, nv); // clear row counters @@ -248,9 +248,9 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { int nv = m->nv; mjMARKSTACK; - mjtNum* plus = mj_stackAlloc(d, nv); - mjtNum* minus = mj_stackAlloc(d, nv); - mjtNum* fd = mj_stackAlloc(d, nv); + mjtNum* plus = mj_stackAllocNum(d, nv); + mjtNum* minus = mj_stackAllocNum(d, nv); + mjtNum* fd = mj_stackAllocNum(d, nv); int* cnt = mj_stackAllocInt(d, nv); // clear row counters @@ -332,22 +332,22 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, mjMARKSTACK; // states - mjtNum *state = mj_stackAlloc(d, nq+nv+na); // current state - mjtNum *next = mj_stackAlloc(d, nq+nv+na); // next state - mjtNum *next_plus = mj_stackAlloc(d, nq+nv+na); // forward-nudged next state - mjtNum *next_minus = mj_stackAlloc(d, nq+nv+na); // backward-nudged next state + mjtNum *state = mj_stackAllocNum(d, nq+nv+na); // current state + mjtNum *next = mj_stackAllocNum(d, nq+nv+na); // next state + mjtNum *next_plus = mj_stackAllocNum(d, nq+nv+na); // forward-nudged next state + mjtNum *next_minus = mj_stackAllocNum(d, nq+nv+na); // backward-nudged next state // warmstart accelerations - mjtNum *warmstart = mjDISABLED(mjDSBL_WARMSTART) ? NULL : mj_stackAlloc(d, nv); + mjtNum *warmstart = mjDISABLED(mjDSBL_WARMSTART) ? NULL : mj_stackAllocNum(d, nv); // sensors int skipsensor = !DsDq && !DsDv && !DsDa && !DsDu; - mjtNum *sensor = skipsensor ? NULL : mj_stackAlloc(d, ns); // sensor values - mjtNum *sensor_plus = skipsensor ? NULL : mj_stackAlloc(d, ns); // forward-nudged sensors - mjtNum *sensor_minus = skipsensor ? NULL : mj_stackAlloc(d, ns); // backward-nudged sensors + mjtNum *sensor = skipsensor ? NULL : mj_stackAllocNum(d, ns); // sensor values + mjtNum *sensor_plus = skipsensor ? NULL : mj_stackAllocNum(d, ns); // forward-nudged sensors + mjtNum *sensor_minus = skipsensor ? NULL : mj_stackAllocNum(d, ns); // backward-nudged sensors // controls - mjtNum *ctrl = mj_stackAlloc(d, nu); + mjtNum *ctrl = mj_stackAllocNum(d, nu); // save current inputs mjtNum time = d->time; @@ -508,7 +508,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, // finite-difference positions: skip=mjSTAGE_NONE if (DyDq || DsDq) { - mjtNum *dpos = mj_stackAlloc(d, nv); // allocate position perturbation + mjtNum *dpos = mj_stackAllocNum(d, nv); // allocate position perturbation for (int i=0; i < nv; i++) { // nudge forward mju_zero(dpos, nv); @@ -582,10 +582,10 @@ void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_cente mjMARKSTACK; // allocate transposed matrices - mjtNum *AT = A ? mj_stackAlloc(d, ndx*ndx) : NULL; // state-transition matrix (transposed) - mjtNum *BT = B ? mj_stackAlloc(d, nu*ndx) : NULL; // control-transition matrix (transposed) - mjtNum *CT = C ? mj_stackAlloc(d, ndx*ns) : NULL; // state-observation matrix (transposed) - mjtNum *DT = D ? mj_stackAlloc(d, nu*ns) : NULL; // control-observation matrix (transposed) + mjtNum *AT = A ? mj_stackAllocNum(d, ndx*ndx) : NULL; // state-transition matrix (transposed) + mjtNum *BT = B ? mj_stackAllocNum(d, nu*ndx) : NULL; // control-transition matrix (transposed) + mjtNum *CT = C ? mj_stackAllocNum(d, ndx*ns) : NULL; // state-observation matrix (transposed) + mjtNum *DT = D ? mj_stackAllocNum(d, nu*ns) : NULL; // control-observation matrix (transposed) // set offset pointers if (A) { @@ -648,11 +648,11 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio int skipsensor = !DsDq && !DsDv && !DsDa; // local vectors - mjtNum *pos = mj_stackAlloc(d, nq); // position - mjtNum *force = mj_stackAlloc(d, nv); // force - mjtNum *force_plus = mj_stackAlloc(d, nv); // nudged force - mjtNum *sensor = skipsensor ? NULL : mj_stackAlloc(d, ns); // sensor values - mjtNum *mass = DmDq ? mj_stackAlloc(d, nM) : NULL; // mass matrix + mjtNum *pos = mj_stackAllocNum(d, nq); // position + mjtNum *force = mj_stackAllocNum(d, nv); // force + mjtNum *force_plus = mj_stackAllocNum(d, nv); // nudged force + mjtNum *sensor = skipsensor ? NULL : mj_stackAllocNum(d, ns); // sensor values + mjtNum *mass = DmDq ? mj_stackAllocNum(d, nM) : NULL; // mass matrix // save current positions mju_copy(pos, d->qpos, nq); @@ -706,7 +706,7 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio // position: skip = mjSTAGE_NONE if (DfDq || DsDq || DmDq) { - mjtNum *dpos = mj_stackAlloc(d, nv); // allocate position perturbation + mjtNum *dpos = mj_stackAllocNum(d, nv); // allocate position perturbation for (int i=0; i < nv; i++) { // nudge mju_zero(dpos, nv); diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 9b217dfa..38f342ba 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -200,7 +200,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // local, clamped copy of ctrl mjMARKSTACK; - mjtNum *ctrl = mj_stackAlloc(d, nu); + mjtNum *ctrl = mj_stackAllocNum(d, nu); if (mjDISABLED(mjDSBL_CLAMPCTRL)) { mju_copy(ctrl, d->ctrl, nu); } else { @@ -426,7 +426,7 @@ static void warmstart(const mjModel* m, mjData* d) { // warmstart with best of (qacc_warmstart, qacc_smooth) if (!mjDISABLED(mjDSBL_WARMSTART)) { mjMARKSTACK; - mjtNum* jar = mj_stackAlloc(d, nefc); + mjtNum* jar = mj_stackAllocNum(d, nefc); // start with qacc = qacc_warmstart mju_copy(d->qacc, d->qacc_warmstart, nv); @@ -443,7 +443,7 @@ static void warmstart(const mjModel* m, mjData* d) { if (m->opt.solver == mjSOL_PGS) { // cost(force_warmstart) mjtNum PGS_warmstart = mju_dot(d->efc_force, d->efc_b, nefc); - mjtNum* ARf = mj_stackAlloc(d, nefc); + mjtNum* ARf = mj_stackAllocNum(d, nefc); if (mj_isSparse(m)) mju_mulMatVecSparse(ARf, d->efc_AR, d->efc_force, nefc, d->efc_AR_rownnz, d->efc_AR_rowadr, @@ -463,7 +463,7 @@ static void warmstart(const mjModel* m, mjData* d) { // non-PGS else { // add Gauss to cost(qacc_warmstart) - mjtNum* Ma = mj_stackAlloc(d, nv); + mjtNum* Ma = mj_stackAllocNum(d, nv); mj_mulM(m, d, Ma, d->qacc_warmstart); for (int i=0; i < nv; i++) { cost_warmstart += 0.5*(Ma[i]-d->qfrc_smooth[i])*(d->qacc_warmstart[i]-d->qacc_smooth[i]); @@ -590,8 +590,8 @@ static void mj_advance(const mjModel* m, mjData* d, void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { int nv = m->nv, nM = m->nM; mjMARKSTACK; - mjtNum* qfrc = mj_stackAlloc(d, nv); - mjtNum* qacc = mj_stackAlloc(d, nv); + mjtNum* qfrc = mj_stackAllocNum(d, nv); + mjtNum* qacc = mj_stackAllocNum(d, nv); // check for dof damping if disable flag is not set int dof_damping = 0; @@ -612,7 +612,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { // damping: integrate implicitly else { if (!skipfactor) { - mjtNum* MhB = mj_stackAlloc(d, nM); + mjtNum* MhB = mj_stackAllocNum(d, nM); // MhB = M + h*diag(B) mju_copy(MhB, d->qM, m->nM); @@ -673,10 +673,10 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { } // allocate space for intermediate solutions - dX = mj_stackAlloc(d, 2*nv+na); + dX = mj_stackAllocNum(d, 2*nv+na); for (int i=0; i < N; i++) { - X[i] = mj_stackAlloc(d, nq+nv+na); - F[i] = mj_stackAlloc(d, nv+na); + X[i] = mj_stackAllocNum(d, nq+nv+na); + F[i] = mj_stackAllocNum(d, nv+na); } // precompute C and T; C,T,A have size (N-1) @@ -756,8 +756,8 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { int nv = m->nv; mjMARKSTACK; - mjtNum* qfrc = mj_stackAlloc(d, nv); - mjtNum* qacc = mj_stackAlloc(d, nv); + mjtNum* qfrc = mj_stackAllocNum(d, nv); + mjtNum* qacc = mj_stackAllocNum(d, nv); // set qfrc = qfrc_smooth + qfrc_constraint mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv); @@ -790,7 +790,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { mjd_smooth_vel(m, d, /* flg_bias = */ 0); // modified mass matrix MhB = qDeriv[Lower] - mjtNum* MhB = mj_stackAlloc(d, m->nM); + mjtNum* MhB = mj_stackAllocNum(d, m->nM); mj_copyD2MSparse(m, d, MhB, d->qDeriv); // set MhB = M - dt*qDeriv diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index 9d9ebb01..3cfa3321 100644 --- a/src/engine/engine_inverse.c +++ b/src/engine/engine_inverse.c @@ -96,7 +96,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { mjtNum *qacc = d->qacc; mjMARKSTACK; - mjtNum* qfrc = mj_stackAlloc(d, nv); + mjtNum* qfrc = mj_stackAllocNum(d, nv); // use selected integrator switch ((mjtIntegrator) m->opt.integrator) { @@ -149,11 +149,11 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { mjd_smooth_vel(m, d, /* flg_bias = */ 0); // save mass matrix - mjtNum* qMsave = mj_stackAlloc(d, m->nM); + mjtNum* qMsave = mj_stackAllocNum(d, m->nM); mju_copy(qMsave, d->qM, m->nM); // set M = M - dt*qDeriv (reduced to M nonzeros) - mjtNum* qDerivReduced = mj_stackAlloc(d, m->nM); + mjtNum* qDerivReduced = mj_stackAllocNum(d, m->nM); mj_copyD2MSparse(m, d, qDerivReduced, d->qDeriv); mju_addToScl(d->qM, qDerivReduced, -m->opt.timestep, m->nM); @@ -186,7 +186,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) { } mjMARKSTACK; - mjtNum* jar = mj_stackAlloc(d, nefc); + mjtNum* jar = mj_stackAllocNum(d, nefc); // compute jar = Jac*qacc - aref mj_mulJacVec(m, d, jar, d->qacc); @@ -233,7 +233,7 @@ void mj_inverseSkip(const mjModel* m, mjData* d, if (mjENABLED(mjENBL_INVDISCRETE)) { // save current qacc - qacc = mj_stackAlloc(d, nv); + qacc = mj_stackAllocNum(d, nv); mju_copy(qacc, d->qacc, nv); // modify qacc in-place @@ -286,10 +286,10 @@ void mj_compareFwdInv(const mjModel* m, mjData* d) { } // allocate - qforce = mj_stackAlloc(d, nv); - dif = mj_stackAlloc(d, nv); - save_qfrc_constraint = mj_stackAlloc(d, nv); - save_efc_force = mj_stackAlloc(d, nefc); + qforce = mj_stackAllocNum(d, nv); + dif = mj_stackAllocNum(d, nv); + save_qfrc_constraint = mj_stackAllocNum(d, nv); + save_efc_force = mj_stackAllocNum(d, nefc); // qforce = qfrc_applied + J'*xfrc_applied + qfrc_actuator // should equal result of inverse dynamics diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 796cf73f..c65d2f53 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1266,7 +1266,7 @@ void* mj_stackAllocByte(mjData* d, size_t size) { return (void*)start_ptr; } -mjtNum* mj_stackAlloc(mjData* d, int size) { +mjtNum* mj_stackAllocNum(mjData* d, int size) { return (mjtNum*)mj_stackAllocByte(d, size * sizeof(mjtNum)); } diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index 5de5c594..e3647289 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -103,7 +103,7 @@ MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key); MJAPI void* mj_arenaAlloc(mjData* d, int bytes, int alignment); // mjData stack allocate for array of mjtNums -MJAPI mjtNum* mj_stackAlloc(mjData* d, int size); +MJAPI mjtNum* mj_stackAllocNum(mjData* d, int size); // mjData stack allocate for array of ints MJAPI int* mj_stackAllocInt(mjData* d, int size); diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index e6ae3c6f..9f512687 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -755,7 +755,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, } // allocate full inertia - M = mj_stackAlloc(d, m->nv*m->nv); + M = mj_stackAllocNum(d, m->nv*m->nv); #ifdef MEMORY_SANITIZER // If memory sanitizer is active, d->buffer will be marked as poisoned, even diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index 32c2c6d3..462ba914 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -1161,7 +1161,7 @@ void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* mjMARKSTACK; // allocate source - mjtNum* geom_ba = mj_stackAlloc(d, 4*m->ngeom); + mjtNum* geom_ba = mj_stackAllocNum(d, 4*m->ngeom); int* geom_eliminate = mj_stackAllocInt(d, m->ngeom); // initialize source diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 7b90ba2c..38ae6e00 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -861,7 +861,7 @@ void mj_energyVel(const mjModel* m, mjData* d) { return; } - vec = mj_stackAlloc(d, m->nv); + vec = mj_stackAllocNum(d, m->nv); // kinetic energy: 0.5 * qvel' * M * qvel mj_mulM(m, d, vec, d->qvel); diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index c8a5cdec..7313d029 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -62,8 +62,8 @@ static void set0(mjModel* m, mjData* d) { int id, id1, id2, dnum, nv = m->nv; mjtNum A[36] = {0}, pos[3], quat[4]; mjMARKSTACK; - mjtNum* jac = mj_stackAlloc(d, 6*nv); - mjtNum* tmp = mj_stackAlloc(d, 6*nv); + mjtNum* jac = mj_stackAllocNum(d, 6*nv); + mjtNum* tmp = mj_stackAllocNum(d, 6*nv); int* cammode = 0; int* lightmode = 0; @@ -284,7 +284,7 @@ static void setStat(mjModel* m, mjData* d) { mjtNum xmax[3] = {-1E+10, -1E+10, -1E+10}; mjtNum rbound; mjMARKSTACK; - mjtNum* body = mj_stackAlloc(d, m->nbody); + mjtNum* body = mj_stackAllocNum(d, m->nbody); // compute bounding box of bodies, joint centers, geoms and sites for (int i=1; i < m->nbody; i++) { diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index a3ee385f..2884616d 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 v[6], v1[6], Athis[36], Ac[25], bc[5], res[6], oldforce[6]; mjContact* con; mjMARKSTACK; - mjtNum* ARinv = mj_stackAlloc(d, nefc); + mjtNum* ARinv = mj_stackAllocNum(d, nefc); int* oldstate = mj_stackAllocInt(d, nefc); // precompute inverse diagonal of AR @@ -511,7 +511,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { mjtNum v[5], Ac[25], bc[5], res[5], oldforce[5], delta[5], mid, y, K0, K1; mjContact* con; mjMARKSTACK; - mjtNum* ARinv = mj_stackAlloc(d, nefc); + mjtNum* ARinv = mj_stackAllocNum(d, nefc); int* oldstate = mj_stackAllocInt(d, nefc); // precompute inverse diagonal of A @@ -755,20 +755,20 @@ static void CGallocate(const mjModel* m, mjData* d, memset(ctx, 0, sizeof(mjCGContext)); // common arrays - ctx->Jaref = mj_stackAlloc(d, nefc); - ctx->Jv = mj_stackAlloc(d, nefc); - ctx->Ma = mj_stackAlloc(d, nv); - ctx->Mv = mj_stackAlloc(d, nv); - ctx->grad = mj_stackAlloc(d, nv); - ctx->Mgrad = mj_stackAlloc(d, nv); - ctx->search = mj_stackAlloc(d, nv); - ctx->quad = mj_stackAlloc(d, nefc*3); + ctx->Jaref = mj_stackAllocNum(d, nefc); + ctx->Jv = mj_stackAllocNum(d, nefc); + ctx->Ma = mj_stackAllocNum(d, nv); + ctx->Mv = mj_stackAllocNum(d, nv); + ctx->grad = mj_stackAllocNum(d, nv); + ctx->Mgrad = mj_stackAllocNum(d, nv); + ctx->search = mj_stackAllocNum(d, nv); + ctx->quad = mj_stackAllocNum(d, nefc*3); // Hessian (Newton only) ctx->flg_Newton = flg_Newton; if (flg_Newton) { - ctx->H = mj_stackAlloc(d, nv*nv); - ctx->Hcone = mj_stackAlloc(d, nv*nv); + ctx->H = mj_stackAllocNum(d, nv*nv); + ctx->Hcone = mj_stackAllocNum(d, nv*nv); ctx->rownnz = mj_stackAllocInt(d, nv); ctx->rowadr = mj_stackAllocInt(d, nv); ctx->colind = mj_stackAllocInt(d, nv*nv); @@ -1279,8 +1279,8 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) { mjMARKSTACK; // storage for L'*J - mjtNum* LTJ = mj_stackAlloc(d, 6*nv); - mjtNum* LTJ_row = mj_stackAlloc(d, nv); + mjtNum* LTJ = mj_stackAllocNum(d, 6*nv); + mjtNum* LTJ_row = mj_stackAllocNum(d, nv); int* LTJ_ind = mj_stackAllocInt(d, nv); // start with Hcone = H @@ -1357,7 +1357,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) { mjMARKSTACK; // compute D corresponding to quad states - mjtNum* D = mj_stackAlloc(d, nefc); + mjtNum* D = mj_stackAllocNum(d, nefc); for (int i=0; i < nefc; i++) { if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { D[i] = d->efc_D[i]; @@ -1372,7 +1372,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) { int nnz = m->nD; // use sparse dof-dof matrix int* M_rownnz = mj_stackAllocInt(d, nv); // actual nnz count int* M_colind = mj_stackAllocInt(d, nnz); - mjtNum* M = mj_stackAlloc(d, nnz); + mjtNum* M = mj_stackAllocNum(d, nnz); mj_makeMSparse(m, d, M, M_rownnz, NULL, M_colind); // compute H = J'*D*J @@ -1446,7 +1446,7 @@ static void HessianIncremental(const mjModel* m, mjData* d, mjMARKSTACK; // local space - mjtNum* vec = mj_stackAlloc(d, nv); + mjtNum* vec = mj_stackAllocNum(d, nv); int* vec_ind = mj_stackAllocInt(d, nv); // clear update counter @@ -1521,9 +1521,9 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New // allocate local storage if (!flg_Newton) { - gradold = mj_stackAlloc(d, nv); - Mgradold = mj_stackAlloc(d, nv); - Mgraddif = mj_stackAlloc(d, nv); + gradold = mj_stackAllocNum(d, nv); + Mgradold = mj_stackAllocNum(d, nv); + Mgraddif = mj_stackAllocNum(d, nv); } int* oldstate = mj_stackAllocInt(d, nefc); diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index a1421331..e959ac00 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 void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) { int nv = m->nv; mjMARKSTACK; - mjtNum* jacp_b = mj_stackAlloc(d, 3*nv); + mjtNum* jacp_b = mj_stackAllocNum(d, 3*nv); // clear output mju_zero(jacp, 3*nv); @@ -325,8 +325,8 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA // get full Jacobian of point mjMARKSTACK; - mjtNum* jacp = (jacPoint ? jacPoint : mj_stackAlloc(d, 3*nv)); - mjtNum* jacr = mj_stackAlloc(d, 3*nv); + mjtNum* jacp = (jacPoint ? jacPoint : mj_stackAllocNum(d, 3*nv)); + mjtNum* jacr = mj_stackAllocNum(d, 3*nv); mj_jac(m, d, jacp, jacr, point, body); // jacAxis_col = cross(jacr_col, axis) @@ -957,7 +957,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, int nnz = m->nD; // use sparse dof-dof matrix int* M_rownnz = mj_stackAllocInt(d, nv); // actual nnz count int* M_colind = mj_stackAllocInt(d, nnz); - mjtNum* M = mj_stackAlloc(d, nnz); + mjtNum* M = mj_stackAllocNum(d, nnz); mj_makeMSparse(m, d, M, M_rownnz, NULL, M_colind); mj_addMSparse(m, d, dst, rownnz, rowadr, colind, M, @@ -1051,7 +1051,7 @@ void mj_addMSparse(const mjModel* m, mjData* d, mjtNum* dst, mjMARKSTACK; int* buf_ind = mj_stackAllocInt(d, nv); - mjtNum* sparse_buf = mj_stackAlloc(d, nv); + mjtNum* sparse_buf = mj_stackAllocNum(d, nv); // add to destination for (int i=0; i < nv; i++) { @@ -1162,9 +1162,9 @@ void mj_applyFT(const mjModel* m, mjData* d, // allocate local variables mjMARKSTACK; - mjtNum* jacp = mj_stackAlloc(d, 3*nv); - mjtNum* jacr = mj_stackAlloc(d, 3*nv); - mjtNum* qforce = mj_stackAlloc(d, nv); + mjtNum* jacp = mj_stackAllocNum(d, 3*nv); + mjtNum* jacr = mj_stackAllocNum(d, 3*nv); + mjtNum* qforce = mj_stackAllocNum(d, nv); // make sure body is in range if (body < 0 || body >= m->nbody) { diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index d9e9c2ad..4fdc82a9 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -150,7 +150,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, mjMARKSTACK; int* buf_ind = mj_stackAllocInt(d, n); - mjtNum* sparse_buf = mj_stackAlloc(d, n); + mjtNum* sparse_buf = mj_stackAllocNum(d, n); // shrink rows so that rownnz ends at diagonal for (int r=0; r < n; r++) { @@ -256,7 +256,7 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, mjData* d) { mjMARKSTACK; int* buf_ind = mj_stackAllocInt(d, n); - mjtNum* sparse_buf = mj_stackAlloc(d, n); + mjtNum* sparse_buf = mj_stackAllocNum(d, n); // backpass over rows corresponding to non-zero x(r) int rank = n, i = x_nnz - 1; diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index a561c4a8..c521e599 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -559,7 +559,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, mjMARKSTACK; // a dense row buffer that stores the current row in the resulting matrix - mjtNum* buffer = mj_stackAlloc(d, nc); + mjtNum* buffer = mj_stackAllocNum(d, nc); // these mark the currently set columns in the dense row buffer, // used for when creating the resulting sparse row diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 49ad50c6..5ece1d52 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -524,8 +524,8 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur int sel = pert->select; mjtNum headpos[3], forward[3], dif[3]; - mjtNum* jac = mj_stackAlloc(d, 3*nv); - mjtNum* jacM2 = mj_stackAlloc(d, 3*nv); + mjtNum* jac = mj_stackAllocNum(d, 3*nv); + mjtNum* jacM2 = mj_stackAllocNum(d, 3*nv); // invalid selected body: return if (sel <= 0 || sel >= m->nbody) { diff --git a/test/benchmark/engine_core_smooth_benchmark_test.cc b/test/benchmark/engine_core_smooth_benchmark_test.cc index fc8f72f0..4decaa13 100644 --- a/test/benchmark/engine_core_smooth_benchmark_test.cc +++ b/test/benchmark/engine_core_smooth_benchmark_test.cc @@ -112,9 +112,9 @@ static void BM_solveLD(benchmark::State& state, bool new_function) { // allocate gadient mjMARKSTACK; - mjtNum *grad = mj_stackAlloc(d, m->nv); - mjtNum *Ma = mj_stackAlloc(d, m->nv); - mjtNum *res = mj_stackAlloc(d, m->nv); + mjtNum *grad = mj_stackAllocNum(d, m->nv); + mjtNum *Ma = mj_stackAllocNum(d, m->nv); + mjtNum *res = mj_stackAllocNum(d, m->nv); // compute gradient mj_mulM(m, d, Ma, d->qacc); diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index de493ae3..f3880aa1 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -49,7 +49,7 @@ void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline( const int* colindT, const int* rowsuperT, mjData* d) { mjMARKSTACK; int* chain = mj_stackAllocInt(d, 2 * nc); - mjtNum* buffer = mj_stackAlloc(d, nc); + mjtNum* buffer = mj_stackAllocNum(d, nc); for (int r = 0; r < nc; r++) { res_rowadr[r] = r * nc; @@ -355,11 +355,11 @@ static void BM_MatVecSparse(benchmark::State& state, int unroll) { // allocate gradient mjMARKSTACK; - mjtNum *Ma = mj_stackAlloc(d, m->nv); - mjtNum *vec = mj_stackAlloc(d, m->nv); - mjtNum *res = mj_stackAlloc(d, d->nefc); - mjtNum *grad = mj_stackAlloc(d, m->nv); - mjtNum *Mgrad = mj_stackAlloc(d, m->nv); + mjtNum *Ma = mj_stackAllocNum(d, m->nv); + mjtNum *vec = mj_stackAllocNum(d, m->nv); + mjtNum *res = mj_stackAllocNum(d, d->nefc); + mjtNum *grad = mj_stackAllocNum(d, m->nv); + mjtNum *Mgrad = mj_stackAllocNum(d, m->nv); // compute gradient mj_mulM(m, d, Ma, d->qacc); @@ -434,13 +434,13 @@ static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { // allocate mjMARKSTACK; - mjtNum* H = mj_stackAlloc(d, m->nv*m->nv); + mjtNum* H = mj_stackAllocNum(d, m->nv*m->nv); int* rownnz = mj_stackAllocInt(d, m->nv); int* rowadr = mj_stackAllocInt(d, m->nv); int* colind = mj_stackAllocInt(d, m->nv*m->nv); // compute D corresponding to quad states - mjtNum* D = mj_stackAlloc(d, d->nefc); + mjtNum* D = mj_stackAllocNum(d, d->nefc); for (int i = 0; i < d->nefc; i++) { if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { D[i] = d->efc_D[i]; @@ -513,7 +513,7 @@ static void BM_transposeSparse(benchmark::State& state, TransposeFuncPtr func) { mjMARKSTACK; // need uncompressed layout - mjtNum* res = mj_stackAlloc(d, m->nv * d->nefc); + mjtNum* res = mj_stackAllocNum(d, m->nv * d->nefc); int* res_rownnz = mj_stackAllocInt(d, m->nv); int* res_rowadr = mj_stackAllocInt(d, m->nv); int* res_colind = mj_stackAllocInt(d, m->nv * d->nefc); @@ -557,13 +557,13 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { // allocate mjMARKSTACK; - mjtNum* H = mj_stackAlloc(d, m->nv * m->nv); + mjtNum* H = mj_stackAllocNum(d, m->nv * m->nv); int* rownnz = mj_stackAllocInt(d, m->nv); int* rowadr = mj_stackAllocInt(d, m->nv); int* colind = mj_stackAllocInt(d, m->nv * m->nv); // compute D corresponding to quad states - mjtNum* D = mj_stackAlloc(d, d->nefc); + mjtNum* D = mj_stackAllocNum(d, d->nefc); for (int i = 0; i < d->nefc; i++) { if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { D[i] = d->efc_D[i]; diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 67c68bed..10af8856 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -291,7 +291,7 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) { // === state-transition matrix A if (A) { - mjtNum *Ac = mj_stackAlloc(d, 2*nv*nv); + mjtNum *Ac = mj_stackAllocNum(d, 2*nv*nv); // Ac = H^-1 [diag(-stiffness) diag(-damping)] mju_zero(Ac, 2*nv*nv); for (int i=0; i < nv; i++) { @@ -321,8 +321,8 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) { // === control-transition matrix B if (B) { - mjtNum *Bc = mj_stackAlloc(d, nu*nv); - mjtNum *BcT = mj_stackAlloc(d, nv*nu); + mjtNum *Bc = mj_stackAllocNum(d, nu*nv); + mjtNum *BcT = mj_stackAllocNum(d, nv*nu); mju_copy(Bc, d->actuator_moment, nv*nu); mj_solveLD(m, Bc, nu, d->qH, d->qHDiagInv); mju_transpose(BcT, Bc, nu, nv); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 4ccc8a78..110f8d2e 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -3087,7 +3087,7 @@ public static unsafe extern void mj_resetDataDebug(mjModel_* m, mjData_* d, byte public static unsafe extern void mj_resetDataKeyframe(mjModel_* m, mjData_* d, int key); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern double* mj_stackAlloc(mjData_* d, int size); +public static unsafe extern double* mj_stackAllocNum(mjData_* d, int size); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern int* mj_stackAllocInt(mjData_* d, int size);