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

Remove mjMARKSTACK and mjFREESTACK macros.

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