chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rename mj_stackAlloc to mj_stackAllocNum.

Browse Source 
PiperOrigin-RevId: 561040193
Change-Id: I49ea21ffb5b596471d2aacefb1961c11c5c905d0
This commit is contained in:
Saran Tunyasuvunakool
2023-08-29 08:42:05 -07:00
committed by Copybara-Service
parent ea5e00cad8
commit b979a26b10
31 changed files with 187 additions and 185 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/APIfunctions.rst
+2 -2
View File
@@ -53,12 +53,12 @@ mjMARKSTACK
#define mjMARKSTACK int _mark = d->pstack; #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 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 .. code-block:: C
mjMARKSTACK; mjMARKSTACK;
mjtNum* temp = mj_stackAlloc(d, 100); mjtNum* temp = mj_stackAllocNum(d, 100);
// ... use temp as needed // ... use temp as needed
mjFREESTACK; mjFREESTACK;
doc/APIreference/functions.rst
+4 -4
View File
@@ -1232,12 +1232,12 @@ mj_resetDataKeyframe
Reset data, set fields from specified keyframe. 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. Allocate array of mjtNums on :ref:`mjData` stack. Call mju_error on stack overflow.
doc/changelog.rst
+3 -2
View File
@@ -38,17 +38,18 @@ General
Euler integrator. See the :ref:`Numerical Integration<geIntegration>` section for more details. Euler integrator. See the :ref:`Numerical Integration<geIntegration>` section for more details.
#. Added the flag :ref:`invdiscrete<option-flag-invdiscrete>`, which enables discrete-time inverse dynamics for all #. Added the flag :ref:`invdiscrete<option-flag-invdiscrete>`, which enables discrete-time inverse dynamics for all
:ref:`integrators<option-integrator>` other than ``RK4``. See the flag documentation for more details. :ref:`integrators<option-integrator>` other than ``RK4``. See the flag documentation for more details.
#. Renamed the function ``mj_stackAlloc`` to ``mj_stackAllocNum``.
Python bindings Python bindings
^^^^^^^^^^^^^^^ ^^^^^^^^^^^^^^^
8. Fixed `#870 <https://github.com/deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid 9. Fixed `#870 <https://github.com/deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
camera name used the default camera. camera name used the default camera.
Bug fixes 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) Version 2.3.7 (July 20, 2023)
doc/includes/references.h
+1 -1
View File
@@ -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_resetData(const mjModel* m, mjData* d);
void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value); void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value);
void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key); 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); int* mj_stackAllocInt(mjData* d, int size);
void mj_deleteData(mjData* d); void mj_deleteData(mjData* d);
void mj_resetCallbacks(void); void mj_resetCallbacks(void);
doc/programming/simulation.rst
+7 -6
View File
@@ -713,16 +713,17 @@ the scope. If not, saving and restoring the stack pointer should be done manuall
mjMARKSTACK; mjMARKSTACK;
// allocate space // allocate space
mjtNum* myqpos = mj_stackAlloc(d, m->nq); mjtNum* myqpos = mj_stackAllocNum(d, m->nq);
mjtNum* myqvel = mj_stackAlloc(d, m->nv); mjtNum* myqvel = mj_stackAllocNum(d, m->nv);
// restore stack from _mark // restore stack from _mark
mjFREESTACK; mjFREESTACK;
The function :ref:`mj_stackAlloc` checks if there is enough space, and if so it advances the stack pointer, otherwise it The function :ref:`mj_stackAllocNum` checks if there is enough space, and if so it advances the stack pointer,
triggers an error. It also keeps track of the maximum stack allocation; see :ref:`diagnostics <siDiagnostics>` below. otherwise it triggers an error. It also keeps track of the maximum stack allocation;
Note that :ref:`mj_stackAlloc` is only used for allocating ``mjtNum`` arrays, the most common type of array. see :ref:`diagnostics <siDiagnostics>` below. Note that :ref:`mj_stackAllocNum` is only used for allocating
:ref:`mj_stackAllocInt` is provided for integer array allocation. Allocators for other types are also possible, as in ``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 <https://github.com/deepmind/mujoco/blob/main/src/engine/engine_collision_driver.c>`__. `engine_collision_driver.c <https://github.com/deepmind/mujoco/blob/main/src/engine/engine_collision_driver.c>`__.
.. _siError: .. _siError:
include/mujoco/mujoco.h
+1 -1
View File
@@ -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); MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
// Allocate array of mjtNums on mjData stack. Call mju_error on stack overflow. // 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. // Allocate array of ints on mjData stack. Call mju_error on stack overflow.
MJAPI int* mj_stackAllocInt(mjData* d, int size); MJAPI int* mj_stackAllocInt(mjData* d, int size);
introspect/functions.py
+2 -2
View File
@@ -677,9 +677,9 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
), ),
doc='Reset data, set fields from specified keyframe.', doc='Reset data, set fields from specified keyframe.',
)), )),
('mj_stackAlloc', ('mj_stackAllocNum',
FunctionDecl( FunctionDecl(
name='mj_stackAlloc', name='mj_stackAllocNum',
return_type=PointerType( return_type=PointerType(
inner_type=ValueType(name='mjtNum'), inner_type=ValueType(name='mjtNum'),
), ),
plugin/sensor/touch_grid.cc
+7 -7
View File
@@ -292,8 +292,8 @@ void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) {
mjtNum* site_mat = d->site_xmat + 9*site_id; mjtNum* site_mat = d->site_xmat + 9*site_id;
// allocate contact forces and positions // allocate contact forces and positions
mjtNum* forces = mj_stackAlloc(d, ncon*6); mjtNum* forces = mj_stackAllocNum(d, ncon*6);
mjtNum* positions = mj_stackAlloc(d, ncon*3); mjtNum* positions = mj_stackAllocNum(d, ncon*3);
// Get forces and positions in spherical coordinates. // Get forces and positions in spherical coordinates.
int contact = 0; int contact = 0;
@@ -338,12 +338,12 @@ void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) {
} }
// Transpose forces. // Transpose forces.
mjtNum* forcesT = mj_stackAlloc(d, ncon*6); mjtNum* forcesT = mj_stackAllocNum(d, ncon*6);
mju_transpose(forcesT, forces, ncon, 6); mju_transpose(forcesT, forces, ncon, 6);
// Allocate bin edges. // Allocate bin edges.
mjtNum* x_edges = mj_stackAlloc(d, size_[0] + 1); mjtNum* x_edges = mj_stackAllocNum(d, size_[0] + 1);
mjtNum* y_edges = mj_stackAlloc(d, size_[1] + 1); mjtNum* y_edges = mj_stackAllocNum(d, size_[1] + 1);
// Make bin edges. // Make bin edges.
BinEdges(x_edges, y_edges, size_, fov_, gamma_); 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); mju_mat2Quat(site_quat, site_mat);
// Allocate bin edges. // Allocate bin edges.
mjtNum* x_edges = mj_stackAlloc(d, size_[0] + 1); mjtNum* x_edges = mj_stackAllocNum(d, size_[0] + 1);
mjtNum* y_edges = mj_stackAlloc(d, size_[1] + 1); mjtNum* y_edges = mj_stackAllocNum(d, size_[1] + 1);
// Make bin edges. // Make bin edges.
BinEdges(x_edges, y_edges, size_, fov_, gamma_); BinEdges(x_edges, y_edges, size_, fov_, gamma_);
sample/derivative.cc
+3 -3
View File
@@ -62,8 +62,8 @@ void worker(const mjModel* m, const mjData* dmain, mjData* d, int id) {
// allocate stack space for result at center // allocate stack space for result at center
mjMARKSTACK; mjMARKSTACK;
mjtNum* center = mj_stackAlloc(d, nv); mjtNum* center = mj_stackAllocNum(d, nv);
mjtNum* warmstart = mj_stackAlloc(d, nv); mjtNum* warmstart = mj_stackAllocNum(d, nv);
// prepare static schedule: range of derivative columns to be computed by this thread // prepare static schedule: range of derivative columns to be computed by this thread
int chunk = (m->nv + nthread-1) / nthread; int chunk = (m->nv + nthread-1) / nthread;
@@ -223,7 +223,7 @@ void checkderiv(const mjModel* m, mjData* d, mjtNum error[7]) {
// allocate space // allocate space
mjMARKSTACK; mjMARKSTACK;
mjtNum* mat = mj_stackAlloc(d, nv*nv); mjtNum* mat = mj_stackAllocNum(d, nv*nv);
// get pointers to derivative matrices // get pointers to derivative matrices
mjtNum* G0 = deriv; // dinv/dpos mjtNum* G0 = deriv; // dinv/dpos
src/engine/engine_collision_driver.c
+1 -1
View File
@@ -732,7 +732,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) {
mju_eig3(eigval, frame, quat, cov); mju_eig3(eigval, frame, quat, cov);
// allocate AABB; clear world entry (not used) // allocate AABB; clear world entry (not used)
aabb = mj_stackAlloc(d, 6*nbody); aabb = mj_stackAllocNum(d, 6*nbody);
mju_zero(aabb, 6); mju_zero(aabb, 6);
// construct body AABB for the aligned frame, count collidable // construct body AABB for the aligned frame, count collidable
src/engine/engine_core_constraint.c
+15 -15
View File
@@ -433,14 +433,14 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
} }
// allocate space // allocate space
jac[0] = mj_stackAlloc(d, 6*nv); jac[0] = mj_stackAllocNum(d, 6*nv);
jac[1] = mj_stackAlloc(d, 6*nv); jac[1] = mj_stackAllocNum(d, 6*nv);
jacdif = mj_stackAlloc(d, 6*nv); jacdif = mj_stackAllocNum(d, 6*nv);
if (issparse) { if (issparse) {
chain = mj_stackAllocInt(d, nv); chain = mj_stackAllocInt(d, nv);
chain2 = mj_stackAllocInt(d, nv); chain2 = mj_stackAllocInt(d, nv);
buf_ind = 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 // find active equality constraints
@@ -641,7 +641,7 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
} }
// allocate Jacobian // allocate Jacobian
jac = mj_stackAlloc(d, nv); jac = mj_stackAllocNum(d, nv);
// find frictional dofs // find frictional dofs
for (int i=0; i < nv; i++) { for (int i=0; i < nv; i++) {
@@ -702,7 +702,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
} }
// allocate Jacobian // allocate Jacobian
jac = mj_stackAlloc(d, nv); jac = mj_stackAllocNum(d, nv);
// find joint limits // find joint limits
for (int i=0; i < m->njnt; i++) { for (int i=0; i < m->njnt; i++) {
@@ -847,13 +847,13 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
} }
// allocate Jacobian // allocate Jacobian
jac = mj_stackAlloc(d, 6*NV); jac = mj_stackAllocNum(d, 6*NV);
jacdifp = mj_stackAlloc(d, 3*NV); jacdifp = mj_stackAllocNum(d, 3*NV);
jacdifr = mj_stackAlloc(d, 3*NV); jacdifr = mj_stackAllocNum(d, 3*NV);
jac1p = mj_stackAlloc(d, 3*NV); jac1p = mj_stackAllocNum(d, 3*NV);
jac2p = mj_stackAlloc(d, 3*NV); jac2p = mj_stackAllocNum(d, 3*NV);
jac1r = mj_stackAlloc(d, 3*NV); jac1r = mj_stackAllocNum(d, 3*NV);
jac2r = mj_stackAlloc(d, 3*NV); jac2r = mj_stackAllocNum(d, 3*NV);
if (issparse) { if (issparse) {
chain = mj_stackAllocInt(d, NV); chain = mj_stackAllocInt(d, NV);
} }
@@ -1755,8 +1755,8 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
} }
// space for backsubM2(J')' and its traspose // space for backsubM2(J')' and its traspose
mjtNum* JM2 = mj_stackAlloc(d, nefc*nv); mjtNum* JM2 = mj_stackAllocNum(d, nefc*nv);
mjtNum* JM2T = mj_stackAlloc(d, nv*nefc); mjtNum* JM2T = mj_stackAllocNum(d, nv*nefc);
// sparse // sparse
if (mj_isSparse(m)) { if (mj_isSparse(m)) {
src/engine/engine_core_smooth.c
+19 -19
View File
@@ -179,7 +179,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
void mj_comPos(const mjModel* m, mjData* d) { void mj_comPos(const mjModel* m, mjData* d) {
mjtNum offset[3], axis[3]; mjtNum offset[3], axis[3];
mjMARKSTACK; mjMARKSTACK;
mjtNum* mass_subtree = mj_stackAlloc(d, m->nbody); mjtNum* mass_subtree = mj_stackAllocNum(d, m->nbody);
// clear subtree // clear subtree
mju_zero(mass_subtree, m->nbody); mju_zero(mass_subtree, m->nbody);
@@ -401,14 +401,14 @@ void mj_tendon(const mjModel* m, mjData* d) {
} }
// allocate space // allocate space
jac1 = mj_stackAlloc(d, 3*nv); jac1 = mj_stackAllocNum(d, 3*nv);
jac2 = mj_stackAlloc(d, 3*nv); jac2 = mj_stackAllocNum(d, 3*nv);
jacdif = mj_stackAlloc(d, 3*nv); jacdif = mj_stackAllocNum(d, 3*nv);
tmp = mj_stackAlloc(d, nv); tmp = mj_stackAllocNum(d, nv);
if (issparse) { if (issparse) {
chain = mj_stackAllocInt(d, nv); chain = mj_stackAllocInt(d, nv);
buf_ind = mj_stackAllocInt(d, nv); buf_ind = mj_stackAllocInt(d, nv);
sparse_buf = mj_stackAlloc(d, nv); sparse_buf = mj_stackAllocNum(d, nv);
} }
// clear results // clear results
@@ -629,9 +629,9 @@ void mj_transmission(const mjModel* m, mjData* d) {
} }
// allocate space, clear moments // allocate space, clear moments
jac = mj_stackAlloc(d, 3*nv); jac = mj_stackAllocNum(d, 3*nv);
jacA = mj_stackAlloc(d, 3*nv); jacA = mj_stackAllocNum(d, 3*nv);
jacS = mj_stackAlloc(d, 3*nv); jacS = mj_stackAllocNum(d, 3*nv);
mju_zero(moment, nu*nv); mju_zero(moment, nu*nv);
// define variables required for body transmission, don't allocate // define variables required for body transmission, don't allocate
@@ -804,7 +804,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
// reference site defined // reference site defined
else { else {
int refid = m->actuator_trnid[2*i+1]; 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 // clear length
length[i] = 0; 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); mju_rotVecMat(wrench, gear+3, d->site_xmat+9*refid);
// moment_tmp: global Jacobian projected on wrench, add to moment // 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_mulMatTVec(moment_tmp, jacS, wrench, 3, nv);
mju_addTo(moment+i*nv, moment_tmp, 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 // allocate stack variables for the first mjTRN_BODY
if (!efc_force) { if (!efc_force) {
efc_force = mj_stackAlloc(d, d->nefc); efc_force = mj_stackAllocNum(d, d->nefc);
moment_exclude = mj_stackAlloc(d, nv); moment_exclude = mj_stackAllocNum(d, nv);
jacdifp = mj_stackAlloc(d, 3*nv); jacdifp = mj_stackAllocNum(d, 3*nv);
jac1p = mj_stackAlloc(d, 3*nv); jac1p = mj_stackAllocNum(d, 3*nv);
jac2p = mj_stackAlloc(d, 3*nv); jac2p = mj_stackAllocNum(d, 3*nv);
chain = issparse ? mj_stackAllocInt(d, nv) : NULL; 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) { void mj_subtreeVel(const mjModel* m, mjData* d) {
mjtNum dx[3], dv[3], dp[3], dL[3]; mjtNum dx[3], dv[3], dp[3], dL[3];
mjMARKSTACK; mjMARKSTACK;
mjtNum* body_vel = mj_stackAlloc(d, 6*m->nbody); mjtNum* body_vel = mj_stackAllocNum(d, 6*m->nbody);
// bodywise quantities // bodywise quantities
for (int i=0; i < m->nbody; i++) { 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) { void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) {
mjtNum tmp[6], tmp1[6]; mjtNum tmp[6], tmp1[6];
mjMARKSTACK; mjMARKSTACK;
mjtNum* loc_cacc = mj_stackAlloc(d, m->nbody*6); mjtNum* loc_cacc = mj_stackAllocNum(d, m->nbody*6);
mjtNum* loc_cfrc_body = mj_stackAlloc(d, m->nbody*6); mjtNum* loc_cfrc_body = mj_stackAllocNum(d, m->nbody*6);
// set world acceleration to -gravity // set world acceleration to -gravity
mju_zero(loc_cacc, 6); mju_zero(loc_cacc, 6);
src/engine/engine_derivative.c
+16 -16
View File
@@ -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]; mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6];
mjMARKSTACK; mjMARKSTACK;
mjtNum* Dcvel = mj_stackAlloc(d, nbody*6*nv); mjtNum* Dcvel = mj_stackAllocNum(d, nbody*6*nv);
mjtNum* Dcdofdot = mj_stackAlloc(d, nv*6*nv); mjtNum* Dcdofdot = mj_stackAllocNum(d, nv*6*nv);
mjtNum* Dcacc = mj_stackAlloc(d, nbody*6*nv); mjtNum* Dcacc = mj_stackAllocNum(d, nbody*6*nv);
mjtNum* Dcfrcbody = mj_stackAlloc(d, nbody*6*nv); mjtNum* Dcfrcbody = mj_stackAllocNum(d, nbody*6*nv);
mjtNum* row = mj_stackAlloc(d, nv); mjtNum* row = mj_stackAllocNum(d, nv);
// compute Dcvel and Dcdofdot // compute Dcvel and Dcdofdot
mjd_comVel_vel_dense(m, d, Dcvel, 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]; mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6];
mjMARKSTACK; mjMARKSTACK;
mjtNum* Dcdofdot = mj_stackAlloc(d, 6*m->nD); mjtNum* Dcdofdot = mj_stackAllocNum(d, 6*m->nD);
mjtNum* Dcvel = mj_stackAlloc(d, 6*m->nB); mjtNum* Dcvel = mj_stackAllocNum(d, 6*m->nB);
mjtNum* Dcacc = mj_stackAlloc(d, 6*m->nB); mjtNum* Dcacc = mj_stackAllocNum(d, 6*m->nB);
mjtNum* Dcfrcbody = mj_stackAlloc(d, 6*m->nB); mjtNum* Dcfrcbody = mj_stackAllocNum(d, 6*m->nB);
mjtNum* row = mj_stackAlloc(d, nv); mjtNum* row = mj_stackAllocNum(d, nv);
// clear // clear
mju_zero(Dcdofdot, 6*m->nD); 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 // allocate dense row
mjMARKSTACK; mjMARKSTACK;
mjtNum* row = mj_stackAlloc(d, nv); mjtNum* row = mj_stackAllocNum(d, nv);
// process non-zero elements of B // process non-zero elements of B
for (int i=0; i < n; i++) { for (int i=0; i < n; i++) {
@@ -774,7 +774,7 @@ static void addJTBJSparse(
// allocate row // allocate row
mjMARKSTACK; 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) ) // compute qDeriv(k,p) += sum_{i,j} ( J(i,k)*B(i,j)*J(j,p) )
for (int i = 0; i < n; i++) { 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 nv = m->nv;
int nnz = nv; int nnz = nv;
int rownnz[6], rowadr[6]; int rownnz[6], rowadr[6];
mjtNum* J = mj_stackAlloc(d, 6*nv); mjtNum* J = mj_stackAllocNum(d, 6*nv);
mjtNum* tmp = mj_stackAlloc(d, 3*nv); mjtNum* tmp = mj_stackAllocNum(d, 3*nv);
int* colind = mj_stackAllocInt(d, 6*nv); int* colind = mj_stackAllocInt(d, 6*nv);
int* colind_compressed = 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 nv = m->nv;
int rownnz[6], rowadr[6]; int rownnz[6], rowadr[6];
mjtNum* J = mj_stackAlloc(d, 6*nv); mjtNum* J = mj_stackAllocNum(d, 6*nv);
mjtNum* tmp = mj_stackAlloc(d, 3*nv); mjtNum* tmp = mj_stackAllocNum(d, 3*nv);
int* colind = mj_stackAllocInt(d, 6*nv); int* colind = mj_stackAllocInt(d, 6*nv);
mjtNum lvel[6], wind[6], lwind[6], box[3], B; mjtNum lvel[6], wind[6], lwind[6], box[3], B;
src/engine/engine_derivative_fd.c
+25 -25
View File
@@ -197,8 +197,8 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) {
int nv = m->nv; int nv = m->nv;
mjMARKSTACK; mjMARKSTACK;
mjtNum* qfrc_passive = mj_stackAlloc(d, nv); mjtNum* qfrc_passive = mj_stackAllocNum(d, nv);
mjtNum* fd = mj_stackAlloc(d, nv); mjtNum* fd = mj_stackAllocNum(d, nv);
int* cnt = mj_stackAllocInt(d, nv); int* cnt = mj_stackAllocInt(d, nv);
// clear row counters // clear row counters
@@ -248,9 +248,9 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) {
int nv = m->nv; int nv = m->nv;
mjMARKSTACK; mjMARKSTACK;
mjtNum* plus = mj_stackAlloc(d, nv); mjtNum* plus = mj_stackAllocNum(d, nv);
mjtNum* minus = mj_stackAlloc(d, nv); mjtNum* minus = mj_stackAllocNum(d, nv);
mjtNum* fd = mj_stackAlloc(d, nv); mjtNum* fd = mj_stackAllocNum(d, nv);
int* cnt = mj_stackAllocInt(d, nv); int* cnt = mj_stackAllocInt(d, nv);
// clear row counters // clear row counters
@@ -332,22 +332,22 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
mjMARKSTACK; mjMARKSTACK;
// states // states
mjtNum *state = mj_stackAlloc(d, nq+nv+na); // current state mjtNum *state = mj_stackAllocNum(d, nq+nv+na); // current state
mjtNum *next = mj_stackAlloc(d, nq+nv+na); // next state mjtNum *next = mj_stackAllocNum(d, nq+nv+na); // next state
mjtNum *next_plus = mj_stackAlloc(d, nq+nv+na); // forward-nudged next state mjtNum *next_plus = mj_stackAllocNum(d, nq+nv+na); // forward-nudged next state
mjtNum *next_minus = mj_stackAlloc(d, nq+nv+na); // backward-nudged next state mjtNum *next_minus = mj_stackAllocNum(d, nq+nv+na); // backward-nudged next state
// warmstart accelerations // warmstart accelerations
mjtNum *warmstart = mjDISABLED(mjDSBL_WARMSTART) ? NULL : mj_stackAlloc(d, nv); mjtNum *warmstart = mjDISABLED(mjDSBL_WARMSTART) ? NULL : mj_stackAllocNum(d, nv);
// sensors // sensors
int skipsensor = !DsDq && !DsDv && !DsDa && !DsDu; int skipsensor = !DsDq && !DsDv && !DsDa && !DsDu;
mjtNum *sensor = skipsensor ? NULL : mj_stackAlloc(d, ns); // sensor values mjtNum *sensor = skipsensor ? NULL : mj_stackAllocNum(d, ns); // sensor values
mjtNum *sensor_plus = skipsensor ? NULL : mj_stackAlloc(d, ns); // forward-nudged sensors mjtNum *sensor_plus = skipsensor ? NULL : mj_stackAllocNum(d, ns); // forward-nudged sensors
mjtNum *sensor_minus = skipsensor ? NULL : mj_stackAlloc(d, ns); // backward-nudged sensors mjtNum *sensor_minus = skipsensor ? NULL : mj_stackAllocNum(d, ns); // backward-nudged sensors
// controls // controls
mjtNum *ctrl = mj_stackAlloc(d, nu); mjtNum *ctrl = mj_stackAllocNum(d, nu);
// save current inputs // save current inputs
mjtNum time = d->time; 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 // finite-difference positions: skip=mjSTAGE_NONE
if (DyDq || DsDq) { 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++) { for (int i=0; i < nv; i++) {
// nudge forward // nudge forward
mju_zero(dpos, nv); mju_zero(dpos, nv);
@@ -582,10 +582,10 @@ void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_cente
mjMARKSTACK; mjMARKSTACK;
// allocate transposed matrices // allocate transposed matrices
mjtNum *AT = A ? mj_stackAlloc(d, ndx*ndx) : NULL; // state-transition matrix (transposed) mjtNum *AT = A ? mj_stackAllocNum(d, ndx*ndx) : NULL; // state-transition matrix (transposed)
mjtNum *BT = B ? mj_stackAlloc(d, nu*ndx) : NULL; // control-transition matrix (transposed) mjtNum *BT = B ? mj_stackAllocNum(d, nu*ndx) : NULL; // control-transition matrix (transposed)
mjtNum *CT = C ? mj_stackAlloc(d, ndx*ns) : NULL; // state-observation matrix (transposed) mjtNum *CT = C ? mj_stackAllocNum(d, ndx*ns) : NULL; // state-observation matrix (transposed)
mjtNum *DT = D ? mj_stackAlloc(d, nu*ns) : NULL; // control-observation matrix (transposed) mjtNum *DT = D ? mj_stackAllocNum(d, nu*ns) : NULL; // control-observation matrix (transposed)
// set offset pointers // set offset pointers
if (A) { if (A) {
@@ -648,11 +648,11 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio
int skipsensor = !DsDq && !DsDv && !DsDa; int skipsensor = !DsDq && !DsDv && !DsDa;
// local vectors // local vectors
mjtNum *pos = mj_stackAlloc(d, nq); // position mjtNum *pos = mj_stackAllocNum(d, nq); // position
mjtNum *force = mj_stackAlloc(d, nv); // force mjtNum *force = mj_stackAllocNum(d, nv); // force
mjtNum *force_plus = mj_stackAlloc(d, nv); // nudged force mjtNum *force_plus = mj_stackAllocNum(d, nv); // nudged force
mjtNum *sensor = skipsensor ? NULL : mj_stackAlloc(d, ns); // sensor values mjtNum *sensor = skipsensor ? NULL : mj_stackAllocNum(d, ns); // sensor values
mjtNum *mass = DmDq ? mj_stackAlloc(d, nM) : NULL; // mass matrix mjtNum *mass = DmDq ? mj_stackAllocNum(d, nM) : NULL; // mass matrix
// save current positions // save current positions
mju_copy(pos, d->qpos, nq); 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 // position: skip = mjSTAGE_NONE
if (DfDq || DsDq || DmDq) { 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++) { for (int i=0; i < nv; i++) {
// nudge // nudge
mju_zero(dpos, nv); mju_zero(dpos, nv);
src/engine/engine_forward.c
+13 -13
View File
@@ -200,7 +200,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// local, clamped copy of ctrl // local, clamped copy of ctrl
mjMARKSTACK; mjMARKSTACK;
mjtNum *ctrl = mj_stackAlloc(d, nu); mjtNum *ctrl = mj_stackAllocNum(d, nu);
if (mjDISABLED(mjDSBL_CLAMPCTRL)) { if (mjDISABLED(mjDSBL_CLAMPCTRL)) {
mju_copy(ctrl, d->ctrl, nu); mju_copy(ctrl, d->ctrl, nu);
} else { } else {
@@ -426,7 +426,7 @@ static void warmstart(const mjModel* m, mjData* d) {
// warmstart with best of (qacc_warmstart, qacc_smooth) // warmstart with best of (qacc_warmstart, qacc_smooth)
if (!mjDISABLED(mjDSBL_WARMSTART)) { if (!mjDISABLED(mjDSBL_WARMSTART)) {
mjMARKSTACK; mjMARKSTACK;
mjtNum* jar = mj_stackAlloc(d, nefc); mjtNum* jar = mj_stackAllocNum(d, nefc);
// start with qacc = qacc_warmstart // start with qacc = qacc_warmstart
mju_copy(d->qacc, d->qacc_warmstart, nv); 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) { if (m->opt.solver == mjSOL_PGS) {
// cost(force_warmstart) // cost(force_warmstart)
mjtNum PGS_warmstart = mju_dot(d->efc_force, d->efc_b, nefc); 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)) if (mj_isSparse(m))
mju_mulMatVecSparse(ARf, d->efc_AR, d->efc_force, nefc, mju_mulMatVecSparse(ARf, d->efc_AR, d->efc_force, nefc,
d->efc_AR_rownnz, d->efc_AR_rowadr, d->efc_AR_rownnz, d->efc_AR_rowadr,
@@ -463,7 +463,7 @@ static void warmstart(const mjModel* m, mjData* d) {
// non-PGS // non-PGS
else { else {
// add Gauss to cost(qacc_warmstart) // 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); mj_mulM(m, d, Ma, d->qacc_warmstart);
for (int i=0; i < nv; i++) { 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]); 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) { void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
int nv = m->nv, nM = m->nM; int nv = m->nv, nM = m->nM;
mjMARKSTACK; mjMARKSTACK;
mjtNum* qfrc = mj_stackAlloc(d, nv); mjtNum* qfrc = mj_stackAllocNum(d, nv);
mjtNum* qacc = mj_stackAlloc(d, nv); mjtNum* qacc = mj_stackAllocNum(d, nv);
// check for dof damping if disable flag is not set // check for dof damping if disable flag is not set
int dof_damping = 0; int dof_damping = 0;
@@ -612,7 +612,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
// damping: integrate implicitly // damping: integrate implicitly
else { else {
if (!skipfactor) { if (!skipfactor) {
mjtNum* MhB = mj_stackAlloc(d, nM); mjtNum* MhB = mj_stackAllocNum(d, nM);
// MhB = M + h*diag(B) // MhB = M + h*diag(B)
mju_copy(MhB, d->qM, m->nM); 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 // 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++) { for (int i=0; i < N; i++) {
X[i] = mj_stackAlloc(d, nq+nv+na); X[i] = mj_stackAllocNum(d, nq+nv+na);
F[i] = mj_stackAlloc(d, nv+na); F[i] = mj_stackAllocNum(d, nv+na);
} }
// precompute C and T; C,T,A have size (N-1) // 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; int nv = m->nv;
mjMARKSTACK; mjMARKSTACK;
mjtNum* qfrc = mj_stackAlloc(d, nv); mjtNum* qfrc = mj_stackAllocNum(d, nv);
mjtNum* qacc = mj_stackAlloc(d, nv); mjtNum* qacc = mj_stackAllocNum(d, nv);
// set qfrc = qfrc_smooth + qfrc_constraint // set qfrc = qfrc_smooth + qfrc_constraint
mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv); 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); mjd_smooth_vel(m, d, /* flg_bias = */ 0);
// modified mass matrix MhB = qDeriv[Lower] // 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); mj_copyD2MSparse(m, d, MhB, d->qDeriv);
// set MhB = M - dt*qDeriv // set MhB = M - dt*qDeriv
src/engine/engine_inverse.c
+9 -9
View File
@@ -96,7 +96,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
mjtNum *qacc = d->qacc; mjtNum *qacc = d->qacc;
mjMARKSTACK; mjMARKSTACK;
mjtNum* qfrc = mj_stackAlloc(d, nv); mjtNum* qfrc = mj_stackAllocNum(d, nv);
// use selected integrator // use selected integrator
switch ((mjtIntegrator) m->opt.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); mjd_smooth_vel(m, d, /* flg_bias = */ 0);
// save mass matrix // save mass matrix
mjtNum* qMsave = mj_stackAlloc(d, m->nM); mjtNum* qMsave = mj_stackAllocNum(d, m->nM);
mju_copy(qMsave, d->qM, m->nM); mju_copy(qMsave, d->qM, m->nM);
// set M = M - dt*qDeriv (reduced to M nonzeros) // 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); mj_copyD2MSparse(m, d, qDerivReduced, d->qDeriv);
mju_addToScl(d->qM, qDerivReduced, -m->opt.timestep, m->nM); mju_addToScl(d->qM, qDerivReduced, -m->opt.timestep, m->nM);
@@ -186,7 +186,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) {
} }
mjMARKSTACK; mjMARKSTACK;
mjtNum* jar = mj_stackAlloc(d, nefc); mjtNum* jar = mj_stackAllocNum(d, nefc);
// compute jar = Jac*qacc - aref // compute jar = Jac*qacc - aref
mj_mulJacVec(m, d, jar, d->qacc); mj_mulJacVec(m, d, jar, d->qacc);
@@ -233,7 +233,7 @@ void mj_inverseSkip(const mjModel* m, mjData* d,
if (mjENABLED(mjENBL_INVDISCRETE)) { if (mjENABLED(mjENBL_INVDISCRETE)) {
// save current qacc // save current qacc
qacc = mj_stackAlloc(d, nv); qacc = mj_stackAllocNum(d, nv);
mju_copy(qacc, d->qacc, nv); mju_copy(qacc, d->qacc, nv);
// modify qacc in-place // modify qacc in-place
@@ -286,10 +286,10 @@ void mj_compareFwdInv(const mjModel* m, mjData* d) {
} }
// allocate // allocate
qforce = mj_stackAlloc(d, nv); qforce = mj_stackAllocNum(d, nv);
dif = mj_stackAlloc(d, nv); dif = mj_stackAllocNum(d, nv);
save_qfrc_constraint = mj_stackAlloc(d, nv); save_qfrc_constraint = mj_stackAllocNum(d, nv);
save_efc_force = mj_stackAlloc(d, nefc); save_efc_force = mj_stackAllocNum(d, nefc);
// qforce = qfrc_applied + J'*xfrc_applied + qfrc_actuator // qforce = qfrc_applied + J'*xfrc_applied + qfrc_actuator
// should equal result of inverse dynamics // should equal result of inverse dynamics
src/engine/engine_io.c
+1 -1
View File
@@ -1266,7 +1266,7 @@ void* mj_stackAllocByte(mjData* d, size_t size) {
return (void*)start_ptr; 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)); return (mjtNum*)mj_stackAllocByte(d, size * sizeof(mjtNum));
} }
src/engine/engine_io.h
+1 -1
View File
@@ -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); MJAPI void* mj_arenaAlloc(mjData* d, int bytes, int alignment);
// mjData stack allocate for array of mjtNums // 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 // mjData stack allocate for array of ints
MJAPI int* mj_stackAllocInt(mjData* d, int size); MJAPI int* mj_stackAllocInt(mjData* d, int size);
src/engine/engine_print.c
+1 -1
View File
@@ -755,7 +755,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
} }
// allocate full inertia // allocate full inertia
M = mj_stackAlloc(d, m->nv*m->nv); M = mj_stackAllocNum(d, m->nv*m->nv);
#ifdef MEMORY_SANITIZER #ifdef MEMORY_SANITIZER
// If memory sanitizer is active, d->buffer will be marked as poisoned, even // If memory sanitizer is active, d->buffer will be marked as poisoned, even
src/engine/engine_ray.c
+1 -1
View File
@@ -1161,7 +1161,7 @@ void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum*
mjMARKSTACK; mjMARKSTACK;
// allocate source // 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); int* geom_eliminate = mj_stackAllocInt(d, m->ngeom);
// initialize source // initialize source
src/engine/engine_sensor.c
+1 -1
View File
@@ -861,7 +861,7 @@ void mj_energyVel(const mjModel* m, mjData* d) {
return; return;
} }
vec = mj_stackAlloc(d, m->nv); vec = mj_stackAllocNum(d, m->nv);
// kinetic energy: 0.5 * qvel' * M * qvel // kinetic energy: 0.5 * qvel' * M * qvel
mj_mulM(m, d, vec, d->qvel); mj_mulM(m, d, vec, d->qvel);
src/engine/engine_setconst.c
+3 -3
View File
@@ -62,8 +62,8 @@ static void set0(mjModel* m, mjData* d) {
int id, id1, id2, dnum, nv = m->nv; int id, id1, id2, dnum, nv = m->nv;
mjtNum A[36] = {0}, pos[3], quat[4]; mjtNum A[36] = {0}, pos[3], quat[4];
mjMARKSTACK; mjMARKSTACK;
mjtNum* jac = mj_stackAlloc(d, 6*nv); mjtNum* jac = mj_stackAllocNum(d, 6*nv);
mjtNum* tmp = mj_stackAlloc(d, 6*nv); mjtNum* tmp = mj_stackAllocNum(d, 6*nv);
int* cammode = 0; int* cammode = 0;
int* lightmode = 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 xmax[3] = {-1E+10, -1E+10, -1E+10};
mjtNum rbound; mjtNum rbound;
mjMARKSTACK; 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 // compute bounding box of bodies, joint centers, geoms and sites
for (int i=1; i < m->nbody; i++) { for (int i=1; i < m->nbody; i++) {
src/engine/engine_solver.c
+20 -20
View File
@@ -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]; mjtNum v[6], v1[6], Athis[36], Ac[25], bc[5], res[6], oldforce[6];
mjContact* con; mjContact* con;
mjMARKSTACK; mjMARKSTACK;
mjtNum* ARinv = mj_stackAlloc(d, nefc); mjtNum* ARinv = mj_stackAllocNum(d, nefc);
int* oldstate = mj_stackAllocInt(d, nefc); int* oldstate = mj_stackAllocInt(d, nefc);
// precompute inverse diagonal of AR // 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; mjtNum v[5], Ac[25], bc[5], res[5], oldforce[5], delta[5], mid, y, K0, K1;
mjContact* con; mjContact* con;
mjMARKSTACK; mjMARKSTACK;
mjtNum* ARinv = mj_stackAlloc(d, nefc); mjtNum* ARinv = mj_stackAllocNum(d, nefc);
int* oldstate = mj_stackAllocInt(d, nefc); int* oldstate = mj_stackAllocInt(d, nefc);
// precompute inverse diagonal of A // precompute inverse diagonal of A
@@ -755,20 +755,20 @@ static void CGallocate(const mjModel* m, mjData* d,
memset(ctx, 0, sizeof(mjCGContext)); memset(ctx, 0, sizeof(mjCGContext));
// common arrays // common arrays
ctx->Jaref = mj_stackAlloc(d, nefc); ctx->Jaref = mj_stackAllocNum(d, nefc);
ctx->Jv = mj_stackAlloc(d, nefc); ctx->Jv = mj_stackAllocNum(d, nefc);
ctx->Ma = mj_stackAlloc(d, nv); ctx->Ma = mj_stackAllocNum(d, nv);
ctx->Mv = mj_stackAlloc(d, nv); ctx->Mv = mj_stackAllocNum(d, nv);
ctx->grad = mj_stackAlloc(d, nv); ctx->grad = mj_stackAllocNum(d, nv);
ctx->Mgrad = mj_stackAlloc(d, nv); ctx->Mgrad = mj_stackAllocNum(d, nv);
ctx->search = mj_stackAlloc(d, nv); ctx->search = mj_stackAllocNum(d, nv);
ctx->quad = mj_stackAlloc(d, nefc*3); ctx->quad = mj_stackAllocNum(d, nefc*3);
// Hessian (Newton only) // Hessian (Newton only)
ctx->flg_Newton = flg_Newton; ctx->flg_Newton = flg_Newton;
if (flg_Newton) { if (flg_Newton) {
ctx->H = mj_stackAlloc(d, nv*nv); ctx->H = mj_stackAllocNum(d, nv*nv);
ctx->Hcone = mj_stackAlloc(d, nv*nv); ctx->Hcone = mj_stackAllocNum(d, nv*nv);
ctx->rownnz = mj_stackAllocInt(d, nv); ctx->rownnz = mj_stackAllocInt(d, nv);
ctx->rowadr = mj_stackAllocInt(d, nv); ctx->rowadr = mj_stackAllocInt(d, nv);
ctx->colind = mj_stackAllocInt(d, nv*nv); ctx->colind = mj_stackAllocInt(d, nv*nv);
@@ -1279,8 +1279,8 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) {
mjMARKSTACK; mjMARKSTACK;
// storage for L'*J // storage for L'*J
mjtNum* LTJ = mj_stackAlloc(d, 6*nv); mjtNum* LTJ = mj_stackAllocNum(d, 6*nv);
mjtNum* LTJ_row = mj_stackAlloc(d, nv); mjtNum* LTJ_row = mj_stackAllocNum(d, nv);
int* LTJ_ind = mj_stackAllocInt(d, nv); int* LTJ_ind = mj_stackAllocInt(d, nv);
// start with Hcone = H // start with Hcone = H
@@ -1357,7 +1357,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
mjMARKSTACK; mjMARKSTACK;
// compute D corresponding to quad states // 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++) { for (int i=0; i < nefc; i++) {
if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) {
D[i] = d->efc_D[i]; 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 nnz = m->nD; // use sparse dof-dof matrix
int* M_rownnz = mj_stackAllocInt(d, nv); // actual nnz count int* M_rownnz = mj_stackAllocInt(d, nv); // actual nnz count
int* M_colind = mj_stackAllocInt(d, nnz); 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_makeMSparse(m, d, M, M_rownnz, NULL, M_colind);
// compute H = J'*D*J // compute H = J'*D*J
@@ -1446,7 +1446,7 @@ static void HessianIncremental(const mjModel* m, mjData* d,
mjMARKSTACK; mjMARKSTACK;
// local space // local space
mjtNum* vec = mj_stackAlloc(d, nv); mjtNum* vec = mj_stackAllocNum(d, nv);
int* vec_ind = mj_stackAllocInt(d, nv); int* vec_ind = mj_stackAllocInt(d, nv);
// clear update counter // clear update counter
@@ -1521,9 +1521,9 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
// allocate local storage // allocate local storage
if (!flg_Newton) { if (!flg_Newton) {
gradold = mj_stackAlloc(d, nv); gradold = mj_stackAllocNum(d, nv);
Mgradold = mj_stackAlloc(d, nv); Mgradold = mj_stackAllocNum(d, nv);
Mgraddif = mj_stackAlloc(d, nv); Mgraddif = mj_stackAllocNum(d, nv);
} }
int* oldstate = mj_stackAllocInt(d, nefc); int* oldstate = mj_stackAllocInt(d, nefc);
src/engine/engine_support.c
+8 -8
View File
@@ -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) { void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) {
int nv = m->nv; int nv = m->nv;
mjMARKSTACK; mjMARKSTACK;
mjtNum* jacp_b = mj_stackAlloc(d, 3*nv); mjtNum* jacp_b = mj_stackAllocNum(d, 3*nv);
// clear output // clear output
mju_zero(jacp, 3*nv); 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 // get full Jacobian of point
mjMARKSTACK; mjMARKSTACK;
mjtNum* jacp = (jacPoint ? jacPoint : mj_stackAlloc(d, 3*nv)); mjtNum* jacp = (jacPoint ? jacPoint : mj_stackAllocNum(d, 3*nv));
mjtNum* jacr = mj_stackAlloc(d, 3*nv); mjtNum* jacr = mj_stackAllocNum(d, 3*nv);
mj_jac(m, d, jacp, jacr, point, body); mj_jac(m, d, jacp, jacr, point, body);
// jacAxis_col = cross(jacr_col, axis) // 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 nnz = m->nD; // use sparse dof-dof matrix
int* M_rownnz = mj_stackAllocInt(d, nv); // actual nnz count int* M_rownnz = mj_stackAllocInt(d, nv); // actual nnz count
int* M_colind = mj_stackAllocInt(d, nnz); 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_makeMSparse(m, d, M, M_rownnz, NULL, M_colind);
mj_addMSparse(m, d, dst, rownnz, rowadr, colind, M, mj_addMSparse(m, d, dst, rownnz, rowadr, colind, M,
@@ -1051,7 +1051,7 @@ void mj_addMSparse(const mjModel* m, mjData* d, mjtNum* dst,
mjMARKSTACK; mjMARKSTACK;
int* buf_ind = mj_stackAllocInt(d, nv); int* buf_ind = mj_stackAllocInt(d, nv);
mjtNum* sparse_buf = mj_stackAlloc(d, nv); mjtNum* sparse_buf = mj_stackAllocNum(d, nv);
// add to destination // add to destination
for (int i=0; i < nv; i++) { for (int i=0; i < nv; i++) {
@@ -1162,9 +1162,9 @@ void mj_applyFT(const mjModel* m, mjData* d,
// allocate local variables // allocate local variables
mjMARKSTACK; mjMARKSTACK;
mjtNum* jacp = mj_stackAlloc(d, 3*nv); mjtNum* jacp = mj_stackAllocNum(d, 3*nv);
mjtNum* jacr = mj_stackAlloc(d, 3*nv); mjtNum* jacr = mj_stackAllocNum(d, 3*nv);
mjtNum* qforce = mj_stackAlloc(d, nv); mjtNum* qforce = mj_stackAllocNum(d, nv);
// make sure body is in range // make sure body is in range
if (body < 0 || body >= m->nbody) { if (body < 0 || body >= m->nbody) {
src/engine/engine_util_solve.c
+2 -2
View File
@@ -150,7 +150,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag,
mjMARKSTACK; mjMARKSTACK;
int* buf_ind = mj_stackAllocInt(d, n); 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 // shrink rows so that rownnz ends at diagonal
for (int r=0; r < n; r++) { 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) { mjData* d) {
mjMARKSTACK; mjMARKSTACK;
int* buf_ind = mj_stackAllocInt(d, n); 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) // backpass over rows corresponding to non-zero x(r)
int rank = n, i = x_nnz - 1; int rank = n, i = x_nnz - 1;
src/engine/engine_util_sparse.c
+1 -1
View File
@@ -559,7 +559,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
mjMARKSTACK; mjMARKSTACK;
// a dense row buffer that stores the current row in the resulting matrix // 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, // these mark the currently set columns in the dense row buffer,
// used for when creating the resulting sparse row // used for when creating the resulting sparse row
src/engine/engine_vis_interact.c
+2 -2
View File
@@ -524,8 +524,8 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur
int sel = pert->select; int sel = pert->select;
mjtNum headpos[3], forward[3], dif[3]; mjtNum headpos[3], forward[3], dif[3];
mjtNum* jac = mj_stackAlloc(d, 3*nv); mjtNum* jac = mj_stackAllocNum(d, 3*nv);
mjtNum* jacM2 = mj_stackAlloc(d, 3*nv); mjtNum* jacM2 = mj_stackAllocNum(d, 3*nv);
// invalid selected body: return // invalid selected body: return
if (sel <= 0 || sel >= m->nbody) { if (sel <= 0 || sel >= m->nbody) {
test/benchmark/engine_core_smooth_benchmark_test.cc
+3 -3
View File
@@ -112,9 +112,9 @@ static void BM_solveLD(benchmark::State& state, bool new_function) {
// allocate gadient // allocate gadient
mjMARKSTACK; mjMARKSTACK;
mjtNum *grad = mj_stackAlloc(d, m->nv); mjtNum *grad = mj_stackAllocNum(d, m->nv);
mjtNum *Ma = mj_stackAlloc(d, m->nv); mjtNum *Ma = mj_stackAllocNum(d, m->nv);
mjtNum *res = mj_stackAlloc(d, m->nv); mjtNum *res = mj_stackAllocNum(d, m->nv);
// compute gradient // compute gradient
mj_mulM(m, d, Ma, d->qacc); mj_mulM(m, d, Ma, d->qacc);
test/benchmark/engine_util_sparse_benchmark_test.cc
+11 -11
View File
@@ -49,7 +49,7 @@ void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline(
const int* colindT, const int* rowsuperT, mjData* d) { const int* colindT, const int* rowsuperT, mjData* d) {
mjMARKSTACK; mjMARKSTACK;
int* chain = mj_stackAllocInt(d, 2 * nc); 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++) { for (int r = 0; r < nc; r++) {
res_rowadr[r] = r * nc; res_rowadr[r] = r * nc;
@@ -355,11 +355,11 @@ static void BM_MatVecSparse(benchmark::State& state, int unroll) {
// allocate gradient // allocate gradient
mjMARKSTACK; mjMARKSTACK;
mjtNum *Ma = mj_stackAlloc(d, m->nv); mjtNum *Ma = mj_stackAllocNum(d, m->nv);
mjtNum *vec = mj_stackAlloc(d, m->nv); mjtNum *vec = mj_stackAllocNum(d, m->nv);
mjtNum *res = mj_stackAlloc(d, d->nefc); mjtNum *res = mj_stackAllocNum(d, d->nefc);
mjtNum *grad = mj_stackAlloc(d, m->nv); mjtNum *grad = mj_stackAllocNum(d, m->nv);
mjtNum *Mgrad = mj_stackAlloc(d, m->nv); mjtNum *Mgrad = mj_stackAllocNum(d, m->nv);
// compute gradient // compute gradient
mj_mulM(m, d, Ma, d->qacc); mj_mulM(m, d, Ma, d->qacc);
@@ -434,13 +434,13 @@ static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) {
// allocate // allocate
mjMARKSTACK; 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* rownnz = mj_stackAllocInt(d, m->nv);
int* rowadr = mj_stackAllocInt(d, m->nv); int* rowadr = mj_stackAllocInt(d, m->nv);
int* colind = mj_stackAllocInt(d, m->nv*m->nv); int* colind = mj_stackAllocInt(d, m->nv*m->nv);
// compute D corresponding to quad states // 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++) { for (int i = 0; i < d->nefc; i++) {
if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) {
D[i] = d->efc_D[i]; D[i] = d->efc_D[i];
@@ -513,7 +513,7 @@ static void BM_transposeSparse(benchmark::State& state, TransposeFuncPtr func) {
mjMARKSTACK; mjMARKSTACK;
// need uncompressed layout // 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_rownnz = mj_stackAllocInt(d, m->nv);
int* res_rowadr = mj_stackAllocInt(d, m->nv); int* res_rowadr = mj_stackAllocInt(d, m->nv);
int* res_colind = mj_stackAllocInt(d, m->nv * d->nefc); int* res_colind = mj_stackAllocInt(d, m->nv * d->nefc);
@@ -557,13 +557,13 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) {
// allocate // allocate
mjMARKSTACK; 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* rownnz = mj_stackAllocInt(d, m->nv);
int* rowadr = mj_stackAllocInt(d, m->nv); int* rowadr = mj_stackAllocInt(d, m->nv);
int* colind = mj_stackAllocInt(d, m->nv * m->nv); int* colind = mj_stackAllocInt(d, m->nv * m->nv);
// compute D corresponding to quad states // 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++) { for (int i = 0; i < d->nefc; i++) {
if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) {
D[i] = d->efc_D[i]; D[i] = d->efc_D[i];
test/engine/engine_derivative_test.cc
+3 -3
View File
@@ -291,7 +291,7 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) {
// === state-transition matrix A // === state-transition matrix A
if (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)] // Ac = H^-1 [diag(-stiffness) diag(-damping)]
mju_zero(Ac, 2*nv*nv); mju_zero(Ac, 2*nv*nv);
for (int i=0; i < nv; i++) { 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 // === control-transition matrix B
if (B) { if (B) {
mjtNum *Bc = mj_stackAlloc(d, nu*nv); mjtNum *Bc = mj_stackAllocNum(d, nu*nv);
mjtNum *BcT = mj_stackAlloc(d, nv*nu); mjtNum *BcT = mj_stackAllocNum(d, nv*nu);
mju_copy(Bc, d->actuator_moment, nv*nu); mju_copy(Bc, d->actuator_moment, nv*nu);
mj_solveLD(m, Bc, nu, d->qH, d->qHDiagInv); mj_solveLD(m, Bc, nu, d->qH, d->qHDiagInv);
mju_transpose(BcT, Bc, nu, nv); mju_transpose(BcT, Bc, nu, nv);
unity/Runtime/Bindings/MjBindings.cs
+1 -1
View File
@@ -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); public static unsafe extern void mj_resetDataKeyframe(mjModel_* m, mjData_* d, int key);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] [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)] [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern int* mj_stackAllocInt(mjData_* d, int size); public static unsafe extern int* mj_stackAllocInt(mjData_* d, int size);