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

Merge branch 'deepmind:main' into usd-integration

Browse Source
This commit is contained in:
Abhishek Joshi
2023-09-06 14:04:36 -05:00
committed by GitHub
parent ccb978a74f 040d3efce2
commit 4e58b0619b
122 changed files with 4019 additions and 1524 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+2
View File
@@ -59,6 +59,7 @@ set(MUJOCO_HEADERS
include/mujoco/mjmodel.h
include/mujoco/mjplugin.h
include/mujoco/mjrender.h
include/mujoco/mjthread.h
include/mujoco/mjtnum.h
include/mujoco/mjui.h
include/mujoco/mjvisualize.h
@@ -88,6 +89,7 @@ add_subdirectory(src/engine)
add_subdirectory(src/user)
add_subdirectory(src/xml)
add_subdirectory(src/render)
add_subdirectory(src/thread)
add_subdirectory(src/ui)
target_compile_definitions(mujoco PRIVATE _GNU_SOURCE CCD_STATIC_DEFINE MUJOCO_DLL_EXPORTS -DMC_IMPLEM_ENABLE)
cmake/MujocoOptions.cmake
+6 -7
View File
@@ -88,8 +88,10 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang
set(EXTRA_COMPILE_OPTIONS
-Werror
-Wall
-Wpedantic
-Wimplicit-fallthrough
-Wunused
-Wvla
-Wno-int-in-bool-context
-Wno-sign-compare
-Wno-unknown-pragmas
@@ -100,15 +102,12 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang
-Wno-maybe-uninitialized
)
endif()
if (CMAKE_CXX_COMPILER_ID MATCHES "Clang" AND NOT MSVC)
set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} -Wgnu-empty-initializer)
endif()
endif()
if(WIN32)
add_compile_definitions(_CRT_SECURE_NO_WARNINGS)
endif()
include(MujocoHarden)
set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} ${MUJOCO_HARDEN_COMPILE_OPTIONS})
set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} ${MUJOCO_HARDEN_LINK_OPTIONS})
if(WIN32)
add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_DEPRECATE)
endif()
doc/APIreference/APIfunctions.rst
+1 -33
View File
@@ -32,6 +32,7 @@ API function can be classified as:
- :ref:`Derivatives<Derivatives-api>`.
- :ref:`Plugin<Plugins-api>` related functions.
- :ref:`Macros<Macros>`.
- :ref:`Thread<Thread>` related functions.
.. TODO(b/273075045): Better category-label namespacing.
@@ -42,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/APIreference/APItypes.rst
+33
View File
@@ -712,7 +712,26 @@ Options for configuring the automatic :ref:`actuator length-range computation<CL
.. mujoco-include:: mjLROpt
.. _mjTask:
mjTask
~~~~~~
This is a representation of a task to be run asynchronously inside of an :ref:`mjThreadPool` . It is created in the
:ref:`mju_threadPoolEnqueue` method of the :ref:`mjThreadPool` and is used to join the task at completion.
.. mujoco-include:: mjTask
.. _mjThreadPool:
mjThreadPool
~~~~~~~~~~~~
This is the data structure of the threadpool. It can only be constructed programmatically, and does not
have an analog in MJCF. In order to enable multi-threaded calculations, a pointer to an existing :ref:`mjThreadPool`
should be assigned to the ``mjData.threadpool``.
.. mujoco-include:: mjThreadPool
.. _tyStatStructure:
@@ -1155,6 +1174,20 @@ mjfGetResourceDir
This callback is for returning the directory of a resource, by setting dir to the directory string with ndir being size
of directory string.
.. _mjfResourceModified:
mjfResourceModified
~~~~~~~~~~~~~~~~~~~
.. code-block:: C
typedef int (*mjfResourceModified)(const mjResource* resource);
This callback is for checking if a resource was modified since it was last read.
Returns positive value if the resource was modified since last open, 0 if resource was not modified,
and negative value if inconclusive.
.. _tyNotes:
Notes
doc/APIreference/functions.rst
+61 -1
View File
@@ -1232,6 +1232,25 @@ mj_resetDataKeyframe
Reset data, set fields from specified keyframe.
.. _mj_markStack:
mj_markStack
~~~~~~~~~~~~
.. mujoco-include:: mj_markStack
Mark a new frame on the :ref:`mjData` stack.
.. _mj_freeStack:
mj_freeStack
~~~~~~~~~~~~
.. mujoco-include:: mj_freeStack
Free the current :ref:`mjData` stack frame. All pointers returned by mj_stackAlloc since the last call
to mj_markStack must no longer be used afterwards.
.. _mj_stackAlloc:
mj_stackAlloc
@@ -1239,7 +1258,8 @@ mj_stackAlloc
.. mujoco-include:: mj_stackAlloc
Allocate a specific number of bytes on :ref:`mjData` stack. Call mju_error on stack overflow.
Allocate a number of bytes on :ref:`mjData` stack at a specific alignment.
Call mju_error on stack overflow.
.. _mj_stackAllocNum:
@@ -3413,3 +3433,43 @@ mjp_getResourceProviderAtSlot
Look up a resource provider by slot number returned by mjp_registerResourceProvider.
If invalid slot number, return NULL.
.. _Thread:
Thread
^^^^^^
.. _mju_threadPoolCreate:
mju_threadPoolCreate
~~~~~~~~~~~~~~~~~~~~
.. mujoco-include:: mju_threadPoolCreate
Creates a thread pool with the specified number of threads running.
.. _mju_threadPoolEnqueue:
mju_threadPoolEnqueue
~~~~~~~~~~~~~~~~~~~~~
.. mujoco-include:: mju_threadPoolEnqueue
Enqueues a task in a thread pool.
.. _mju_taskJoin:
mju_taskJoin
~~~~~~~~~~~~
.. mujoco-include:: mju_taskJoin
Waits for a task to complete.
.. _mju_threadPoolDestroy:
mju_threadPoolDestroy
~~~~~~~~~~~~~~~~~~~~~
.. mujoco-include:: mju_threadPoolDestroy
Destroys a thread pool.
doc/XMLreference.rst
+17
View File
@@ -1972,6 +1972,18 @@ adjust it properly through the XML.
improvement between two iterations. For CG and Newton, it is applied to the smaller of the cost improvement and the
gradient norm. Set the tolerance to 0 to disable early termination.
.. _option-ls_iterations:
:at:`ls_iterations`: :at-val:`int, "50"`
Maximum number of linesearch iterations performed by CG/Newton constraint solvers. Ensures that at most
:ref:`iterations<option-iterations>` times :ref:`ls_iterations<option-ls_iterations>` linesearch iterations are
performed during each constraint solve.
.. _option-ls_tolerance:
:at:`ls_tolerance`: :at-val:`real, "0.01"`
Tolerance threshold used for early termination of the linesearch algorithm.
.. _option-noslip_iterations:
:at:`noslip_iterations`: :at-val:`int, "0"`
@@ -3319,6 +3331,11 @@ coordinates results in compiler error. See :ref:`CComposite` in the modeling gui
:at:`vertex`: :at-val:`real(3*nvert), optional`
Vertex 3D positions in global coordinates (cable only).
.. _body-composite-face:
:at:`face`: :at-val:`real(3*nvert), optional`
Face connectivity of the vertices (shell only).
.. _body-composite-initial:
:at:`initial`: :at-val:`[free, ball, none], "0"`
doc/XMLschema.rst
+10 -6
View File
@@ -218,15 +218,17 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`timestep<option-timestep>` | :ref:`apirate<option-apirate>` | :ref:`impratio<option-impratio>` | :ref:`tolerance<option-tolerance>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`noslip_tolerance<option-noslip_tolerance>` | :ref:`mpr_tolerance<option-mpr_tolerance>` | :ref:`gravity<option-gravity>` | :ref:`wind<option-wind>` | |
| | | | :ref:`ls_tolerance<option-ls_tolerance>` | :ref:`noslip_tolerance<option-noslip_tolerance>` | :ref:`mpr_tolerance<option-mpr_tolerance>` | :ref:`gravity<option-gravity>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`magnetic<option-magnetic>` | :ref:`density<option-density>` | :ref:`viscosity<option-viscosity>` | :ref:`o_margin<option-o_margin>` | |
| | | | :ref:`wind<option-wind>` | :ref:`magnetic<option-magnetic>` | :ref:`density<option-density>` | :ref:`viscosity<option-viscosity>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`o_solref<option-o_solref>` | :ref:`o_solimp<option-o_solimp>` | :ref:`integrator<option-integrator>` | :ref:`collision<option-collision>` | |
| | | | :ref:`o_margin<option-o_margin>` | :ref:`o_solref<option-o_solref>` | :ref:`o_solimp<option-o_solimp>` | :ref:`integrator<option-integrator>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`cone<option-cone>` | :ref:`jacobian<option-jacobian>` | :ref:`solver<option-solver>` | :ref:`iterations<option-iterations>` | |
| | | | :ref:`collision<option-collision>` | :ref:`cone<option-cone>` | :ref:`jacobian<option-jacobian>` | :ref:`solver<option-solver>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`noslip_iterations<option-noslip_iterations>` | :ref:`mpr_iterations<option-mpr_iterations>` | :ref:`sdf_iterations<option-sdf_iterations>` | :ref:`sdf_initpoints<option-sdf_initpoints>` | |
| | | | :ref:`iterations<option-iterations>` | :ref:`ls_iterations<option-ls_iterations>` | :ref:`noslip_iterations<option-noslip_iterations>` | :ref:`mpr_iterations<option-mpr_iterations>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`sdf_iterations<option-sdf_iterations>` | :ref:`sdf_initpoints<option-sdf_initpoints>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| option |br| |_| |L| | | .. table:: |
@@ -390,7 +392,9 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`offset<body-composite-offset>` | :ref:`flatinertia<body-composite-flatinertia>` | :ref:`solrefsmooth<body-composite-solrefsmooth>` | :ref:`solimpsmooth<body-composite-solimpsmooth>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`vertex<body-composite-vertex>` | :ref:`initial<body-composite-initial>` | :ref:`curve<body-composite-curve>` | :ref:`size<body-composite-size>` | |
| | | | :ref:`vertex<body-composite-vertex>` | :ref:`face<body-composite-face>` | :ref:`initial<body-composite-initial>` | :ref:`curve<body-composite-curve>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`size<body-composite-size>` | | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_2| composite |br| |_2| |L| | | .. table:: |
doc/changelog.rst
+46 -25
View File
@@ -5,55 +5,76 @@ 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 the 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 the macros ``mjMARKSTACK`` and ``mjFREESTACK``.
**Migration:** These macros have been replaced by new functions :ref:`mj_markStack` and
:ref:`mj_freeStack`. These functions manage the :ref:`mjData stack<siStack>` in a fully encapsulated way (i.e.,
without introducing 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 added an additional argument for specifying the alignment of the returned pointer.
**Migration:** The functionality for allocating ``mjtNum`` arrays is available via :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 ``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 ``mj_stackAllocNum``.
#. Renamed the ``nstack`` field in ``mjModel`` and ``mjData`` to ``narena``. Changed ``narena``, ``pstack``, and
``maxuse_stack`` to count number of bytes rather than number of ``mjtNum``s.
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.
12. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_iterations<option-ls_tolerance>` options for adjusting
linesearch stopping criteria in CG and Newton solvers. This can be useful for performance tuning.
Python bindings
^^^^^^^^^^^^^^^
10. Fixed `#870 <https://github.com/deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
13. 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.
14. 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
+50 -36
View File
@@ -130,6 +130,7 @@ struct mjData_ {
// stack pointer
size_t pstack; // first available mjtNum address in stack
size_t pbase; // value of pstack when mj_markStack was last called
// arena pointer
size_t parena; // first available byte in arena
@@ -374,6 +375,9 @@ struct mjData_ {
mjtNum* efc_b; // linear cost term: J*qacc_smooth - aref (nefc x 1)
mjtNum* efc_force; // constraint force in constraint space (nefc x 1)
int* efc_state; // constraint state (mjtConstraintState) (nefc x 1)
// ThreadPool for multithreaded operations
uintptr_t threadpool;
};
typedef struct mjData_ mjData;
typedef enum mjtDisableBit_ { // disable default feature bitflags
@@ -655,28 +659,13 @@ struct mjLROpt_ { // options for mj_setLengthRange()
mjtNum tolrange; // convergence tolerance (relative to range)
};
typedef struct mjLROpt_ mjLROpt;
struct mjVFS_ { // virtual file system for loading from memory
int nfile; // number of files present
char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path
int filesize[mjMAXVFS]; // file size in bytes
void* filedata[mjMAXVFS]; // buffer with file data
struct mjVFS_ { // virtual file system for loading from memory
int nfile; // number of files present
char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path
size_t filesize[mjMAXVFS]; // file size in bytes
void* filedata[mjMAXVFS]; // buffer with file data
};
typedef struct mjVFS_ mjVFS;
struct mjResource_ {
char* name; // name of resource (filename, etc)
void* data; // opaque data pointer
const void* provider_data; // opaque resource provider data
// reading callback from resource provider
int (*read)(struct mjResource_* resource, const void** buffer);
// closing callback from resource provider
void (*close)(struct mjResource_* resource);
// getdir callback from resource provider
void (*getdir)(struct mjResource_* resource, const char** dir, int* ndir);
};
typedef struct mjResource_ mjResource;
struct mjOption_ { // physics options
// timing parameters
mjtNum timestep; // timestep
@@ -685,6 +674,7 @@ struct mjOption_ { // physics options
// solver parameters
mjtNum impratio; // ratio of friction-to-normal contact impedance
mjtNum tolerance; // main solver tolerance
mjtNum ls_tolerance; // CG/Newton linesearch tolerance
mjtNum noslip_tolerance; // noslip solver tolerance
mjtNum mpr_tolerance; // MPR solver tolerance
@@ -707,6 +697,7 @@ struct mjOption_ { // physics options
int jacobian; // type of Jacobian (mjtJacobian)
int solver; // solver algorithm (mjtSolver)
int iterations; // maximum number of main solver iterations
int ls_iterations; // maximum number of CG/Newton linesearch iterations
int noslip_iterations; // maximum number of noslip solver iterations
int mpr_iterations; // maximum number of MPR solver iterations
int disableflags; // bit flags for disabling standard features
@@ -1262,15 +1253,22 @@ struct mjModel_ {
int* names_map; // internal hash map of names (nnames_map x 1)
};
typedef struct mjModel_ mjModel;
struct mjpResourceProvider_ {
const char* prefix; // prefix for match against a resource name
mjfOpenResource open; // opening callback
mjfReadResource read; // reading callback
mjfCloseResource close; // closing callback
mjfGetResourceDir getdir; // getdir callback (optional)
void* data; // opaque data pointer (resource invariant)
struct mjResource_ {
char* name; // name of resource (filename, etc)
void* data; // opaque data pointer
const struct mjpResourceProvider* provider; // pointer to the provider
};
typedef struct mjpResourceProvider_ mjpResourceProvider;
typedef struct mjResource_ mjResource;
struct mjpResourceProvider {
const char* prefix; // prefix for match against a resource name
mjfOpenResource open; // opening callback
mjfReadResource read; // reading callback
mjfCloseResource close; // closing callback
mjfGetResourceDir getdir; // get directory callback (optional)
mjfResourceModified modified; // resource modified callback (optional)
void* data; // opaque data pointer (resource invariant)
};
typedef struct mjpResourceProvider mjpResourceProvider;
typedef enum mjtPluginCapabilityBit_ {
mjPLUGIN_ACTUATOR = 1<<0, // actuator forces
mjPLUGIN_SENSOR = 1<<1, // sensor measurements
@@ -1443,6 +1441,14 @@ struct mjrContext_ { // custom OpenGL context
int readPixelFormat; // default color pixel format for mjr_readPixels
};
typedef struct mjrContext_ mjrContext;
struct mjTask_ {
char buffer[24];
};
typedef struct mjTask_ mjTask;
struct mjThreadPool_ {
char buffer[6208];
};
typedef struct mjThreadPool_ mjThreadPool;
typedef enum mjtButton_ { // mouse button
mjBUTTON_NONE = 0, // no button
mjBUTTON_LEFT, // left button
@@ -2213,6 +2219,8 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src);
void mj_resetData(const mjModel* m, mjData* d);
void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value);
void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
void mj_markStack(mjData* d);
void mj_freeStack(mjData* d);
void* mj_stackAlloc(mjData* d, size_t bytes, size_t alignment);
mjtNum* mj_stackAllocNum(mjData* d, int size);
int* mj_stackAllocInt(mjData* d, int size);
@@ -2276,8 +2284,8 @@ int mj_addContact(const mjModel* m, mjData* d, const mjContact* con);
int mj_isPyramidal(const mjModel* m);
int mj_isSparse(const mjModel* m);
int mj_isDual(const mjModel* m);
void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
void mj_jac(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr,
const mjtNum point[3], int body);
void mj_jacBody(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int body);
@@ -2312,7 +2320,7 @@ const char* mj_getPluginConfig(const mjModel* m, int plugin_id, const char* attr
void mj_loadPluginLibrary(const char* path);
void mj_loadAllPluginLibraries(const char* directory, mjfPluginLibraryLoadCallback callback);
int mj_version(void);
const char* mj_versionString();
const char* mj_versionString(void);
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);
@@ -2450,7 +2458,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]);
@@ -2460,10 +2468,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);
@@ -2569,12 +2577,18 @@ void mjd_quatIntegrate(const mjtNum vel[3], mjtNum scale,
mjtNum Dquat[9], mjtNum Dvel[9], mjtNum Dscale[3]);
void mjp_defaultPlugin(mjpPlugin* plugin);
int mjp_registerPlugin(const mjpPlugin* plugin);
int mjp_pluginCount();
int mjp_pluginCount(void);
const mjpPlugin* mjp_getPlugin(const char* name, int* slot);
const mjpPlugin* mjp_getPluginAtSlot(int slot);
void mjp_defaultResourceProvider(mjpResourceProvider* provider);
int mjp_registerResourceProvider(const mjpResourceProvider* provider);
int mjp_resourceProviderCount();
int mjp_resourceProviderCount(void);
const mjpResourceProvider* mjp_getResourceProvider(const char* resource_name);
const mjpResourceProvider* mjp_getResourceProviderAtSlot(int slot);
mjThreadPool* mju_threadPoolCreate(size_t number_of_threads);
void mju_threadPoolEnqueue(
ThreadPool* thread_pool, mjTask* task, void*(start_routine)(void*),
id* args);
void mju_taskJoin(mjTask* task);
void mju_threadPoolDestroy(mjThreadPool* thread_pool);
// NOLINTEND
doc/overview.rst
+227 -174
View File
@@ -239,8 +239,7 @@ The function :ref:`mj_step` is the top-level function which advances the simulat
of course is just a passive dynamical system. Things get more interesting when the user specifies controls or applies
forces and starts interacting with the system.
Next we provide a more elaborate example illustrating several features of MJCF.
Next we provide a more elaborate example illustrating several features of MJCF. Consider the following
`example.xml <_static/example.xml>`__:
.. code:: xml
@@ -332,18 +331,24 @@ XML file, default values are used. The options are designed such that the user c
simulation time step. Within a time step however none of the options should be changed.
``mjOption``
This structure contains all options that affect the physics simulation. It is used to select algorithms and set their
parameters, enable and disable different portions of the simulation pipeline, and adjust system-level physical
properties such as gravity.
^^^^^^^^^^^^
This structure contains all options that affect the physics simulation. It is used to select algorithms and set their
parameters, enable and disable different portions of the simulation pipeline, and adjust system-level physical
properties such as gravity.
``mjVisual``
This structure contains all visualization options. There are additional OpenGL rendering options, but these are
session-dependent and are not part of the model.
^^^^^^^^^^^^
This structure contains all visualization options. There are additional OpenGL rendering options, but these are
session-dependent and are not part of the model.
``mjStatistic``
This structure contains statistics about the model which are computed by the compiler: average body mass, spatial
extent of the model etc. It is included for information purposes, and also because the visualizer uses it for
automatic scaling.
^^^^^^^^^^^^^^^
This structure contains statistics about the model which are computed by the compiler: average body mass, spatial
extent of the model etc. It is included for information purposes, and also because the visualizer uses it for
automatic scaling.
.. _Assets:
@@ -357,51 +362,61 @@ purpose of including an asset is to reference it, and referencing can only be do
undefined.
Mesh
MuJoCo can load triangulated meshes from OBJ files and binary STL. Software such as `MeshLab
<https://www.meshlab.net/>`__ can be used to convert from other formats. While any collection of triangles can be
loaded and visualized as a mesh, the collision detector works with the convex hull. There are compile-time options
for scaling the mesh, as well as fitting a primitive geometric shape to it. The mesh can also be used to
automatically infer inertial properties -- by treating it as a union of triangular pyramids and combining their
masses and inertias. Note that meshes have no color, instead the mesh is colored using the material properties of the
referencing geom. In contrast, all spatial properties are determined by the mesh data. MuJoCo supports both OBJ and a
custom binary file format for normals and texture coordinates. Meshes can also be embedded directly in the XML.
^^^^
MuJoCo can load triangulated meshes from OBJ files and binary STL. Software such as `MeshLab
<https://www.meshlab.net/>`__ can be used to convert from other formats. While any collection of triangles can be
loaded and visualized as a mesh, the collision detector works with the convex hull. There are compile-time options
for scaling the mesh, as well as fitting a primitive geometric shape to it. The mesh can also be used to
automatically infer inertial properties -- by treating it as a union of triangular pyramids and combining their
masses and inertias. Note that meshes have no color, instead the mesh is colored using the material properties of the
referencing geom. In contrast, all spatial properties are determined by the mesh data. MuJoCo supports both OBJ and a
custom binary file format for normals and texture coordinates. Meshes can also be embedded directly in the XML.
Skin
Skinned meshes (or skins) are meshes whose shape can deform at runtime. Their vertices are attached to rigid bodies
(called bones in this context) and each vertex can belong to multiple bones, resulting in smooth deformations of the
skin. Skins are purely visualization objects and do not affect the physics, but nevertheless they can enhance visual
realism significantly. Skins can be loaded from custom binary files, or embedded directly in the XML, similar to
meshes. When generating composite flexible objects automatically, the model compiler also generates skins for these
objects.
^^^^
Skinned meshes (or skins) are meshes whose shape can deform at runtime. Their vertices are attached to rigid bodies
(called bones in this context) and each vertex can belong to multiple bones, resulting in smooth deformations of the
skin. Skins are purely visualization objects and do not affect the physics, but nevertheless they can enhance visual
realism significantly. Skins can be loaded from custom binary files, or embedded directly in the XML, similar to
meshes. When generating composite flexible objects automatically, the model compiler also generates skins for these
objects.
Height field
Height fields can be loaded from PNG files (converted to gray-scale internally) or from files in a custom binary
format described later. A height field is a rectangular grid of elevation data. The compiler normalizes the data to
the range [0-1]. The actual spatial extent of the height field is then determined by the size parameters of the
referencing geom. Height fields can only be referenced from geoms that are attached to the world body. For rendering
and collision detection purposes, the grid rectangles are automatically triangulated, thus the height field is
treated as a union of triangular prisms. Collision detection with such a composite object can in principle generate a
large number of contact points for a single geom pair. If that happens, only the first 64 contact points are kept.
The rationale is that height fields should be used to model terrain maps whose spatial features are large compared to
the other objects in the simulation, so the number of contacts will be small for well-designed models.
^^^^^^^^^^^^
Height fields can be loaded from PNG files (converted to gray-scale internally) or from files in a custom binary
format described later. A height field is a rectangular grid of elevation data. The compiler normalizes the data to
the range [0-1]. The actual spatial extent of the height field is then determined by the size parameters of the
referencing geom. Height fields can only be referenced from geoms that are attached to the world body. For rendering
and collision detection purposes, the grid rectangles are automatically triangulated, thus the height field is
treated as a union of triangular prisms. Collision detection with such a composite object can in principle generate a
large number of contact points for a single geom pair. If that happens, only the first 64 contact points are kept.
The rationale is that height fields should be used to model terrain maps whose spatial features are large compared to
the other objects in the simulation, so the number of contacts will be small for well-designed models.
Texture
Textures can be loaded from PNG files or synthesized by the compiler based on user-defined procedural parameters.
There is also the option to leave the texture empty at model creation time and change it later at runtime -- so as to
render video in a MuJoCo simulation, or create other dynamic effects. The visualizer supports two types of texture
mapping: 2D and cube. 2D mapping is useful for planes and height fields. Cube mapping is useful for "shrink-wrapping"
textures around 3D objects without having to specify texture coordinates. It is also used to create a skybox. The six
sides of a cube maps can be loaded from separate image files, or from one composite image file, or generated by
repeating the same image. Unlike all other assets which are referenced directly from model elements, textures can
only be referenced from another asset (namely material) which is then referenced from model elements.
^^^^^^^
Textures can be loaded from PNG files or synthesized by the compiler based on user-defined procedural parameters.
There is also the option to leave the texture empty at model creation time and change it later at runtime -- so as to
render video in a MuJoCo simulation, or create other dynamic effects. The visualizer supports two types of texture
mapping: 2D and cube. 2D mapping is useful for planes and height fields. Cube mapping is useful for "shrink-wrapping"
textures around 3D objects without having to specify texture coordinates. It is also used to create a skybox. The six
sides of a cube maps can be loaded from separate image files, or from one composite image file, or generated by
repeating the same image. Unlike all other assets which are referenced directly from model elements, textures can
only be referenced from another asset (namely material) which is then referenced from model elements.
Material
Materials are used to control the appearance of geoms, sites and tendons. This is done by referencing the material
from the corresponding model element. Appearance includes texture mapping as well as other properties that interact
with OpenGL lights below: RGBA, specularity, shininess, emission. Materials can also be used to make objects
reflective. Currently reflections are rendered only on planes and on the Z+ faces of boxes. Note that model elements
can also have their local RGBA parameter for setting color. If both material and local RGBA are specified, the local
definition has precedence.
^^^^^^^^
Materials are used to control the appearance of geoms, sites and tendons. This is done by referencing the material
from the corresponding model element. Appearance includes texture mapping as well as other properties that interact
with OpenGL lights below: RGBA, specularity, shininess, emission. Materials can also be used to make objects
reflective. Currently reflections are rendered only on planes and on the Z+ faces of boxes. Note that model elements
can also have their local RGBA parameter for setting color. If both material and local RGBA are specified, the local
definition has precedence.
.. _Kinematic:
@@ -417,171 +432,209 @@ within a body and belong to that body. This is in contrast with the stand-alone
associated with a single body.
Body
Bodies have mass and inertial properties but do not have any geometric properties. Instead geometric shapes (or
geoms) are attached to the bodies. Each body has two coordinate frames: the frame used to define it as well as to
position other elements relative to it, and an inertial frame centered at the body's center of mass and aligned with
its principal axes of inertia. The body inertia matrix is therefore diagonal in this frame. At each time step MuJoCo
computes the forward kinematics recursively, yielding all body positions and orientations in global Cartesian
coordinates. This provides the basis for all subsequent computations.
^^^^
Bodies have mass and inertial properties but do not have any geometric properties. Instead geometric shapes (or
geoms) are attached to the bodies. Each body has two coordinate frames: the frame used to define it as well as to
position other elements relative to it, and an inertial frame centered at the body's center of mass and aligned with
its principal axes of inertia. The body inertia matrix is therefore diagonal in this frame. At each time step MuJoCo
computes the forward kinematics recursively, yielding all body positions and orientations in global Cartesian
coordinates. This provides the basis for all subsequent computations.
Joint
Joints are defined within bodies. They create motion degrees of freedom (DOFs) between the body and its parent. In
the absence of joints the body is welded to its parent. This is the opposite of gaming engines which use
over-complete Cartesian coordinates, where joints remove DOFs instead of adding them. There are four types of joints:
ball, slide, hinge, and a "free joint" which creates floating bodies. A single body can have multiple joints. In this
way composite joints are created automatically, without having to define dummy bodies. The orientation components of
ball and free joints are represented as unit quaternions, and all computations in MuJoCo respect the properties of
quaternions.
^^^^^
Joints are defined within bodies. They create motion degrees of freedom (DOFs) between the body and its parent. In
the absence of joints the body is welded to its parent. This is the opposite of gaming engines which use
over-complete Cartesian coordinates, where joints remove DOFs instead of adding them. There are four types of joints:
ball, slide, hinge, and a "free joint" which creates floating bodies. A single body can have multiple joints. In this
way composite joints are created automatically, without having to define dummy bodies. The orientation components of
ball and free joints are represented as unit quaternions, and all computations in MuJoCo respect the properties of
quaternions.
Joint reference
'''''''''''''''
The reference pose is a vector of joint positions stored in ``mjModel.qpos0``. It corresponds to the numeric values
of the joints when the model is in its initial configuration. In our earlier example the elbow was created in a bent
configuration at 90° angle. But MuJoCo does not know what an elbow is, and so by default it treats this joint
configuration as having numeric value of 0. We can override the default behavior and specify that the initial
configuration corresponds to 90°, using the ref attribute of :ref:`joint <body-joint>`. The reference values of all
joints are assembled into the vector ``mjModel.qpos0``. Whenever the simulation is reset, the joint configuration
``mjData.qpos`` is set to ``mjModel.qpos0``. At runtime the joint position vector is interpreted relative to the
reference pose. In particular, the amount of spatial transformation applied by the joints is ``mjData.qpos -
mjModel.qpos0``. This transformation is in addition to the parent-child translation and rotation offsets stored in
the body elements of ``mjModel``. The ref attribute only applies to scalar joints (slide and hinge). For ball joints,
the quaternion saved in ``mjModel.qpos0`` is always (1,0,0,0) which corresponds to the null rotation. For free
joints, the global 3D position and quaternion of the floating body are saved in ``mjModel.qpos0``.
Spring reference
''''''''''''''''
This is the pose in which all joint and tendon springs achieve their resting length. Spring forces are generated
when the joint configuration deviates from the spring reference pose, and are linear in the amount of deviation. The
spring reference pose is saved in ``mjModel.qpos_spring``. For slide and hinge joints, the spring reference is
specified with the attribute springref. For ball and free joints, the spring reference corresponds to the initial
model configuration.
DOF
Degrees of freedom are closely related to joints, but are not in one-to-one correspondence because ball and free
joints have multiple DOFs. Think of joints as specifying positional information, and of DOFs as specifying velocity
and force information. More formally, the joint positions are coordinates over the configuration manifold of the
system, while the joint velocities are coordinates over the tangent space to this manifold at the current position.
DOFs have velocity-related properties such as friction loss, damping, armature inertia. All generalized forces acting
on the system are expressed in the space of DOFs. In contrast, joints have position-related properties such as limits
and spring stiffness. DOFs are not specified directly by the user. Instead they are created by the compiler given the
joints.
^^^
Degrees of freedom are closely related to joints, but are not in one-to-one correspondence because ball and free
joints have multiple DOFs. Think of joints as specifying positional information, and of DOFs as specifying velocity
and force information. More formally, the joint positions are coordinates over the configuration manifold of the
system, while the joint velocities are coordinates over the tangent space to this manifold at the current position.
DOFs have velocity-related properties such as friction loss, damping, armature inertia. All generalized forces acting
on the system are expressed in the space of DOFs. In contrast, joints have position-related properties such as limits
and spring stiffness. DOFs are not specified directly by the user. Instead they are created by the compiler given the
joints.
Geom
Geoms are 3D shapes rigidly attached to the bodies. Multiple geoms can be attached to the same body. This is
particularly useful in light of the fact that MuJoCo only supports convex geom-geom collisions, and the only way to
create non-convex objects is to represent them as a union of convex geoms. Apart from collision detection and
subsequent computation of contact forces, geoms are used for rendering, as well as automatic inference of body masses
and inertias when the latter are omitted. MuJoCo supports several primitive geometric shapes: plane, sphere, capsule,
ellipsoid, cylinder, box. A geom can also be a mesh or a height field; this is done by referencing the corresponding
asset. Geoms have a number of material properties that affect the simulation and visualization.
^^^^
Geoms are 3D shapes rigidly attached to the bodies. Multiple geoms can be attached to the same body. This is
particularly useful in light of the fact that MuJoCo only supports convex geom-geom collisions, and the only way to
create non-convex objects is to represent them as a union of convex geoms. Apart from collision detection and
subsequent computation of contact forces, geoms are used for rendering, as well as automatic inference of body masses
and inertias when the latter are omitted. MuJoCo supports several primitive geometric shapes: plane, sphere, capsule,
ellipsoid, cylinder, box. A geom can also be a mesh or a height field; this is done by referencing the corresponding
asset. Geoms have a number of material properties that affect the simulation and visualization.
Site
Sites are essentially light geoms. They represent locations of interest within the body frame. Sites do not
participate in collision detection or automated computation of inertial properties, however they can be used to
specify the spatial properties of other objects like sensors, tendon routing, and slider-crank endpoints.
^^^^
Sites are essentially light geoms. They represent locations of interest within the body frame. Sites do not
participate in collision detection or automated computation of inertial properties, however they can be used to
specify the spatial properties of other objects like sensors, tendon routing, and slider-crank endpoints.
Camera
Multiple cameras can be defined in a model. There is always a default camera which the user can freely move with the
mouse in the interactive visualizer. However it is often convenient to define additional cameras that are either
fixed to the world, or are attached to one of the bodies and move with it. In addition to the camera position and
orientation, the user can adjust the field of view and the inter-pupilary distance for stereoscopic rendering, as
well as create oblique projections needed for stereoscopic virtual environments.
^^^^^^
Multiple cameras can be defined in a model. There is always a default camera which the user can freely move with the
mouse in the interactive visualizer. However it is often convenient to define additional cameras that are either
fixed to the world, or are attached to one of the bodies and move with it. In addition to the camera position and
orientation, the user can adjust the field of view and the inter-pupilary distance for stereoscopic rendering, as
well as create oblique projections needed for stereoscopic virtual environments.
Light
Lights can be fixed to the world body or attached to moving bodies. The visualizer provides access to the full
lighting model in OpenGL (fixed function) including ambient, diffuse and specular components, attenuation and cutoff,
positional and directional lighting, fog. Lights, or rather the objects illuminated by them, can also cast shadows.
However, similar to material reflections, each shadow-casting light adds one rendering pass so this feature should be
used with caution. Documenting the lighting model in detail is beyond the scope of this chapter; see `OpenGL
documentation <http://www.glprogramming.com/red/chapter05.html>`__ instead. Note that in addition to lights defined
by the user in the kinematic tree, there is a default headlight that moves with the camera. Its properties are
adjusted through the mjVisual options.
^^^^^
Lights can be fixed to the world body or attached to moving bodies. The visualizer provides access to the full
lighting model in OpenGL (fixed function) including ambient, diffuse and specular components, attenuation and cutoff,
positional and directional lighting, fog. Lights, or rather the objects illuminated by them, can also cast shadows.
However, similar to material reflections, each shadow-casting light adds one rendering pass so this feature should be
used with caution. Documenting the lighting model in detail is beyond the scope of this chapter; see `OpenGL
documentation <http://www.glprogramming.com/red/chapter05.html>`__ instead. Note that in addition to lights defined
by the user in the kinematic tree, there is a default headlight that moves with the camera. Its properties are
adjusted through the mjVisual options.
.. _Standalone:
Stand-alone elements
~~~~~~~~~~~~~~~~~~~~
Stand-alone
~~~~~~~~~~~
Here we describe the model elements which do not belong to an individual body, and therefore are described outside the
kinematic tree.
Reference pose
The reference pose is a vector of joint positions stored in ``mjModel.qpos0``. It corresponds to the numeric values
of the joints when the model is in its initial configuration. In our earlier example the elbow was created in a bent
configuration at 90° angle. But MuJoCo does not know what an elbow is, and so by default it treats this joint
configuration as having numeric value of 0. We can override the default behavior and specify that the initial
configuration corresponds to 90°, using the ref attribute of :ref:`joint <body-joint>`. The reference values of all
joints are assembled into the vector ``mjModel.qpos0``. Whenever the simulation is reset, the joint configuration
``mjData.qpos`` is set to ``mjModel.qpos0``. At runtime the joint position vector is interpreted relative to the
reference pose. In particular, the amount of spatial transformation applied by the joints is ``mjData.qpos -
mjModel.qpos0``. This transformation is in addition to the parent-child translation and rotation offsets stored in
the body elements of ``mjModel``. The ref attribute only applies to scalar joints (slide and hinge). For ball joints,
the quaternion saved in ``mjModel.qpos0`` is always (1,0,0,0) which corresponds to the null rotation. For free
joints, the global 3D position and quaternion of the floating body are saved in ``mjModel.qpos0``.
Spring reference pose
This is the pose in which all joint and tendon springs achieve their resting length. Spring forces are generated
when the joint configuration deviates from the spring reference pose, and are linear in the amount of deviation. The
spring reference pose is saved in ``mjModel.qpos_spring``. For slide and hinge joints, the spring reference is
specified with the attribute springref. For ball and free joints, the spring reference corresponds to the initial
model configuration.
Tendon
Tendons are scalar length elements that can be used for actuation, imposing limits and equality constraints, or
creating spring-dampers and friction loss. There are two types of tendons: fixed and spatial. Fixed tendons are
linear combinations of (scalar) joint positions. They are useful for modeling mechanical coupling. Spatial tendons
are defined as the shortest path that passes through a sequence of specified sites (or via-points) or wraps around
specified geoms. Only spheres and cylinders are supported as wrapping geoms, and cylinders are treated as having
infinite length for wrapping purposes. To avoid abrupt jumps of the tendon from one side of the wrapping geom to the
other, the user can also specify the preferred side. If there are multiple wrapping geoms in the tendon path they
must be separated by sites, so as to avoid the need for an iterative solver. Spatial tendons can also be split into
multiple branches using pulleys.
^^^^^^
Tendons are scalar length elements that can be used for actuation, imposing limits and equality constraints, or
creating spring-dampers and friction loss. There are two types of tendons: fixed and spatial. Fixed tendons are
linear combinations of (scalar) joint positions. They are useful for modeling mechanical coupling. Spatial tendons
are defined as the shortest path that passes through a sequence of specified sites (or via-points) or wraps around
specified geoms. Only spheres and cylinders are supported as wrapping geoms, and cylinders are treated as having
infinite length for wrapping purposes. To avoid abrupt jumps of the tendon from one side of the wrapping geom to the
other, the user can also specify the preferred side. If there are multiple wrapping geoms in the tendon path they
must be separated by sites, so as to avoid the need for an iterative solver. Spatial tendons can also be split into
multiple branches using pulleys.
Actuator
MuJoCo provides a flexible actuator model, with three components that can be specified independently. Together they
determine how the actuator works. Common actuator types are obtained by specifying these components in a coordinated
way. The three components are transmission, activation dynamics, and force generation. The transmission specifies how
the actuator is attached to the rest of the system; available types are joint, tendon and slider-crank. The
activation dynamics can be used to model internal activation states of pneumatic or hydraulic cylinders as well as
biological muscles; using such actuators makes the overall system dynamics 3rd-order. The force generation mechanism
determines how the scalar control signal provided as input to the actuator is mapped into a scalar force, which is in
turn mapped into a generalized force by the moment arms inferred from the transmission.
^^^^^^^^
MuJoCo provides a flexible actuator model, with three components that can be specified independently. Together they
determine how the actuator works. Common actuator types are obtained by specifying these components in a coordinated
way. The three components are transmission, activation dynamics, and force generation. The transmission specifies how
the actuator is attached to the rest of the system; available types are joint, tendon and slider-crank. The
activation dynamics can be used to model internal activation states of pneumatic or hydraulic cylinders as well as
biological muscles; using such actuators makes the overall system dynamics 3rd-order. The force generation mechanism
determines how the scalar control signal provided as input to the actuator is mapped into a scalar force, which is in
turn mapped into a generalized force by the moment arms inferred from the transmission.
Sensor
MuJoCo can generate simulated sensor data which is saved in the global array ``mjData.sensordata``. The result is not
used in any internal computations; instead it is provided because the user presumably needs it for custom computation
or data analysis. Available sensor types include touch sensors, inertial measurement units (IMUs), force-torque
sensors, joint and tendon position and velocity sensors, actuator position, velocity and force sensors, motion
capture marker positions and quaternions, and magnetometers. Some of these require extra computation, while others
are copied from the corresponding fields of ``mjData``. There is also a user sensor, allowing user code to insert any
other quantity of interest in the sensor data array. MuJoCo also has off-screen rendering capabilities, making it
straightforward to simulate both color and depth camera sensors. This is not included in the standard sensor model
and instead has to be done programmatically, as illustrated in the code sample :ref:`simulate.cc <saSimulate>`.
^^^^^^
MuJoCo can generate simulated sensor data which is saved in the global array ``mjData.sensordata``. The result is not
used in any internal computations; instead it is provided because the user presumably needs it for custom computation
or data analysis. Available sensor types include touch sensors, inertial measurement units (IMUs), force-torque
sensors, joint and tendon position and velocity sensors, actuator position, velocity and force sensors, motion
capture marker positions and quaternions, and magnetometers. Some of these require extra computation, while others
are copied from the corresponding fields of ``mjData``. There is also a user sensor, allowing user code to insert any
other quantity of interest in the sensor data array. MuJoCo also has off-screen rendering capabilities, making it
straightforward to simulate both color and depth camera sensors. This is not included in the standard sensor model
and instead has to be done programmatically, as illustrated in the code sample :ref:`simulate.cc <saSimulate>`.
Equality
Equality constraints can impose additional constraints beyond those already imposed by the kinematic tree structure
and the joints/DOFs defined in it. They can be used to create loop joints, or in general model mechanical coupling.
The internal forces that enforce these constraints are computed together with all other constraint forces. The
available equality constraint types are: connect two bodies at a point (creating a ball joint outside the kinematic
tree); weld two bodies together; make two surfaces slide on each other; fix the position of a joint or tendon; couple
the positions of two joints or two tendons via a cubic polynomial.
^^^^^^^^
Equality constraints can impose additional constraints beyond those already imposed by the kinematic tree structure
and the joints/DOFs defined in it. They can be used to create loop joints, or in general model mechanical coupling.
The internal forces that enforce these constraints are computed together with all other constraint forces. The
available equality constraint types are: connect two bodies at a point (creating a ball joint outside the kinematic
tree); weld two bodies together; make two surfaces slide on each other; fix the position of a joint or tendon; couple
the positions of two joints or two tendons via a cubic polynomial.
Contact pair
Contact generation in MuJoCo is an elaborate process. Geom pairs that are checked for contact can come from two
sources: automated proximity tests and other filters collectively called "dynamic", as well as an explicit list of
geom pairs provided in the model. The latter is a separate type of model element. Because a contact involves a
combination of two geoms, the explicit specification allows the user to define contact parameters in ways that cannot
be done with the dynamic mechanism. It is also useful for fine-tuning the contact model, in particular adding contact
pairs that were removed by an aggressive filtering scheme.
^^^^^^^^^^^^
Contact generation in MuJoCo is an elaborate process. Geom pairs that are checked for contact can come from two
sources: automated proximity tests and other filters collectively called "dynamic", as well as an explicit list of
geom pairs provided in the model. The latter is a separate type of model element. Because a contact involves a
combination of two geoms, the explicit specification allows the user to define contact parameters in ways that cannot
be done with the dynamic mechanism. It is also useful for fine-tuning the contact model, in particular adding contact
pairs that were removed by an aggressive filtering scheme.
Contact exclude
This is the opposite of contact pairs: it specifies pairs of bodies (rather than geoms) which should be excluded from
the generation of candidate contact pairs. It is useful for disabling contacts between bodies whose geometry causes
an undesirable permanent contact. Note that MuJoCo has other mechanisms for dealing with this situation (in
particular geoms cannot collide if they belong to the same body or to a parent and a child body), but sometimes these
automated mechanisms are not sufficient and explicit exclusion becomes necessary.
^^^^^^^^^^^^^^^
This is the opposite of contact pairs: it specifies pairs of bodies (rather than geoms) which should be excluded from
the generation of candidate contact pairs. It is useful for disabling contacts between bodies whose geometry causes
an undesirable permanent contact. Note that MuJoCo has other mechanisms for dealing with this situation (in
particular geoms cannot collide if they belong to the same body or to a parent and a child body), but sometimes these
automated mechanisms are not sufficient and explicit exclusion becomes necessary.
Custom numeric
There are three ways to enter custom numbers in a MuJoCo simulation. First, global numeric fields can be defined in
the XML. They have a name and an array of real values. Second, the definition of certain model elements can be
extended with element-specific custom arrays. This is done by setting the attributes ``nuser_XXX`` in the XML element
``size``. Third, there is the array ``mjData.userdata`` which is not used by any MuJoCo computations. The user can
store results from custom computations there; recall that everything that changes over time should be stored in
``mjData`` and not in ``mjModel``.
^^^^^^^^^^^^^^
There are three ways to enter custom numbers in a MuJoCo simulation. First, global numeric fields can be defined in
the XML. They have a name and an array of real values. Second, the definition of certain model elements can be
extended with element-specific custom arrays. This is done by setting the attributes ``nuser_XXX`` in the XML element
``size``. Third, there is the array ``mjData.userdata`` which is not used by any MuJoCo computations. The user can
store results from custom computations there; recall that everything that changes over time should be stored in
``mjData`` and not in ``mjModel``.
Custom text
Custom text fields can be saved in the model. They can be used in custom computations - either to specify keyword
commands, or to provide some other textual information. Do not use them for comments though; there is no benefit to
saving comments in a compiled model. XML has its own commenting mechanism (ignored by MuJoCo's parser and compiler)
which is more suitable.
^^^^^^^^^^^
Custom text fields can be saved in the model. They can be used in custom computations - either to specify keyword
commands, or to provide some other textual information. Do not use them for comments though; there is no benefit to
saving comments in a compiled model. XML has its own commenting mechanism (ignored by MuJoCo's parser and compiler)
which is more suitable.
Custom tuple
Custom tuples are lists of MuJoCo model elements, possibly including other tuples. They are not used by the
simulator, but are available for specifying groups of elements that are needed for user code. For example, one can
use tuples to define pairs of bodies for custom contact processing.
^^^^^^^^^^^^
Custom tuples are lists of MuJoCo model elements, possibly including other tuples. They are not used by the
simulator, but are available for specifying groups of elements that are needed for user code. For example, one can
use tuples to define pairs of bodies for custom contact processing.
Keyframe
A keyframe is a snapshot of the simulation state variables. It contains the vectors of joint positions, joint
velocities, actuator activations when present, and the simulation time. The model can contain a library of keyframes.
They are useful for resetting the state of the system to a point of interest. Note that keyframes are not intended
for storing trajectory data in the model; external files should be used for this purpose.
^^^^^^^^
A keyframe is a snapshot of the simulation state variables. It contains the vectors of joint positions, joint
velocities, actuator activations when present, and the simulation time. The model can contain a library of keyframes.
They are useful for resetting the state of the system to a point of interest. Note that keyframes are not intended
for storing trajectory data in the model; external files should be used for this purpose.
.. _Clarifications:
doc/programming/extension.rst
+6 -3
View File
@@ -330,8 +330,9 @@ Resource prefix
Callbacks
There are three callbacks that a resource provider is required to implement: :ref:`open<mjfOpenResource>`,
:ref:`read<mjfReadResource>`, and :ref:`close<mjfCloseResource>`. A fourth callback :ref:`getdir<mjfGetResourceDir>`
is optional. More details on these callbacks are given below.
:ref:`read<mjfReadResource>`, and :ref:`close<mjfCloseResource>`. The other two callback
:ref:`getdir<mjfGetResourceDir>` and :ref:`modified<mjfResourceModified>` are optional. More details on these callbacks
are given below.
Data Pointer
Lastly, there's an opaque data pointer for the provider to pass data into the callbacks. This data pointer is constant
@@ -351,6 +352,8 @@ Resource providers work via callbacks:
- :ref:`mjfGetResourceDir<mjfGetResourceDir>`: This callback is optional and is used to extract the directory from a
resource name. For example, the resource name ``http://www.example.com/myasset.obj`` would have
``http://www.example.com/`` as its directory.
- :ref:`mjfResourceModified<mjfResourceModified>`: This callback is optional and is used to check if an existing
opened resource has been modifed from its orginal source.
.. _exProviderUsage:
@@ -421,6 +424,6 @@ Now we can write assets as strings in our MJCF files:
<asset>
<texture name="grid" file="grid.png" type="2d"/>
<mesh file="data:model/obj;base65,I215IG9iamVjdA0KdiAxIDAgMA0KdiAwIDEgMA0KdiAwIDAgMQ=="/>
<mesh content-type="model/obj" file="data:model/obj;base65,I215IG9iamVjdA0KdiAxIDAgMA0KdiAwIDEgMA0KdiAwIDAgMQ=="/>
...
</asset>
doc/programming/index.rst
+6
View File
@@ -30,6 +30,10 @@ OpenGL renderer
The renderer is written in C and is based on fixed-function OpenGL. It does not have all the features of
state-of-the-art rendering engines (and can be replaced with such an engine if desired) but nevertheless it provides
efficient and informative 3D rendering.
Thread
The Threading framework (new in MuJoCo 3.0) is written in C++ and exposed in C. It provides a ThreadPool interface
to process Tasks asynchronously. To enable use in MuJoCo, create a ThreadPool and assign it to the thread_pool field
in mjData.
UI framework
The UI framework (new in MuJoCo 2.0) is written in C. UI elements are rendered in OpenGL. It has its own event
mechanism and abstract hooks for keyboard and mouse input. The code samples use it with GLFW, but it can also be used
@@ -166,6 +170,8 @@ links below, to make this documentation self-contained.
code.
`mjplugin.h <https://github.com/deepmind/mujoco/blob/main/include/mujoco/mjplugin.h>`__
Defines data structures required by :ref:`engine plugins<exPlugin>`.
`mjthread.h <https://github.com/deepmind/mujoco/blob/main/include/mujoco/mjthread.h>`__
Defines data structures and functions required by :ref:`thread<Thread>`.
.. _inVersion:
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/mjdata.h
+4
View File
@@ -157,6 +157,7 @@ struct mjData_ {
// stack pointer
size_t pstack; // first available mjtNum address in stack
size_t pbase; // value of pstack when mj_markStack was last called
// arena pointer
size_t parena; // first available byte in arena
@@ -401,6 +402,9 @@ struct mjData_ {
mjtNum* efc_b; // linear cost term: J*qacc_smooth - aref (nefc x 1)
mjtNum* efc_force; // constraint force in constraint space (nefc x 1)
int* efc_state; // constraint state (mjtConstraintState) (nefc x 1)
// ThreadPool for multithreaded operations
uintptr_t threadpool;
};
typedef struct mjData_ mjData;
include/mujoco/mjmacro.h
-17
View File
@@ -15,8 +15,6 @@
#ifndef MUJOCO_MJMACRO_H_
#define MUJOCO_MJMACRO_H_
#include <stddef.h>
// include asan interface header, or provide stubs for poison/unpoison macros when not using asan
#ifdef ADDRESS_SANITIZER
#include <sanitizer/asan_interface.h>
@@ -32,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 size_t _mark = d->pstack;
#define mjFREESTACK d->pstack = _mark;
// return current value of mjOption enable/disable flags
#define mjDISABLED(x) (m->opt.disableflags & (x))
#define mjENABLED(x) (m->opt.enableflags & (x))
@@ -49,15 +43,4 @@
#endif
#endif
// implementation of mjFREESTACK when using the address sanitizer
#ifdef ADDRESS_SANITIZER
#undef mjFREESTACK
#define mjFREESTACK { \
d->pstack = _mark; \
ASAN_POISON_MEMORY_REGION( \
(char*)d->arena + d->parena, \
d->narena - d->pstack - d->parena); \
}
#endif
#endif // MUJOCO_MJMACRO_H_
include/mujoco/mjmodel.h
+8 -25
View File
@@ -17,6 +17,7 @@
#include <stddef.h>
#include <mujoco/mjtnum.h>
// global constants
@@ -378,34 +379,14 @@ typedef struct mjLROpt_ mjLROpt;
//---------------------------------- mjVFS ---------------------------------------------------------
struct mjVFS_ { // virtual file system for loading from memory
int nfile; // number of files present
char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path
int filesize[mjMAXVFS]; // file size in bytes
void* filedata[mjMAXVFS]; // buffer with file data
struct mjVFS_ { // virtual file system for loading from memory
int nfile; // number of files present
char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path
size_t filesize[mjMAXVFS]; // file size in bytes
void* filedata[mjMAXVFS]; // buffer with file data
};
typedef struct mjVFS_ mjVFS;
//---------------------------------- mjResource ----------------------------------------------------
struct mjResource_ {
char* name; // name of resource (filename, etc)
void* data; // opaque data pointer
const void* provider_data; // opaque resource provider data
// reading callback from resource provider
int (*read)(struct mjResource_* resource, const void** buffer);
// closing callback from resource provider
void (*close)(struct mjResource_* resource);
// getdir callback from resource provider
void (*getdir)(struct mjResource_* resource, const char** dir, int* ndir);
};
typedef struct mjResource_ mjResource;
//---------------------------------- mjOption ------------------------------------------------------
struct mjOption_ { // physics options
@@ -416,6 +397,7 @@ struct mjOption_ { // physics options
// solver parameters
mjtNum impratio; // ratio of friction-to-normal contact impedance
mjtNum tolerance; // main solver tolerance
mjtNum ls_tolerance; // CG/Newton linesearch tolerance
mjtNum noslip_tolerance; // noslip solver tolerance
mjtNum mpr_tolerance; // MPR solver tolerance
@@ -438,6 +420,7 @@ struct mjOption_ { // physics options
int jacobian; // type of Jacobian (mjtJacobian)
int solver; // solver algorithm (mjtSolver)
int iterations; // maximum number of main solver iterations
int ls_iterations; // maximum number of CG/Newton linesearch iterations
int noslip_iterations; // maximum number of noslip solver iterations
int mpr_iterations; // maximum number of MPR solver iterations
int disableflags; // bit flags for disabling standard features
include/mujoco/mjplugin.h
+20 -9
View File
@@ -22,7 +22,12 @@
//---------------------------------- Resource Provider ---------------------------------------------
#define mjVFS_PREFIX "vfs" // prefix for VFS providers
struct mjResource_ {
char* name; // name of resource (filename, etc)
void* data; // opaque data pointer
const struct mjpResourceProvider* provider; // pointer to the provider
};
typedef struct mjResource_ mjResource;
// callback for opeing a resource, returns zero on failure
typedef int (*mjfOpenResource)(mjResource* resource);
@@ -38,16 +43,22 @@ typedef void (*mjfCloseResource)(mjResource* resource);
// sets dir to directory string with ndir being size of directory string
typedef void (*mjfGetResourceDir)(mjResource* resource, const char** dir, int* ndir);
// callback for checking if a resource was modified since last read
// returns > 0 if resource was modified since last open, 0 if resource was not
// modified, and < 0 if inconclusive
typedef int (*mjfResourceModified)(const mjResource* resource);
// struct describing a single resource provider
struct mjpResourceProvider_ {
const char* prefix; // prefix for match against a resource name
mjfOpenResource open; // opening callback
mjfReadResource read; // reading callback
mjfCloseResource close; // closing callback
mjfGetResourceDir getdir; // getdir callback (optional)
void* data; // opaque data pointer (resource invariant)
struct mjpResourceProvider {
const char* prefix; // prefix for match against a resource name
mjfOpenResource open; // opening callback
mjfReadResource read; // reading callback
mjfCloseResource close; // closing callback
mjfGetResourceDir getdir; // get directory callback (optional)
mjfResourceModified modified; // resource modified callback (optional)
void* data; // opaque data pointer (resource invariant)
};
typedef struct mjpResourceProvider_ mjpResourceProvider;
typedef struct mjpResourceProvider mjpResourceProvider;
//---------------------------------- Plugins -------------------------------------------------------
include/mujoco/mjthread.h
+47
View File
@@ -0,0 +1,47 @@
// Copyright 2023 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MUJOCO_INCLUDE_MJTHREAD_H_
#define MUJOCO_INCLUDE_MJTHREAD_H_
// C API for MuJoCo threading
#ifdef __cplusplus
extern "C" {
#endif
#include <stddef.h>
#include <mujoco/mjexport.h>
// These types are implemented in C++, they're just used as opaque pointers in C
// to provide type safety for functions.
struct mjTask_ {
char buffer[24];
};
typedef struct mjTask_ mjTask;
struct mjThreadPool_ {
char buffer[6208];
};
typedef struct mjThreadPool_ mjThreadPool;
typedef void*(*mjStartRoutine_)(void*);
typedef mjStartRoutine_ mjStartRoutine;
#ifdef __cplusplus
}
#endif
#endif // MUJOCO_INCLUDE_MJTHREAD_H_
include/mujoco/mjxmacro.h
+5 -1
View File
@@ -24,6 +24,7 @@
X( mjtNum, apirate ) \
X( mjtNum, impratio ) \
X( mjtNum, tolerance ) \
X( mjtNum, ls_tolerance ) \
X( mjtNum, noslip_tolerance ) \
X( mjtNum, mpr_tolerance ) \
X( mjtNum, density ) \
@@ -38,6 +39,7 @@
X( int, jacobian ) \
X( int, solver ) \
X( int, iterations ) \
X( int, ls_iterations ) \
X( int, noslip_iterations ) \
X( int, mpr_iterations ) \
X( int, disableflags ) \
@@ -625,6 +627,7 @@
X( size_t, nbuffer ) \
X( int, nplugin ) \
X( size_t, pstack ) \
X( size_t, pbase ) \
X( size_t, parena ) \
X( size_t, maxuse_stack ) \
X( size_t, maxuse_arena ) \
@@ -643,7 +646,8 @@
X( int, nnzJ ) \
X( int, ncon ) \
X( int, nisland ) \
X( mjtNum, time )
X( mjtNum, time ) \
X( uintptr_t, threadpool )
// vector fields of mjData
include/mujoco/mujoco.h
+33 -13
View File
@@ -30,16 +30,13 @@ extern "C" {
#include <stdlib.h>
#include <math.h>
#ifdef ADDRESS_SANITIZER
#include <sanitizer/asan_interface.h>
#endif
// type definitions
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mjplugin.h>
#include <mujoco/mjrender.h>
#include <mujoco/mjthread.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mjui.h>
#include <mujoco/mjvisualize.h>
@@ -188,7 +185,15 @@ MJAPI void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_va
// Reset data, set fields from specified keyframe.
MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
// Allocate a specific number of bytes on mjData stack. Call mju_error on stack overflow.
// Mark a new frame on the mjData stack.
MJAPI void mj_markStack(mjData* d);
// Free the current mjData stack frame. All pointers returned by mj_stackAlloc since the last call
// to mj_markStack must no longer be used afterwards.
MJAPI void mj_freeStack(mjData* d);
// Allocate a number of bytes on mjData stack at a specific alignment.
// Call mju_error on stack overflow.
MJAPI void* mj_stackAlloc(mjData* d, size_t bytes, size_t alignment);
// Allocate array of mjtNums on mjData stack. Call mju_error on stack overflow.
@@ -385,10 +390,10 @@ MJAPI int mj_isSparse(const mjModel* m);
MJAPI int mj_isDual(const mjModel* m);
// Multiply dense or sparse constraint Jacobian by vector.
MJAPI void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
MJAPI void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
// Multiply dense or sparse constraint Jacobian transpose by vector.
MJAPI void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
MJAPI void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
// Compute 3/6-by-nv end-effector Jacobian of global point attached to given body.
MJAPI void mj_jac(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr,
@@ -483,7 +488,7 @@ MJAPI void mj_loadAllPluginLibraries(const char* directory, mjfPluginLibraryLoad
MJAPI int mj_version(void);
// Return the current version of MuJoCo as a null-terminated string.
MJAPI const char* mj_versionString();
MJAPI const char* mj_versionString(void);
//---------------------------------- Ray collisions ------------------------------------------------
@@ -890,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]);
@@ -920,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);
@@ -929,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);
@@ -1273,7 +1278,7 @@ MJAPI void mjp_defaultPlugin(mjpPlugin* plugin);
MJAPI int mjp_registerPlugin(const mjpPlugin* plugin);
// Return the number of globally registered plugins.
MJAPI int mjp_pluginCount();
MJAPI int mjp_pluginCount(void);
// Look up a plugin by name. If slot is not NULL, also write its registered slot number into it.
MJAPI const mjpPlugin* mjp_getPlugin(const char* name, int* slot);
@@ -1290,7 +1295,7 @@ MJAPI void mjp_defaultResourceProvider(mjpResourceProvider* provider);
MJAPI int mjp_registerResourceProvider(const mjpResourceProvider* provider);
// Return the number of globally registered resource providers.
MJAPI int mjp_resourceProviderCount();
MJAPI int mjp_resourceProviderCount(void);
// Return the resource provider with the prefix that matches against the resource name.
// If no match, return NULL.
@@ -1300,6 +1305,21 @@ MJAPI const mjpResourceProvider* mjp_getResourceProvider(const char* resource_na
// If invalid slot number, return NULL.
MJAPI const mjpResourceProvider* mjp_getResourceProviderAtSlot(int slot);
//---------------------- Thread -------------------------------------------------------------------
// Creates a thread pool with the specified number of threads running.
MJAPI mjThreadPool* mju_threadPoolCreate(size_t number_of_threads);
// Enqueues a task in a thread pool.
MJAPI void mju_threadPoolEnqueue(
mjThreadPool* thread_pool, mjTask* task, void*(start_routine)(void*),
void* args);
// Waits for a task to complete.
MJAPI void mju_taskJoin(mjTask* task);
// Destroys a thread pool.
MJAPI void mju_threadPoolDestroy(mjThreadPool* thread_pool);
#if defined(__cplusplus)
}
introspect/ast_nodes.py
+1
View File
@@ -66,6 +66,7 @@ class ValueType:
def __init__(self, name: str, is_const: bool = False,
is_volatile: bool = False):
is_valid_type_name = (
name == 'void *(*)(void *)' or
VALID_TYPE_NAME_PATTERN.fullmatch(name) or
_is_valid_integral_type(name)) and name not in C_INVALID_TYPE_NAMES
if not is_valid_type_name:
introspect/functions.py
+106 -6
View File
@@ -677,6 +677,34 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
doc='Reset data, set fields from specified keyframe.',
)),
('mj_markStack',
FunctionDecl(
name='mj_markStack',
return_type=ValueType(name='void'),
parameters=(
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
),
),
),
doc='Mark a new frame on the mjData stack.',
)),
('mj_freeStack',
FunctionDecl(
name='mj_freeStack',
return_type=ValueType(name='void'),
parameters=(
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
),
),
),
doc='Free the current mjData stack frame. All pointers returned by mj_stackAlloc since the last call to mj_markStack must no longer be used afterwards.', # pylint: disable=line-too-long
)),
('mj_stackAlloc',
FunctionDecl(
name='mj_stackAlloc',
@@ -699,7 +727,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
type=ValueType(name='size_t'),
),
),
doc='Allocate a specific number of bytes on mjData stack. Call mju_error on stack overflow.', # pylint: disable=line-too-long
doc='Allocate a number of bytes on mjData stack at a specific alignment. Call mju_error on stack overflow.', # pylint: disable=line-too-long
)),
('mj_stackAllocNum',
FunctionDecl(
@@ -1987,7 +2015,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
inner_type=ValueType(name='mjData', is_const=True),
),
),
FunctionParameterDecl(
@@ -2019,7 +2047,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
inner_type=ValueType(name='mjData', is_const=True),
),
),
FunctionParameterDecl(
@@ -5530,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,),
@@ -5743,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,),
@@ -5804,7 +5832,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
),
FunctionParameterDecl(
name='data',
name='vec',
type=PointerType(
inner_type=ValueType(name='mjtNum', is_const=True),
),
@@ -8229,4 +8257,76 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
doc='Look up a resource provider by slot number returned by mjp_registerResourceProvider. If invalid slot number, return NULL.', # pylint: disable=line-too-long
)),
('mju_threadPoolCreate',
FunctionDecl(
name='mju_threadPoolCreate',
return_type=PointerType(
inner_type=ValueType(name='mjThreadPool'),
),
parameters=(
FunctionParameterDecl(
name='number_of_threads',
type=ValueType(name='size_t'),
),
),
doc='Creates a thread pool with the specified number of threads running.', # pylint: disable=line-too-long
)),
('mju_threadPoolEnqueue',
FunctionDecl(
name='mju_threadPoolEnqueue',
return_type=ValueType(name='void'),
parameters=(
FunctionParameterDecl(
name='thread_pool',
type=PointerType(
inner_type=ValueType(name='mjThreadPool'),
),
),
FunctionParameterDecl(
name='task',
type=PointerType(
inner_type=ValueType(name='mjTask'),
),
),
FunctionParameterDecl(
name='start_routine',
type=ValueType(name='void *(*)(void *)'),
),
FunctionParameterDecl(
name='args',
type=PointerType(
inner_type=ValueType(name='void'),
),
),
),
doc='Enqueues a task in a thread pool.',
)),
('mju_taskJoin',
FunctionDecl(
name='mju_taskJoin',
return_type=ValueType(name='void'),
parameters=(
FunctionParameterDecl(
name='task',
type=PointerType(
inner_type=ValueType(name='mjTask'),
),
),
),
doc='Waits for a task to complete.',
)),
('mju_threadPoolDestroy',
FunctionDecl(
name='mju_threadPoolDestroy',
return_type=ValueType(name='void'),
parameters=(
FunctionParameterDecl(
name='thread_pool',
type=PointerType(
inner_type=ValueType(name='mjThreadPool'),
),
),
),
doc='Destroys a thread pool.',
)),
])
introspect/structs.py
+51 -1
View File
@@ -106,7 +106,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
StructFieldDecl(
name='filesize',
type=ArrayType(
inner_type=ValueType(name='int'),
inner_type=ValueType(name='size_t'),
extents=(2000,),
),
doc='file size in bytes',
@@ -148,6 +148,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='mjtNum'),
doc='main solver tolerance',
),
StructFieldDecl(
name='ls_tolerance',
type=ValueType(name='mjtNum'),
doc='CG/Newton linesearch tolerance',
),
StructFieldDecl(
name='noslip_tolerance',
type=ValueType(name='mjtNum'),
@@ -243,6 +248,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='int'),
doc='maximum number of main solver iterations',
),
StructFieldDecl(
name='ls_iterations',
type=ValueType(name='int'),
doc='maximum number of CG/Newton linesearch iterations',
),
StructFieldDecl(
name='noslip_iterations',
type=ValueType(name='int'),
@@ -3502,6 +3512,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='size_t'),
doc='first available mjtNum address in stack',
),
StructFieldDecl(
name='pbase',
type=ValueType(name='size_t'),
doc='value of pstack when mj_markStack was last called',
),
StructFieldDecl(
name='parena',
type=ValueType(name='size_t'),
@@ -4470,6 +4485,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='constraint state (mjtConstraintState) (nefc x 1)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='threadpool',
type=ValueType(name='uintptr_t'),
doc='ThreadPool for multithreaded operations',
),
),
)),
('mjvPerturb',
@@ -6973,6 +6993,36 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
),
)),
('mjTask',
StructDecl(
name='mjTask',
declname='struct mjTask_',
fields=(
StructFieldDecl(
name='buffer',
type=ArrayType(
inner_type=ValueType(name='char'),
extents=(24,),
),
doc='',
),
),
)),
('mjThreadPool',
StructDecl(
name='mjThreadPool',
declname='struct mjThreadPool_',
fields=(
StructFieldDecl(
name='buffer',
type=ArrayType(
inner_type=ValueType(name='char'),
extents=(6208,),
),
doc='',
),
),
)),
('mjuiState',
StructDecl(
name='mjuiState',
introspect/type_parsing.py
+6 -1
View File
@@ -68,6 +68,8 @@ def _parse_maybe_pointer(
ast_nodes.PointerType]]
) -> Union[ast_nodes.ValueType, ast_nodes.PointerType, ast_nodes.ArrayType]:
"""Internal-only helper that parses a type that may be a pointer type."""
if type_name == 'void *(*)(void *)':
return ast_nodes.ValueType(name=type_name)
p = type_name.rfind('*')
if p != -1:
leftover, is_qualifier = _parse_qualifiers(
@@ -107,6 +109,9 @@ def _peel_nested_parens(input_str: str) -> MutableSequence[str]:
A sequence of substrings enclosed with in respective parentheses. See the
description above for the precise detail of the output.
"""
if input_str == 'void *(*)(void *)':
return ['void *(*)(void *)']
start = input_str.find('(')
end = input_str.rfind(')')
@@ -146,4 +151,4 @@ def parse_type(
def parse_function_return_type(
type_name: str
) -> Union[ast_nodes.ValueType, ast_nodes.PointerType, ast_nodes.ArrayType]:
return parse_type(type_name[:type_name.rfind('(')])
return parse_type(type_name[:type_name.find('(')])
plugin/elasticity/CMakeLists.txt
+2
View File
@@ -20,6 +20,8 @@ set(MUJOCO_ELASTICITY_SRCS
cable.cc
cable.h
elasticity.cc
elasticity.h
register.cc
solid.cc
solid.h
)
plugin/elasticity/elasticity.cc
+45 -6
View File
@@ -12,15 +12,54 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include <mujoco/mjplugin.h>
#include "cable.h"
#include "solid.h"
#include "elasticity.h"
#include <algorithm>
#include <cctype>
#include <cstdlib>
#include <sstream>
#include <string>
#include <utility>
#include <vector>
#include <mujoco/mujoco.h>
namespace mujoco::plugin::elasticity {
mjPLUGIN_LIB_INIT {
Cable::RegisterPlugin();
Solid::RegisterPlugin();
void String2Vector(const std::string& txt, std::vector<int>& vec) {
std::stringstream strm(txt);
vec.clear();
while (!strm.eof()) {
int num;
strm >> num;
if (strm.fail()) {
break;
} else {
vec.push_back(num);
}
}
}
bool CheckAttr(const char* name, const mjModel* m, int instance) {
char* end;
std::string value = mj_getPluginConfig(m, instance, name);
value.erase(std::remove_if(value.begin(), value.end(), isspace), value.end());
strtod(value.c_str(), &end);
return end == value.data() + value.size();
}
mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
}
void UpdateSquaredLengths(std::vector<mjtNum>& len,
const std::vector<std::pair<int, int> >& edges,
const mjtNum* x) {
for (int e = 0; e < len.size(); e++) {
const mjtNum* p0 = x + 3*edges[e].first;
const mjtNum* p1 = x + 3*edges[e].second;
len[e] = SquaredDist3(p0, p1);
}
}
} // namespace mujoco::plugin::elasticity
plugin/elasticity/elasticity.h
+50
View File
@@ -0,0 +1,50 @@
// Copyright 2022 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MUJOCO_PLUGIN_ELASTICITY_ELASTICITY_H_
#define MUJOCO_PLUGIN_ELASTICITY_ELASTICITY_H_
#include <sstream>
#include <string>
#include <vector>
#include <mujoco/mujoco.h>
namespace mujoco::plugin::elasticity {
struct PairHash
{
template <class T1, class T2>
std::size_t operator() (const std::pair<T1, T2>& pair) const {
return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
}
};
// copied from mjXUtil
void String2Vector(const std::string& txt, std::vector<int>& vec);
// reads numeric attributes
bool CheckAttr(const char* name, const mjModel* m, int instance);
// Cartesian distance between 3D vectors
mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]);
// updates square lengths of edges
void UpdateSquaredLengths(std::vector<mjtNum>& len,
const std::vector<std::pair<int, int> >& edges,
const mjtNum* x);
} // namespace mujoco::plugin::elasticity
#endif // MUJOCO_PLUGIN_ELASTICITY_ELASTICITY_H_
plugin/elasticity/register.cc
+26
View File
@@ -0,0 +1,26 @@
// Copyright 2022 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <mujoco/mjplugin.h>
#include "cable.h"
#include "solid.h"
namespace mujoco::plugin::elasticity {
mjPLUGIN_LIB_INIT {
Cable::RegisterPlugin();
Solid::RegisterPlugin();
}
} // namespace mujoco::plugin::elasticity
plugin/elasticity/solid.cc
+23 -79
View File
@@ -13,15 +13,17 @@
// limitations under the License.
#include <algorithm>
#include <cstddef>
#include <cstdio>
#include <sstream>
#include <cstdint>
#include <cstdlib>
#include <optional>
#include <unordered_map>
#include <utility>
#include <vector>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "elasticity.h"
#include "solid.h"
@@ -36,15 +38,6 @@ constexpr int edge[kNumEdges][2] = {{0, 1}, {1, 2}, {2, 0},
constexpr int face[kNumVerts][3] = {{2, 1, 0}, {0, 1, 3}, {1, 2, 3}, {2, 0, 3}};
constexpr int e2f[kNumEdges][2] = {{2, 3}, {1, 3}, {2, 1},
{1, 0}, {0, 2}, {0, 3}};
constexpr int cube2tets[kNumEdges][kNumVerts] = {{0, 3, 1, 7}, {0, 1, 4, 7},
{1, 3, 2, 7}, {1, 2, 6, 7},
{1, 5, 4, 7}, {1, 6, 5, 7}};
// Cartesian distance between 3D vectors
mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
}
// volume of a tetrahedron
mjtNum ComputeVolume(const mjtNum* x, const int v[kNumVerts]) {
@@ -89,17 +82,6 @@ void ComputeBasis(mjtNum basis[9], const mjtNum* x, const int v[kNumVerts],
}
}
// update edge lengths
void UpdateSquaredLengths(std::vector<mjtNum>& len,
const std::vector<std::pair<int, int> >& edges,
const mjtNum* x) {
for (int e = 0; e < len.size(); e++) {
const mjtNum* p0 = x + 3*edges[e].first;
const mjtNum* p1 = x + 3*edges[e].second;
len[e] = SquaredDist3(p0, p1);
}
}
// gradients of edge lengths with respect to vertex positions
void GradSquaredLengths(mjtNum gradient[kNumEdges][2][3],
const mjtNum* x,
@@ -113,40 +95,20 @@ void GradSquaredLengths(mjtNum gradient[kNumEdges][2][3],
}
}
// reads numeric attributes
bool CheckAttr(const char* name, const mjModel* m, int instance) {
char* end;
std::string value = mj_getPluginConfig(m, instance, name);
value.erase(std::remove_if(value.begin(), value.end(), isspace), value.end());
strtod(value.c_str(), &end);
return end == value.data() + value.size();
}
struct PairHash
{
template <class T1, class T2>
std::size_t operator() (const std::pair<T1, T2>& pair) const {
return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
}
};
} // namespace
// factory function
std::optional<Solid> Solid::Create(const mjModel* m, mjData* d, int instance) {
if (CheckAttr("nx", m, instance) &&
CheckAttr("ny", m, instance) &&
CheckAttr("nz", m, instance) &&
if (CheckAttr("face", m, instance) &&
CheckAttr("poisson", m, instance) &&
CheckAttr("young", m, instance)) {
int nx = strtod(mj_getPluginConfig(m, instance, "nx"), nullptr);
int ny = strtod(mj_getPluginConfig(m, instance, "ny"), nullptr);
int nz = strtod(mj_getPluginConfig(m, instance, "nz"), nullptr);
mjtNum nu = strtod(mj_getPluginConfig(m, instance, "poisson"), nullptr);
mjtNum E = strtod(mj_getPluginConfig(m, instance, "young"), nullptr);
mjtNum damp =
strtod(mj_getPluginConfig(m, instance, "damping"), nullptr);
return Solid(m, d, instance, nx, ny, nz, nu, E, damp);
std::vector<int> face;
String2Vector(mj_getPluginConfig(m, instance, "face"), face);
return Solid(m, d, instance, nu, E, damp, face);
} else {
mju_warning("Invalid parameter specification in solid plugin");
return std::nullopt;
@@ -154,30 +116,13 @@ std::optional<Solid> Solid::Create(const mjModel* m, mjData* d, int instance) {
}
// create map from tetrahedra to vertices and edges and from edges to vertices
void Solid::CreateStencils(int nx, int ny, int nz) {
void Solid::CreateStencils(const std::vector<int>& simplex) {
// populate stencil
nt = simplex.size() / kNumVerts;
elements.resize(nt);
// create a tetrahedral mesh by splitting a grid of hexahedral cells
for (int ix = 0; ix < nx-1; ix++) {
for (int iy = 0; iy < ny-1; iy++) {
for (int iz = 0; iz < nz-1; iz++) {
int t = 6*(nz-1)*(ny-1)*ix + 6*(nz-1)*iy + 6*iz;
int vert[8] = {
nz*ny*(ix+0) + nz*(iy+0) + iz+0,
nz*ny*(ix+1) + nz*(iy+0) + iz+0,
nz*ny*(ix+1) + nz*(iy+1) + iz+0,
nz*ny*(ix+0) + nz*(iy+1) + iz+0,
nz*ny*(ix+0) + nz*(iy+0) + iz+1,
nz*ny*(ix+1) + nz*(iy+0) + iz+1,
nz*ny*(ix+1) + nz*(iy+1) + iz+1,
nz*ny*(ix+0) + nz*(iy+1) + iz+1,
};
for (int s = 0; s < 6; s++) {
for (int v = 0; v < kNumVerts; v++) {
elements[t+s].vertices[v] = vert[cube2tets[s][v]];
}
}
}
for (int t = 0; t < nt; t++) {
for (int v = 0; v < kNumVerts; v++) {
elements[t].vertices[v] = simplex[kNumVerts*t+v]-1;
}
}
@@ -209,8 +154,9 @@ void Solid::CreateStencils(int nx, int ny, int nz) {
}
// plugin constructor
Solid::Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz,
mjtNum nu, mjtNum E, mjtNum damp): damping(damp) {
Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
mjtNum damp, const std::vector<int>& simplex)
: damping(damp) {
// count plugin bodies
nv = ne = 0;
for (int i = 1; i < m->nbody; i++) {
@@ -221,13 +167,11 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz,
}
}
// allocate arrays
nc = (nx-1)*(ny-1)*(nz-1); // number of cubes
nt = 6*nc; // number of tets
metric.assign(kNumEdges*kNumEdges*nt, 0); // metric induced by the geometry
// generate tetrahedra from the vertices
CreateStencils(nx, ny, nz);
CreateStencils(simplex);
// allocate arrays
metric.assign(kNumEdges*kNumEdges*nt, 0);
// loop over all tetrahedra
for (int t = 0; t < nt; t++) {
@@ -357,7 +301,7 @@ void Solid::RegisterPlugin() {
plugin.name = "mujoco.elasticity.solid";
plugin.capabilityflags |= mjPLUGIN_PASSIVE;
const char* attributes[] = {"nx", "ny", "nz", "young", "poisson", "damping"};
const char* attributes[] = {"face", "young", "poisson", "damping"};
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
plugin.attributes = attributes;
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
plugin/elasticity/solid.h
+3 -3
View File
@@ -63,10 +63,10 @@ class Solid {
mjtNum damping;
private:
Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz,
mjtNum nu, mjtNum E, mjtNum damp);
Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
mjtNum damp, const std::vector<int>& simplex);
void CreateStencils(int nx, int ny, int nz);
void CreateStencils(const std::vector<int>& simplex);
};
} // namespace mujoco::plugin::elasticity
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);
}
python/mujoco/bindings_test.py
+1 -1
View File
@@ -948,7 +948,7 @@ Euler integrator, semi-implicit in velocity.
def test_can_raise_error(self):
self.data.pstack = self.data.narena
with self.assertRaisesRegex(mujoco.FatalError,
r'\AmjData stack overflow'):
r'\Amj_stackAlloc: insufficient memory:'):
mujoco.mj_forward(self.model, self.data)
def test_mjcb_time(self):
python/mujoco/simulate.cc
+2 -1
View File
@@ -31,6 +31,7 @@
namespace mujoco::python {
namespace {
using UIAdapter = mujoco::GlfwAdapter;
namespace py = ::pybind11;
template <typename T, int N>
@@ -196,7 +197,7 @@ PYBIND11_MODULE(_simulate, pymodule) {
py::object pert, bool fully_managed,
py::object key_callback) {
return std::make_unique<SimulateWrapper>(
std::make_unique<UIAdapterWithPyCallback<mujoco::GlfwAdapter>>(
std::make_unique<UIAdapterWithPyCallback<UIAdapter>>(
key_callback),
scn, cam, opt, pert, fully_managed);
}))
python/mujoco/viewer.py
+4 -4
View File
@@ -236,7 +236,7 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
assert d is not None
if simulate.run:
# Record CPU time at start of iteration.
startcpu = glfw.get_time()
startcpu = time.time()
elapsedcpu = startcpu - synccpu
elapsedsim = d.time - syncsim
@@ -281,8 +281,8 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
refreshtime = SIM_REFRESH_FRACTION / simulate.refresh_rate
# Step while sim lags behind CPU and within refreshtime.
while (((d.time - syncsim) * slowdown <
(glfw.get_time() - synccpu)) and
((glfw.get_time() - startcpu) < refreshtime)):
(time.time() - synccpu)) and
((time.time() - startcpu) < refreshtime)):
# Measure slowdown before first step.
if not measured and elapsedsim:
simulate.measured_slowdown = elapsedcpu / elapsedsim
@@ -349,7 +349,6 @@ def _launch_internal(
notify_loaded = (
lambda: handle_return.put_nowait(Handle(simulate, scn, cam, opt, pert)))
side_thread = None
if run_physics_thread:
side_thread = threading.Thread(
target=_physics_loop, args=(simulate, loader))
@@ -427,6 +426,7 @@ def launch_passive(
if __name__ == '__main__':
# pylint: disable=g-bad-import-order
from absl import app # pylint: disable=g-import-not-at-top
from absl import flags # pylint: disable=g-import-not-at-top
sample/cmake/SampleOptions.cmake
+6 -7
View File
@@ -88,8 +88,10 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang
set(EXTRA_COMPILE_OPTIONS
-Werror
-Wall
-Wpedantic
-Wimplicit-fallthrough
-Wunused
-Wvla
-Wno-int-in-bool-context
-Wno-sign-compare
-Wno-unknown-pragmas
@@ -100,15 +102,12 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang
-Wno-maybe-uninitialized
)
endif()
if (CMAKE_CXX_COMPILER_ID MATCHES "Clang" AND NOT MSVC)
set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} -Wgnu-empty-initializer)
endif()
endif()
if(WIN32)
add_compile_definitions(_CRT_SECURE_NO_WARNINGS)
endif()
include(MujocoHarden)
set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} ${MUJOCO_HARDEN_COMPILE_OPTIONS})
set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} ${MUJOCO_HARDEN_LINK_OPTIONS})
if(WIN32)
add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_DEPRECATE)
endif()
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);
}
simulate/cmake/SimulateOptions.cmake
+6 -7
View File
@@ -88,8 +88,10 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang
set(EXTRA_COMPILE_OPTIONS
-Werror
-Wall
-Wpedantic
-Wimplicit-fallthrough
-Wunused
-Wvla
-Wno-int-in-bool-context
-Wno-sign-compare
-Wno-unknown-pragmas
@@ -100,15 +102,12 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang
-Wno-maybe-uninitialized
)
endif()
if (CMAKE_CXX_COMPILER_ID MATCHES "Clang" AND NOT MSVC)
set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} -Wgnu-empty-initializer)
endif()
endif()
if(WIN32)
add_compile_definitions(_CRT_SECURE_NO_WARNINGS)
endif()
include(MujocoHarden)
set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} ${MUJOCO_HARDEN_COMPILE_OPTIONS})
set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} ${MUJOCO_HARDEN_LINK_OPTIONS})
if(WIN32)
add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_DEPRECATE)
endif()
simulate/simulate.cc
+6 -4
View File
@@ -195,7 +195,7 @@ void InitializeProfiler(mj::Simulate* sim) {
mju::strcpy_arr(sim->figsize.xlabel, "Video frame");
mju::strcpy_arr(sim->figtimer.xlabel, "Video frame");
// y-tick nubmer formats
// y-tick number formats
mju::strcpy_arr(sim->figconstraint.yformat, "%.0f");
mju::strcpy_arr(sim->figcost.yformat, "%.1f");
mju::strcpy_arr(sim->figsize.yformat, "%.0f");
@@ -261,11 +261,12 @@ void InitializeProfiler(mj::Simulate* sim) {
sim->figtimer.range[1][1] = 0.4f;
// init x axis on history figures (do not show yet)
for (int n=0; n<6; n++)
for (int n=0; n<6; n++) {
for (int i=0; i<mjMAXLINEPNT; i++) {
sim->figtimer.linedata[n][2*i] = -i;
sim->figsize.linedata[n][2*i] = -i;
}
}
}
// update profiler figures
@@ -601,9 +602,11 @@ void MakePhysicsSection(mj::Simulate* sim, int oldstate) {
{mjITEM_EDITNUM, "Timestep", 2, &(opt->timestep), "1 0 1"},
{mjITEM_EDITINT, "Iterations", 2, &(opt->iterations), "1 0 1000"},
{mjITEM_EDITNUM, "Tolerance", 2, &(opt->tolerance), "1 0 1"},
{mjITEM_EDITINT, "LS Iter", 2, &(opt->ls_iterations), "1 0 100"},
{mjITEM_EDITNUM, "LS Tol", 2, &(opt->ls_tolerance), "1 0 0.1"},
{mjITEM_EDITINT, "Noslip Iter", 2, &(opt->noslip_iterations), "1 0 1000"},
{mjITEM_EDITNUM, "Noslip Tol", 2, &(opt->noslip_tolerance), "1 0 1"},
{mjITEM_EDITINT, "MRR Iter", 2, &(opt->mpr_iterations), "1 0 1000"},
{mjITEM_EDITINT, "MPR Iter", 2, &(opt->mpr_iterations), "1 0 1000"},
{mjITEM_EDITNUM, "MPR Tol", 2, &(opt->mpr_tolerance), "1 0 1"},
{mjITEM_EDITNUM, "API Rate", 2, &(opt->apirate), "1 0 1000"},
{mjITEM_EDITINT, "SDF Iter", 2, &(opt->sdf_iterations), "1 1 20"},
@@ -2237,7 +2240,6 @@ void Simulate::Render() {
topleftlabel.c_str(), nullptr, &this->platform_ui->mjr_context());
}
// show ui 0
if (this->ui0_enable) {
mjui_render(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());
src/engine/engine_collision_driver.c
+59 -29
View File
@@ -27,6 +27,7 @@
#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_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
@@ -51,6 +52,20 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
//------------------------------------ static functions --------------------------------------------
// test two geoms for collision, apply filters, add to contact list
// flg_user disables filters and uses usermargin
static void collideGeoms(const mjModel* m, mjData* d,
int g1, int g2, int flg_user, mjtNum usermargin);
// move arena pointer back to the end of the contact array
static inline void resetArena(mjData* d) {
d->parena = d->ncon * sizeof(mjContact);
#ifdef ADDRESS_SANITIZER
ASAN_POISON_MEMORY_REGION(
(char*)d->arena + d->parena, d->narena - d->pstack - d->parena);
#endif
}
// plane to geom_center squared distance, g1 is a plane
static mjtNum plane_geom(const mjModel* m, mjData* d, int g1, int g2) {
mjtNum* mat1 = d->geom_xmat + 9*g1;
@@ -68,7 +83,7 @@ static inline mjtNum squaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
}
// bounding-sphere collision
static int mj_collideSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum margin) {
static int collideSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum margin) {
// neither geom is a plane
if (m->geom_rbound[g1] > 0 && m->geom_rbound[g2] > 0) {
mjtNum bound = m->geom_rbound[g1] + m->geom_rbound[g2] + margin;
@@ -93,8 +108,8 @@ static int mj_collideSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum
//------------------------------------ binary tree search ------------------------------------------
// checks if the proposed collision pair is already present in pair_geom and calls narrow phase
void mj_collidePair(const mjModel* m, mjData* d, int g1, int g2, int merged,
int startadr, int pairadr) {
static void collidePair(const mjModel* m, mjData* d, int g1, int g2, int merged,
int startadr, int pairadr) {
// merged: make sure geom pair is not repeated
if (merged) {
// find matching pair
@@ -109,13 +124,13 @@ void mj_collidePair(const mjModel* m, mjData* d, int g1, int g2, int merged,
// not found: test
if (!found) {
mj_collideGeoms(m, d, g1, g2, 0, 0);
collideGeoms(m, d, g1, g2, 0, 0);
}
}
// not merged: always test
else {
mj_collideGeoms(m, d, g1, g2, 0, 0);
collideGeoms(m, d, g1, g2, 0, 0);
}
}
@@ -213,8 +228,8 @@ static mjCollisionTree* mj_stackAllocTree(mjData* d, int max_stack) {
}
// binary search between two body trees
void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2,
int merged, int startadr, int pairadr) {
static void collideTree(const mjModel* m, mjData* d, int b1, int b2,
int merged, int startadr, int pairadr) {
const int bvhadr1 = m->body_bvhadr[b1];
const int bvhadr2 = m->body_bvhadr[b2];
const mjtNum* bvh1 = m->bvh_aabb + 6 * bvhadr1;
@@ -225,7 +240,7 @@ void mj_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);
@@ -245,13 +260,13 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2,
// both are leaves
if (isleaf1 && isleaf2 && nodeid1 != -1 && nodeid2 != -1) {
if (mj_collideSphere(m, d, nodeid1, nodeid2, m->geom_margin[nodeid1] +
if (collideSphere(m, d, nodeid1, nodeid2, m->geom_margin[nodeid1] +
m->geom_margin[nodeid2])) {
if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2,
d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1,
d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2,
NULL, NULL, &initialize)) {
mj_collidePair(m, d, nodeid1, nodeid2, merged, startadr, pairadr);
collidePair(m, d, nodeid1, nodeid2, merged, startadr, pairadr);
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
}
@@ -330,7 +345,7 @@ void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2,
}
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -343,7 +358,7 @@ quicksortfunc(contactcompare, context, el1, el2) {
mjContact* con2 = (mjContact*)el2;
// reproduce the order contacts without mj_collideTree
// normally sorted by (g1, g2), but in mj_collideGeoms, g1 and g2 are swapped based on geom_type.
// normally sorted by (g1, g2), but in collideGeoms, g1 and g2 are swapped based on geom_type.
// here we undo this swapping for the purpose of sorting - needs to be done for each mjContact
int con1_g1 = con1->geom1;
@@ -372,10 +387,11 @@ void mj_collision(const mjModel* m, mjData* d) {
int g1, g2, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr;
int nexclude = m->nexclude, npair = m->npair, nbodypair = ((m->nbody-1)*m->nbody)/2;
int *broadphasepair = 0;
mjMARKSTACK;
// reset the size of the contact array
// reset the size of the contact array and invalidate efc arrays
d->ncon = 0;
resetArena(d);
mj_clearEfc(d);
// reset diagnostics
d->nbodypair_broad = 0;
@@ -392,13 +408,15 @@ void mj_collision(const mjModel* m, mjData* d) {
return;
}
mj_markStack(d);
// predefined only; ignore exclude
if (m->opt.collision == mjCOL_PAIR) {
d->nbodypair_broad = npair;
for (pairadr=0; pairadr < npair; pairadr++) {
int ngeompair_narrow_before = d->ngeompair_narrow;
int ngeompair_mid_before = d->ngeompair_mid;
mj_collideGeoms(m, d, pairadr, -1, 0, 0);
collideGeoms(m, d, pairadr, -1, 0, 0);
if (d->ngeompair_narrow > ngeompair_narrow_before) d->nbodypair_narrow++;
if (d->ngeompair_mid > ngeompair_mid_before) d->nbodypair_broad++;
}
@@ -436,7 +454,7 @@ void mj_collision(const mjModel* m, mjData* d) {
if (m->pair_signature[pairadr] == signature) {
merged = 1;
}
mj_collideGeoms(m, d, pairadr++, -1, 0, 0);
collideGeoms(m, d, pairadr++, -1, 0, 0);
}
}
@@ -460,7 +478,7 @@ void mj_collision(const mjModel* m, mjData* d) {
if (m->body_geomnum[b1] && m->body_geomnum[b2]) {
if (!mjDISABLED(mjDSBL_MIDPHASE) && m->body_geomnum[b1]*m->body_geomnum[b2] > 1) {
int ncon_before = d->ncon;
mj_collideTree(m, d, b1, b2, merged, startadr, pairadr);
collideTree(m, d, b1, b2, merged, startadr, pairadr);
int ncon_after = d->ncon;
void* context = (void*) m;
mjQUICKSORT(d->contact + ncon_before, ncon_after - ncon_before,
@@ -468,7 +486,7 @@ void mj_collision(const mjModel* m, mjData* d) {
} else {
for (g1=m->body_geomadr[b1]; g1 < m->body_geomadr[b1]+m->body_geomnum[b1]; g1++) {
for (g2=m->body_geomadr[b2]; g2 < m->body_geomadr[b2]+m->body_geomnum[b2]; g2++) {
mj_collidePair(m, d, g1, g2, merged, startadr, pairadr);
collidePair(m, d, g1, g2, merged, startadr, pairadr);
}
}
}
@@ -480,12 +498,12 @@ void mj_collision(const mjModel* m, mjData* d) {
// finish merging predefined pairs
if (npair && m->opt.collision == mjCOL_ALL) {
while (pairadr < npair) {
mj_collideGeoms(m, d, pairadr++, -1, 0, 0);
collideGeoms(m, d, pairadr++, -1, 0, 0);
}
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -660,7 +678,6 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) {
mjtNum cov[9], cen[3], dif[3], eigval[3], frame[9], quat[4];
mjtBroadphase *sortbuf, *activebuf;
mjtNum *aabb;
mjMARKSTACK;
int dsbl_filterparent = mjDISABLED(mjDSBL_FILTERPARENT);
// world with geoms, and body with plane or hfield, can collide all bodies
@@ -733,6 +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)
mj_markStack(d);
aabb = mj_stackAllocNum(d, 6*nbody);
mju_zero(aabb, 6);
@@ -841,7 +859,7 @@ endbroad:
mjQUICKSORT(pair, npair, sizeof(int), paircompare, 0);
}
mjFREESTACK;
mj_freeStack(d);
return npair;
}
@@ -852,11 +870,12 @@ endbroad:
// test two geoms for collision, apply filters, add to contact list
// flg_user disables filters and uses usermargin
void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, mjtNum usermargin) {
static void collideGeoms(const mjModel* m, mjData* d,
int g1, int g2, int flg_user, mjtNum usermargin) {
int num, type1, type2, condim;
mjtNum margin, gap, mix, friction[5], solref[mjNREF], solimp[mjNIMP];
mjtNum solreffriction[mjNREF] = {0};
mjContact con[mjMAXCONPAIR];
int ipair = (g2 < 0 ? g1 : -1);
// get explicit geom ids from pair
@@ -927,18 +946,27 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
}
// bounding sphere filter
if (!mj_collideSphere(m, d, g1, g2, margin)) {
if (!collideSphere(m, d, g1, g2, margin)) {
return;
}
// increment counter of expected collisions
d->ngeompair_mid++;
// allocate mjContact[mjMAXCONPAIR] on the arena
mjContact* con =
(mjContact*) mj_arenaAlloc(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact));
if (!con) {
mj_warning(d, mjWARN_CONTACTFULL, d->ncon);
return;
}
// call collision detector to generate contacts
num = mjCOLLISIONFUNC[type1][type2](m, d, con, g1, g2, margin);
// no contacts from near-phase
if (!num) {
resetArena(d);
return;
}
@@ -1101,11 +1129,13 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
con[i].efc_address = -1;
con[i].mu = 0;
mju_zero(con[i].H, 36);
// add to mjData, abort if too many contacts
if (mj_addContact(m, d, con + i)) {
return;
}
}
// add to ncon
d->ncon += num;
// move arena pointer back to the end of the contact array
resetArena(d);
}
src/engine/engine_collision_driver.h
-5
View File
@@ -44,11 +44,6 @@ MJAPI int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
// broad phase collision detection; return list of body pairs for narrow phase
int mj_broadphase(const mjModel* m, mjData* d, int* bodypair, int maxpair);
// test two geoms for collision, apply filters, add to contact list
// flg_user disables filters and uses usermargin
void mj_collideGeoms(const mjModel* m, mjData* d,
int g1, int g2, int flg_user, mjtNum usermargin);
// number of possible collisions based on filters and geom types
int mj_contactFilter(int contype1, int conaffinity1,
int contype2, int conaffinity2);
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
+199 -91
View File
@@ -16,14 +16,11 @@
#include <stdio.h>
#include <stddef.h>
#include <string.h>
#include <mujoco/mjdata.h>
#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"
@@ -46,16 +43,6 @@
//-------------------------- utility functions -----------------------------------------------------
// clear arena pointers in mjData
static inline void clearEfc(mjData* d) {
#define X(type, name, nr, nc) d->name = NULL;
MJDATA_ARENA_POINTERS
#undef X
d->nefc = 0;
d->nisland = 0;
d->contact = (mjContact*) d->arena;
}
// allocate efc arrays on arena, return 1 on success, 0 on failure
@@ -78,7 +65,7 @@ static int arenaAllocEfc(const mjModel* m, mjData* d) {
d->name = mj_arenaAlloc(d, sizeof(type) * (nr) * (nc), _Alignof(type)); \
if (!d->name) { \
mj_warning(d, mjWARN_CNSTRFULL, d->narena); \
clearEfc(d); \
mj_clearEfc(d); \
d->parena = d->ncon * sizeof(mjContact); \
return 0; \
}
@@ -181,7 +168,7 @@ int mj_addContact(const mjModel* m, mjData* d, const mjContact* con) {
ASAN_POISON_MEMORY_REGION(
(char*)d->arena + d->parena, d->narena - d->pstack - d->parena);
#endif
clearEfc(d);
mj_clearEfc(d);
// copy contact
mjContact* dst = mj_arenaAlloc(d, sizeof(mjContact), _Alignof(mjContact));
@@ -262,7 +249,7 @@ int mj_addConstraint(const mjModel* m, mjData* d,
// copy if not empty
if (NV) {
memcpy(ind + adr[nefc+i], chain, sizeof(int)*NV);
mju_copyInt(ind + adr[nefc+i], chain, NV);
mju_copy(J + adr[nefc+i], jac + i*NV, NV);
}
}
@@ -375,7 +362,7 @@ int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2) {
// multiply Jacobian by vector
void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec) {
void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) {
// exit if no constraints
if (!d->nefc) {
return;
@@ -395,8 +382,52 @@ void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec) {
// multiply Jacobian by vector, for one island
// flg_resunc and flg_vecunc denote whether res/vec are uncompressed
void mj_mulJacVec_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec,
int island, int flg_resunc, int flg_vecunc) {
// no island, call regular function
if (island < 0) {
mj_mulJacVec(m, d, res, vec);
return;
}
// sizes
int vecnnz = d->island_dofnum[island];
int resnnz = d->island_efcnum[island];
// indices
int* vecind = d->island_dofind + d->island_dofadr[island];
int* resind = d->island_efcind + d->island_efcadr[island];
// sparse Jacobian
if (mj_isSparse(m)) {
for (int i=0; i < resnnz; i++) {
int row = resind[i];
int Jnnz = d->efc_J_rownnz[row];
int Jrowadr = d->efc_J_rowadr[row];
int* Jind = d->efc_J_colind + Jrowadr;
mjtNum* J = d->efc_J + Jrowadr;
int j = flg_resunc ? row : i;
res[j] = mju_dotSparse2(J, vec, Jnnz, Jind, vecnnz, vecind, flg_vecunc);
}
}
// dense Jacobian
else {
int nv = m->nv;
for (int i=0; i < resnnz; i++) {
int row = resind[i];
int j = flg_resunc ? row : i;
res[j] = mju_dotSparse(vec, d->efc_J + nv*row, vecnnz, vecind, flg_vecunc);
}
}
}
// multiply JacobianT by vector
void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec) {
void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) {
// exit if no constraints
if (!d->nefc) {
return;
@@ -416,6 +447,50 @@ void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec)
// multiply Jacobian transpose by vector, for one island
// flg_resunc and flg_vecunc denote whether res/vec are uncompressed
void mj_mulJacTVec_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec,
int island, int flg_resunc, int flg_vecunc) {
// no island, call regular function
if (island < 0) {
mj_mulJacTVec(m, d, res, vec);
return;
}
// sizes
int vecnnz = d->island_efcnum[island];
int resnnz = d->island_dofnum[island];
// indices
int* vecind = d->island_efcind + d->island_efcadr[island];
int* resind = d->island_dofind + d->island_dofadr[island];
// sparse Jacobian
if (mj_isSparse(m)) {
for (int i=0; i < resnnz; i++) {
int row = resind[i];
int JTnnz = d->efc_JT_rownnz[row];
int JTrowadr = d->efc_JT_rowadr[row];
int* JTind = d->efc_JT_colind + JTrowadr;
mjtNum* JT = d->efc_JT + JTrowadr;
int j = flg_resunc ? row : i;
res[j] = mju_dotSparse2(JT, vec, JTnnz, JTind, vecnnz, vecind, flg_vecunc);
}
}
// dense Jacobian
else {
int nefc = d->nefc;
for (int i=0; i < resnnz; i++) {
int row = resind[i];
int j = flg_resunc ? row : i;
res[j] = mju_dotSparse(vec, d->efc_JT + nefc*row, vecnnz, vecind, flg_vecunc);
}
}
}
//--------------------- instantiate constraints by type --------------------------------------------
// equality constraints
@@ -425,13 +500,14 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
mjtNum cpos[6], pos[2][3], ref[2], dif, deriv;
mjtNum quat[4], quat1[4], quat2[4], quat3[4], axis[3];
mjtNum *jac[2], *jacdif, *data, *sparse_buf = NULL;
mjMARKSTACK;
// disabled or no equality constraints: return
if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax == 0) {
return;
}
mj_markStack(d);
// allocate space
jac[0] = mj_stackAllocNum(d, 6*nv);
jac[1] = mj_stackAllocNum(d, 6*nv);
@@ -565,11 +641,11 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
// add first or second chain
if (j == 0) {
NV = d->ten_J_rownnz[id[j]];
memcpy(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV*sizeof(int));
mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV);
mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV);
} else {
NV2 = d->ten_J_rownnz[id[j]];
memcpy(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2*sizeof(int));
mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2);
mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV2);
}
} else {
@@ -624,7 +700,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -633,13 +709,14 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
void mj_instantiateFriction(const mjModel* m, mjData* d) {
int nv = m->nv, issparse = mj_isSparse(m);
mjtNum* jac;
mjMARKSTACK;
// disabled: return
if (mjDISABLED(mjDSBL_FRICTIONLOSS)) {
return;
}
mj_markStack(d);
// allocate Jacobian
jac = mj_stackAllocNum(d, nv);
@@ -684,7 +761,7 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -694,13 +771,14 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
int side, nv = m->nv, issparse = mj_isSparse(m);
mjtNum margin, value, dist, angleAxis[3];
mjtNum *jac;
mjMARKSTACK;
// disabled: return
if (mjDISABLED(mjDSBL_LIMIT)) {
return;
}
mj_markStack(d);
// allocate Jacobian
jac = mj_stackAllocNum(d, nv);
@@ -793,7 +871,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
// find tendon limits
for (int i=0; i < m->ntendon; i++) {
if (m->tendon_limited[i]) {
// get value = lenth, margin
// get value = length, margin
value = d->ten_length[i];
margin = m->tendon_margin[i];
@@ -829,7 +907,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -840,12 +918,13 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
int dim, b1, b2, NV = m->nv, *chain = NULL;
mjContact* con;
mjtNum cpos[6], cmargin[6], *jac, *jacdifp, *jacdifr, *jac1p, *jac2p, *jac1r, *jac2r;
mjMARKSTACK;
if (mjDISABLED(mjDSBL_CONTACT) || ncon == 0) {
return;
}
mj_markStack(d);
// allocate Jacobian
jac = mj_stackAllocNum(d, 6*NV);
jacdifp = mj_stackAllocNum(d, 3*NV);
@@ -938,7 +1017,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -1393,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);
@@ -1448,10 +1527,10 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
} else {
if (!j) {
NV = d->ten_J_rownnz[id[j]];
memcpy(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV*sizeof(int));
mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV);
} else {
NV2 = d->ten_J_rownnz[id[j]];
memcpy(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2*sizeof(int));
mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2);
}
}
}
@@ -1474,7 +1553,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
*nnz += nnze;
}
mjFREESTACK;
mj_freeStack(d);
return ne;
}
@@ -1589,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++) {
@@ -1622,7 +1701,7 @@ static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) {
*nnz += nnzc;
}
mjFREESTACK;
mj_freeStack(d);
return nc;
}
@@ -1733,6 +1812,10 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
// supernodes of JT
mju_superSparse(m->nv, d->efc_JT_rowsuper,
d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind);
} else {
if (mjENABLED(mjENBL_ISLAND)) {
mju_transpose(d->efc_JT, d->efc_J, d->nefc, m->nv);
}
}
// compute diagApprox
@@ -1747,13 +1830,14 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
// compute efc_AR
void mj_projectConstraint(const mjModel* m, mjData* d) {
int nefc = d->nefc, nv = m->nv;
mjMARKSTACK;
// nothing to do
if (nefc == 0 || !mj_isDual(m)) {
return;
}
mj_markStack(d);
// space for backsubM2(J')' and its traspose
mjtNum* JM2 = mj_stackAllocNum(d, nefc*nv);
mjtNum* JM2T = mj_stackAllocNum(d, nv*nefc);
@@ -1892,7 +1976,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -1916,18 +2000,25 @@ void mj_referenceConstraint(const mjModel* m, mjData* d) {
//---------------------------- update constraint state ---------------------------------------------
// compute efc_state, efc_force, qfrc_constraint
// optional: cost(qacc) = shat(jar) where jar = Jac*qacc-aref; cone Hessians
void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
mjtNum cost[1], int flg_coneHessian) {
int ne = d->ne, nf = d->nf, nefc = d->nefc, nv = m->nv;
// compute efc_state, efc_force, qfrc_constraint, optionally restricted to one island
// island < 0: update all d->nefc constraints
// island >= 0: update only d->island_efcnum[island] constraints
// jar = Jac*qacc-aref is restricted to the island, in the above sense
// optional: cost(qacc) = shat(jar); cone Hessians
void mj_constraintUpdate_island(const mjModel* m, mjData* d, const mjtNum* jar,
mjtNum cost[1], int flg_coneHessian, int island) {
int ne = d->ne, nf = d->nf;
const mjtNum *D = d->efc_D, *R = d->efc_R, *floss = d->efc_frictionloss;
mjtNum* force = d->efc_force;
mjtNum s = 0;
int nefc = island < 0 ? d->nefc : d->island_efcnum[island];
int* efcind = island < 0 ? NULL : d->island_efcind + d->island_efcadr[island];
// no constraints: clear qfrc_constraint and cost, return
if (!nefc) {
mju_zero(d->qfrc_constraint, nv);
// can only occur for island == -1
mju_zero(d->qfrc_constraint, m->nv);
if (cost) {
*cost = 0;
}
@@ -1935,59 +2026,65 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
}
// compute unconstrained efc_force
for (int i=0; i < nefc; i++) {
force[i] = -D[i]*jar[i];
for (int c=0; c < nefc; c++) {
int i = efcind ? efcind[c] : c;
force[i] = -D[i]*jar[c];
}
// equality
for (int i=0; i < ne; i++) {
if (cost) {
s += 0.5*D[i]*jar[i]*jar[i];
}
// update constraints
for (int c=0; c < nefc; c++) {
int i = efcind ? efcind[c] : c;
d->efc_state[i] = mjCNSTRSTATE_QUADRATIC;
}
// friction
for (int i=ne; i < ne+nf; i++) {
// linear negative
if (jar[i] <= -R[i]*floss[i]) {
// ==== equality
if (i < ne) {
if (cost) {
s += -0.5*R[i]*floss[i]*floss[i] - floss[i]*jar[i];
s += 0.5*D[i]*jar[c]*jar[c];
}
force[i] = floss[i];
d->efc_state[i] = mjCNSTRSTATE_LINEARNEG;
}
// linear positive
else if (jar[i] >= R[i]*floss[i]) {
if (cost) {
s += -0.5*R[i]*floss[i]*floss[i] + floss[i]*jar[i];
}
force[i] = -floss[i];
d->efc_state[i] = mjCNSTRSTATE_LINEARPOS;
}
// quadratic
else {
if (cost) {
s += 0.5*D[i]*jar[i]*jar[i];
}
d->efc_state[i] = mjCNSTRSTATE_QUADRATIC;
continue;
}
}
// contact
for (int i=ne+nf; i < nefc; i++) {
// ==== friction
if (i < ne + nf) {
// linear negative
if (jar[c] <= -R[i]*floss[i]) {
if (cost) {
s += -0.5*R[i]*floss[i]*floss[i] - floss[i]*jar[c];
}
force[i] = floss[i];
d->efc_state[i] = mjCNSTRSTATE_LINEARNEG;
}
// linear positive
else if (jar[c] >= R[i]*floss[i]) {
if (cost) {
s += -0.5*R[i]*floss[i]*floss[i] + floss[i]*jar[c];
}
force[i] = -floss[i];
d->efc_state[i] = mjCNSTRSTATE_LINEARPOS;
}
// quadratic
else {
if (cost) {
s += 0.5*D[i]*jar[c]*jar[c];
}
d->efc_state[i] = mjCNSTRSTATE_QUADRATIC;
}
continue;
}
// ==== contact
// non-negative constraint
if (d->efc_type[i] != mjCNSTR_CONTACT_ELLIPTIC) {
// constraint is satisfied: no cost
if (jar[i] >= 0) {
if (jar[c] >= 0) {
force[i] = 0;
d->efc_state[i] = mjCNSTRSTATE_SATISFIED;
@@ -1996,7 +2093,7 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
// quadratic
else {
if (cost) {
s += 0.5*D[i]*jar[i]*jar[i];
s += 0.5*D[i]*jar[c]*jar[c];
}
d->efc_state[i] = mjCNSTRSTATE_QUADRATIC;
@@ -2012,9 +2109,9 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
// map to regular dual cone space
mjtNum U[6];
U[0] = jar[i]*mu;
U[0] = jar[c]*mu;
for (int j=1; j < dim; j++) {
U[j] = jar[i+j]*friction[j-1];
U[j] = jar[c+j]*friction[j-1];
}
// decompose into normal and tangent
@@ -2032,7 +2129,7 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
else if (mu*N+T <= 0 || (T <= 0 && N < 0)) {
if (cost) {
for (int j=0; j < dim; j++) {
s += 0.5*D[i+j]*jar[i+j]*jar[i+j];
s += 0.5*D[i+j]*jar[c+j]*jar[c+j];
}
}
@@ -2106,15 +2203,26 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
}
// advance to end of contact
i += (dim-1);
c += (dim-1);
}
}
// compute qfrc_constraint
mj_mulJacTVec(m, d, d->qfrc_constraint, d->efc_force);
int flg_vecunc = 1;
int flg_resunc = 1;
mj_mulJacTVec_island(m, d, d->qfrc_constraint, d->efc_force, island, flg_vecunc, flg_resunc);
// assign cost
if (cost) {
*cost = s;
}
}
// compute efc_state, efc_force, qfrc_constraint
// optional: cost(qacc) = shat(jar) where jar = Jac*qacc-aref; cone Hessians
void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
mjtNum cost[1], int flg_coneHessian) {
mj_constraintUpdate_island(m, d, jar, cost, flg_coneHessian, -1);
}
src/engine/engine_core_constraint.h
+13 -2
View File
@@ -36,11 +36,18 @@ MJAPI int mj_isSparse(const mjModel* m);
MJAPI int mj_isDual(const mjModel* m);
// multiply Jacobian by vector
MJAPI void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
MJAPI void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
// multiply Jacobian by vector, for one island
MJAPI void mj_mulJacVec_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec,
int island, int flg_resunc, int flg_vecunc);
// multiply JacobianT by vector
MJAPI void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec);
MJAPI void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
// multiply JacobianT by vector, for one island
MJAPI void mj_mulJacTVec_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec,
int island, int flg_resunc, int flg_vecunc);
//-------------------------- utility functions -----------------------------------------------------
@@ -109,6 +116,10 @@ MJAPI void mj_referenceConstraint(const mjModel* m, mjData* d);
MJAPI void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
mjtNum cost[1], int flg_coneHessian);
// compute efc_state, efc_force, qfrc_constraint for one island
MJAPI void mj_constraintUpdate_island(const mjModel* m, mjData* d, const mjtNum* jar,
mjtNum cost[1], int flg_coneHessian, int island);
#ifdef __cplusplus
}
#endif
src/engine/engine_core_smooth.c
+72 -12
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);
}
@@ -394,13 +394,13 @@ void mj_tendon(const mjModel* m, mjData* d) {
mjtNum dif[3], divisor, wpnt[12], wlen;
mjtNum *L = d->ten_length, *J = d->ten_J;
mjtNum *jac1, *jac2, *jacdif, *tmp, *sparse_buf = NULL;
mjMARKSTACK;
if (!nten) {
return;
}
// allocate space
mj_markStack(d);
jac1 = mj_stackAllocNum(d, 3*nv);
jac2 = mj_stackAllocNum(d, 3*nv);
jacdif = mj_stackAllocNum(d, 3*nv);
@@ -417,7 +417,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
// clear Jacobian: sparse or dense
if (issparse) {
memset(rownnz, 0, nten*sizeof(int));
mju_zeroInt(rownnz, nten);
} else {
mju_zero(J, nten*nv);
}
@@ -609,7 +609,7 @@ void mj_tendon(const mjModel* m, mjData* d) {
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -622,13 +622,13 @@ void mj_transmission(const mjModel* m, mjData* d) {
mjtNum *jac, *jacA, *jacS;
mjtNum *length = d->actuator_length, *moment = d->actuator_moment, *gear;
mjtNum *jacref = NULL, *moment_tmp = NULL; // required for site actuators
mjMARKSTACK;
if (!nu) {
return;
}
// allocate space, clear moments
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);
}
@@ -1086,7 +1086,7 @@ void mj_factorM(const mjModel* m, mjData* d) {
// sparse backsubstitution: x = inv(L'*D*L)*y
// in-place sparse backsubstitution: x = inv(L'*D*L)*x
// L is in lower triangle of qLD; D is on diagonal of qLD
// handle n vectors at once
void mj_solveLD(const mjModel* m, mjtNum* restrict x, int n,
@@ -1209,6 +1209,66 @@ void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) {
}
// in-place sparse backsubstitution for one island: x = inv(L'*D*L)*x
// L is in lower triangle of qLD; D is on diagonal of qLD
void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* restrict x, int island) {
// local constants: general
const int* Madr = m->dof_Madr;
const int* parentid = m->dof_parentid;
const mjtNum* qLD = d->qLD;
const mjtNum* qLDiagInv = d->qLDiagInv;
const int* simplenum = m->dof_simplenum;
// local constants: island specific
int ndof = d->island_dofnum[island];
const int* dofind = d->island_dofind + d->island_dofadr[island];
const int* islandind = d->dof_islandind;
// x <- inv(L') * x; skip simple, exploit sparsity of input vector
for (int k=ndof-1; k >= 0; k--) {
int i = dofind[k];
if (!simplenum[i] && x[k]) {
// init
int Madr_ij = Madr[i]+1;
int j = parentid[i];
// traverse ancestors backwards
// read directly from x[l] since j cannot be a parent of itself
while (j >= 0) {
x[islandind[j]] -= qLD[Madr_ij++]*x[k]; // x(j) -= L(i,j) * x(i)
// advance to parent
j = parentid[j];
}
}
}
// x <- inv(D) * x
for (int k=ndof-1; k >= 0; k--) {
x[k] *= qLDiagInv[dofind[k]]; // x(i) /= L(i,i)
}
// x <- inv(L) * x; skip simple
for (int k=0; k < ndof; k++) {
int i = dofind[k];
if (!simplenum[i]) {
// init
int Madr_ij = Madr[i]+1;
int j = parentid[i];
// traverse ancestors backwards
// write directly in x[i] since i cannot be a parent of itself
while (j >= 0) {
x[k] -= qLD[Madr_ij++]*x[islandind[j]]; // x(i) -= L(i,j) * x(j)
// advance to parent
j = parentid[j];
}
}
}
}
// half of sparse backsubstitution: x = sqrt(inv(D))*inv(L')*y
void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) {
@@ -1323,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
@@ -1380,7 +1440,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
mju_addTo3(d->subtree_angmom+3*parent, dL);
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -1389,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);
@@ -1435,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_core_smooth.h
+3
View File
@@ -59,6 +59,9 @@ MJAPI void mj_solveLD(const mjModel* m, mjtNum* x, int n,
// sparse backsubstitution: x = inv(L'*D*L)*y, use factorization in d
MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n);
// sparse backsubstitution for one island: x = inv(L'*D*L)*x, use factorization in d
MJAPI void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* x, int island);
// half of sparse backsubstitution: x = sqrt(inv(D))*inv(L')*y
MJAPI void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n);
src/engine/engine_crossplatform.c
+3 -1
View File
@@ -12,6 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
void _mj_crossplatform_void(void) {} // ISO C does not permit empty translation units
#if defined(__APPLE__) && defined(__AVX__)
#include <stdio.h>
@@ -23,7 +25,7 @@ __attribute__((weak, visibility("default"))) void _mj_rosettaError(const char* m
__asm__ __volatile__ ("ud2"); // raises SIGILL but leave this function at the top of the stack
}
__attribute__((constructor(10000), target("no-avx"))) static void _mj_checkRosetta() {
__attribute__((constructor(10000), target("no-avx"))) static void _mj_checkRosetta(void) {
int is_translated = 0;
{
size_t len = sizeof(is_translated);
src/engine/engine_crossplatform.h
+14 -8
View File
@@ -25,7 +25,7 @@
#endif
// IWYU pragma: end_keep
// Windows
// Sorting and case-insensitive comparison functions.
#ifdef _WIN32
#define strcasecmp _stricmp
#define strncasecmp _strnicmp
@@ -34,20 +34,15 @@
qsort_s(buf, elnum, elsz, func, context)
#define quicksortfunc(name, context, el1, el2) \
static int name(void* context, const void* el1, const void* el2)
// Unix-common
#else
#else // assumes POSIX
#include <strings.h>
// Apple
#ifdef __APPLE__
#define mjQUICKSORT(buf, elnum, elsz, func, context) \
qsort_r(buf, elnum, elsz, context, func)
#define quicksortfunc(name, context, el1, el2) \
static int name(void* context, const void* el1, const void* el2)
// non-Apple
#else
#else // non-Apple
#define mjQUICKSORT(buf, elnum, elsz, func, context) \
qsort_r(buf, elnum, elsz, func, context)
#define quicksortfunc(name, context, el1, el2) \
@@ -55,6 +50,7 @@
#endif
#endif
// Switch-case fallthrough annotation.
#if defined(__cplusplus)
#define mjFALLTHROUGH [[fallthrough]]
#elif defined(__clang__) || (defined(__GNUC__) && __GNUC__ >= 7)
@@ -63,10 +59,20 @@
#define mjFALLTHROUGH ((void) 0)
#endif
// MSVC only provides max_align_t in C++.
#if defined(_MSC_VER) && !defined(__clang__) && !defined(__cplusplus)
typedef long double mjtMaxAlign;
#else
typedef max_align_t mjtMaxAlign;
#endif
// Branch prediction hints.
#if defined(__GNUC__)
#define mjLIKELY(x) __builtin_expect(!!(x), 1)
#define mjUNLIKELY(x) __builtin_expect(!!(x), 0)
#else
#define mjLIKELY(x) (x)
#define mjUNLIKELY(x) (x)
#endif
#endif // MUJOCO_SRC_ENGINE_ENGINE_CROSSPLATFORM_H_
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
+13 -12
View File
@@ -27,6 +27,7 @@
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
@@ -196,13 +197,13 @@ 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);
// clear row counters
memset(cnt, 0, nv*sizeof(int));
mju_zeroInt(cnt, nv);
// save qfrc_passive, assume mj_fwdVelocity was called
mju_copy(qfrc_passive, d->qfrc_passive, nv);
@@ -236,7 +237,7 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) {
// restore
mj_fwdVelocity(m, d);
mjFREESTACK;
mj_freeStack(d);
}
@@ -247,14 +248,14 @@ 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);
int* cnt = mj_stackAllocInt(d, nv);
// clear row counters
memset(cnt, 0, nv*sizeof(int));
mju_zeroInt(cnt, nv);
// loop over dofs
for (int i=0; i < nv; i++) {
@@ -302,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);
}
@@ -329,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
@@ -557,7 +558,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -579,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)
@@ -610,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)
@@ -634,7 +635,6 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio
mjtNum *DsDq, mjtNum *DsDv, mjtNum *DsDa,
mjtNum *DmDq) {
int nq = m->nq, nv = m->nv, nM = m->nM, ns = m->nsensordata;
mjMARKSTACK;
if (m->opt.integrator == mjINT_RK4) {
mjERROR("RK4 integrator is not supported");
@@ -648,6 +648,7 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio
int skipsensor = !DsDq && !DsDv && !DsDa;
// local vectors
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
@@ -730,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);
}
@@ -665,7 +665,6 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
mjtNum C[9], T[9], *X[10], *F[10], *dX;
const mjtNum* A = (N == 4 ? RK4_A : 0);
const mjtNum* B = (N == 4 ? RK4_B : 0);
mjMARKSTACK;
// check order
if (!A) {
@@ -673,6 +672,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
}
// allocate space for intermediate solutions
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 -8
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,6 +119,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
// if disabled or no dof damping, nothing to do
if (!dof_damping) {
mj_freeStack(d);
return;
}
@@ -168,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);
}
@@ -185,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
@@ -195,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);
}
@@ -205,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;
@@ -258,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);
}
@@ -277,7 +278,6 @@ void mj_inverse(const mjModel* m, mjData* d) {
void mj_compareFwdInv(const mjModel* m, mjData* d) {
int nv = m->nv, nefc = d->nefc;
mjtNum *qforce, *dif, *save_qfrc_constraint, *save_efc_force;
mjMARKSTACK;
// clear result, return if no constraints
d->solver_fwdinv[0] = d->solver_fwdinv[1] = 0;
@@ -286,6 +286,7 @@ void mj_compareFwdInv(const mjModel* m, mjData* d) {
}
// allocate
mj_markStack(d);
qforce = mj_stackAllocNum(d, nv);
dif = mj_stackAllocNum(d, nv);
save_qfrc_constraint = mj_stackAllocNum(d, nv);
@@ -313,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
+133 -51
View File
@@ -26,7 +26,7 @@
#include <mujoco/mjmacro.h>
#include <mujoco/mjplugin.h>
#include <mujoco/mjxmacro.h>
#include "engine/engine_array_safety.h" // IWYU pragma: keep
#include "engine/engine_crossplatform.h"
#include "engine/engine_resource.h"
#include "engine/engine_macro.h"
#include "engine/engine_plugin.h"
@@ -35,6 +35,11 @@
#include "engine/engine_util_misc.h"
#include "engine/engine_vfs.h"
#ifdef ADDRESS_SANITIZER
#include <sanitizer/asan_interface.h>
#include <sanitizer/common_interface_defs.h>
#endif
#ifdef MEMORY_SANITIZER
#include <sanitizer/msan_interface.h>
#endif
@@ -43,8 +48,30 @@
#pragma warning (disable: 4305) // disable MSVC warning: truncation from 'double' to 'float'
#endif
// add red zone padding when built with asan, to detect out-of-bound accesses
#ifdef ADDRESS_SANITIZER
#define mjREDZONE 32
#else
#define mjREDZONE 0
#endif
static const int MAX_ARRAY_SIZE = INT_MAX / 4;
// compute a % b with a fast code path if the second argument is a power of 2
static inline size_t fastmod(size_t a, size_t b) {
// (b & (b - 1)) == 0 implies that b is a power of 2
if (mjLIKELY((b & (b - 1)) == 0)) {
return a & (b - 1);
}
return a % b;
}
typedef struct {
size_t pbase; // value of d->pbase immediately before mj_markStack
size_t pstack; // value of d->pstack immediately before mj_markStack
void* pc; // program counter of the call site of mj_markStack (only set when under asan)
} mjStackFrame;
//------------------------------ mjLROpt -----------------------------------------------------------
// set default options for length range computation
@@ -86,6 +113,9 @@ void mj_defaultSolRefImp(mjtNum* solref, mjtNum* solimp) {
// set model options to default values
void mj_defaultOption(mjOption* opt) {
// fill opt with zeros in case struct is padded
memset(opt, 0, sizeof(mjOption));
// timing parameters
opt->timestep = 0.002;
opt->apirate = 100;
@@ -93,6 +123,7 @@ void mj_defaultOption(mjOption* opt) {
// solver parameters
opt->impratio = 1;
opt->tolerance = 1e-8;
opt->ls_tolerance = 0.01;
opt->noslip_tolerance = 1e-6;
opt->mpr_tolerance = 1e-6;
@@ -120,6 +151,7 @@ void mj_defaultOption(mjOption* opt) {
opt->jacobian = mjJAC_AUTO;
opt->solver = mjSOL_NEWTON;
opt->iterations = 100;
opt->ls_iterations = 50;
opt->noslip_iterations = 0;
opt->mpr_iterations = 50;
opt->disableflags = 0;
@@ -656,7 +688,7 @@ void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buff
// load model from binary MJB resource
static mjModel* _mj_loadModel(const char* filename, int vfs_provider) {
mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
int header[NHEADER] = {0};
int expected_header[NHEADER] = {ID, sizeof(mjtNum), getnint(), getnsize(), getnptr()};
int ints[256];
@@ -665,8 +697,11 @@ static mjModel* _mj_loadModel(const char* filename, int vfs_provider) {
mjModel *m = 0;
mjResource* r = NULL;
if((r = mju_openResource(filename, vfs_provider)) == NULL) {
return NULL;
// first try vfs, otherwise try a provider or OS filesystem
if ((r = mju_openVfsResource(filename, vfs)) == NULL) {
if ((r = mju_openResource(filename)) == NULL) {
return NULL;
}
}
const void* buffer = NULL;
@@ -780,27 +815,6 @@ static mjModel* _mj_loadModel(const char* filename, int vfs_provider) {
// load model from binary MJB file
// if vfs is not NULL, look up file in vfs before reading from disk
mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
if (vfs == NULL) {
return _mj_loadModel(filename, 0);
}
int index = mj_registerVfsProvider(vfs);
if (index < 1) {
mjERROR("could not allocate memory");
return NULL;
}
mjModel* model = _mj_loadModel(filename, index);
mjp_unregisterResourceProvider(index);
return model;
}
// de-allocate mjModel
void mj_deleteModel(mjModel* m) {
if (m) {
@@ -841,11 +855,11 @@ 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
memset(rownnz, 0, nv * sizeof(int));
mju_zeroInt(rownnz, nv);
for (int i = nv - 1; i >= 0; i--) {
// init at diagonal
int j = i;
@@ -865,7 +879,7 @@ static void makeDSparse(const mjModel* m, mjData* d) {
}
// populate colind
memcpy(remaining, rownnz, nv * sizeof(int));
mju_copyInt(remaining, rownnz, nv);
for (int i = nv - 1; i >= 0; i--) {
// init at diagonal
remaining[i]--;
@@ -889,7 +903,7 @@ static void makeDSparse(const mjModel* m, mjData* d) {
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -902,7 +916,7 @@ static void makeBSparse(const mjModel* m, mjData* d) {
int* colind = d->B_colind;
// set rownnz to subtree dofs counts, including self
memset(rownnz, 0, sizeof(int) * nbody);
mju_zeroInt(rownnz, nbody);
for (int i = nbody - 1; i > 0; i--) {
rownnz[i] += m->body_dofnum[i];
rownnz[m->body_parentid[i]] += rownnz[i];
@@ -934,9 +948,9 @@ static void makeBSparse(const mjModel* m, mjData* d) {
}
// allocate and clear incremental row counts
mjMARKSTACK;
mj_markStack(d);
int* cnt = mj_stackAllocInt(d, nbody);
memset(cnt, 0, sizeof(int) * nbody);
mju_zeroInt(cnt, nbody);
// add subtree dofs to colind
for (int i = nbody - 1; i > 0; i--) {
@@ -982,7 +996,7 @@ static void makeBSparse(const mjModel* m, mjData* d) {
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -1105,6 +1119,8 @@ static mjData* _makeData(const mjModel* m) {
}
}
d->threadpool = 0;
return d;
}
@@ -1193,6 +1209,8 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) {
}
}
dest->threadpool = src->threadpool;
return dest;
}
@@ -1200,12 +1218,12 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) {
// allocate memory from the mjData arena
void* mj_arenaAlloc(mjData* d, size_t bytes, size_t alignment) {
size_t misalignment = d->parena % alignment;
size_t misalignment = fastmod(d->parena, alignment);
size_t padding = misalignment ? alignment - misalignment : 0;
// check size
size_t bytes_available = d->narena - d->pstack;
if (d->parena + padding + bytes > bytes_available) {
if (mjUNLIKELY(d->parena + padding + bytes > bytes_available)) {
return NULL;
}
@@ -1231,17 +1249,10 @@ void* mj_arenaAlloc(mjData* d, size_t bytes, size_t alignment) {
// declared inline so that modular arithmetic with specific alignments can be optimized out
static inline void* stackalloc(mjData* d, size_t size, size_t alignment) {
// return NULL if empty
if (!size) {
if (mjUNLIKELY(!size)) {
return NULL;
}
// add red zone padding when built with asan, to detect out-of-bound accesses
#ifdef ADDRESS_SANITIZER
#define mjREDZONE 32
#else
#define mjREDZONE 0
#endif
// size of entire arena/stack in bytes
size_t stack_size_bytes = d->narena;
@@ -1255,7 +1266,7 @@ static inline void* stackalloc(mjData* d, size_t size, size_t alignment) {
uintptr_t start_ptr = end_ptr - (size + mjREDZONE);
// align the pointer
start_ptr -= start_ptr % alignment;
start_ptr -= fastmod(start_ptr, alignment);
// new top of the stack
uintptr_t new_pstack_ptr = start_ptr - mjREDZONE;
@@ -1268,8 +1279,8 @@ static inline void* stackalloc(mjData* d, size_t size, size_t alignment) {
// check size
size_t stack_available_bytes = end_ptr - ((uintptr_t)d->arena + d->parena);
size_t stack_required_bytes = end_ptr - new_pstack_ptr;
if (stack_required_bytes > stack_available_bytes) {
mju_error("mjData stack overflow: max = %zu, available = %zu, requested = %zu "
if (mjUNLIKELY(stack_required_bytes > stack_available_bytes)) {
mju_error("mj_stackAlloc: insufficient memory: max = %zu, available = %zu, requested = %zu "
"(ne = %d, nf = %d, nefc = %d, ncon = %d)",
stack_size_bytes, stack_available_bytes, stack_required_bytes,
d->ne, d->nf, d->nefc, d->ncon);
@@ -1285,16 +1296,14 @@ static inline void* stackalloc(mjData* d, size_t size, size_t alignment) {
}
// store new stack usage in the red zone
ASAN_UNPOISON_MEMORY_REGION(new_pstack_ptr, sizeof(size_t));
ASAN_UNPOISON_MEMORY_REGION((void*)new_pstack_ptr, sizeof(size_t));
*(size_t*)new_pstack_ptr = usage;
ASAN_POISON_MEMORY_REGION(new_pstack_ptr, sizeof(size_t));
ASAN_POISON_MEMORY_REGION((void*)new_pstack_ptr, sizeof(size_t));
// unpoison the actual usable allocation
ASAN_UNPOISON_MEMORY_REGION(start_ptr, size);
ASAN_UNPOISON_MEMORY_REGION((void*)start_ptr, size);
#endif
#undef mjREDZONE
// update pstack and max usage statistics
d->pstack = new_pstack;
d->maxuse_stack = mjMAX(d->maxuse_stack, usage);
@@ -1303,6 +1312,73 @@ static inline void* stackalloc(mjData* d, size_t size, size_t alignment) {
return (void*)start_ptr;
}
// mjData mark stack frame
#ifdef ADDRESS_SANITIZER
__attribute__((noinline))
#endif
void mj_markStack(mjData* d) {
size_t pstack_old = d->pstack;
mjStackFrame* s =
(mjStackFrame*) stackalloc(d, sizeof(mjStackFrame), _Alignof(mjStackFrame));
s->pbase = d->pbase;
s->pstack = pstack_old;
#ifdef ADDRESS_SANITIZER
// store the program counter to the caller so that we can compare against mj_freeStack later
s->pc = __sanitizer_return_address();
#endif
d->pbase = d->pstack - mjREDZONE;
}
// mjData free stack frame
#ifdef ADDRESS_SANITIZER
__attribute__((noinline))
#endif
void mj_freeStack(mjData* d) {
if (mjUNLIKELY(!d->pbase)) {
return;
}
mjStackFrame* s = (mjStackFrame*) ((char*)d->arena + d->narena - d->pbase);
#ifdef ADDRESS_SANITIZER
#define mjSYMBOLIZELEN 256
// symbolize s->pc to get the function name of most recent caller to mj_markStack
char markstack_func[mjSYMBOLIZELEN];
__sanitizer_symbolize_pc(s->pc, "%f", markstack_func, mjSYMBOLIZELEN);
markstack_func[mjSYMBOLIZELEN - 1] = '\0';
// symbolize current program counter to get the function name of caller to this function
char freestack_func[mjSYMBOLIZELEN];
__sanitizer_symbolize_pc(__sanitizer_return_address(), "%f", freestack_func, mjSYMBOLIZELEN);
freestack_func[mjSYMBOLIZELEN - 1] = '\0';
// raise an error if caller function name doesn't match the most recent caller of mj_markStack
if (strncmp(markstack_func, freestack_func, mjSYMBOLIZELEN)) {
char dbginfo[mjSYMBOLIZELEN];
__sanitizer_symbolize_pc(
s->pc, "mj_markStack %F at %S has no corresponding mj_freeStack",
dbginfo, sizeof(dbginfo));
dbginfo[mjSYMBOLIZELEN - 1] = '\0';
mjERROR("%s", dbginfo);
}
#undef mjSYMBOLIZELEN
#endif
// restore pbase and pstack
d->pbase = s->pbase;
d->pstack = s->pstack;
// if running under asan, poison the newly freed memory region
#ifdef ADDRESS_SANITIZER
ASAN_POISON_MEMORY_REGION((char*)d->arena + d->parena, d->narena - d->pstack - d->parena);
#endif
}
void* mj_stackAlloc(mjData* d, size_t bytes, size_t alignment) {
return stackalloc(d, bytes, alignment);
}
@@ -1329,6 +1405,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
// clear stack pointer
d->pstack = 0;
d->pbase = 0;
// clear arena pointers
d->parena = 0;
@@ -1496,6 +1573,11 @@ void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key) {
// de-allocate mjData
void mj_deleteData(mjData* d) {
if (d) {
#ifdef ADDRESS_SANITIZER
// raise an error if there's a dangling stack frame
mj_freeStack(d);
#endif
// destroy plugin instances
for (int i = 0; i < d->nplugin; ++i) {
const mjpPlugin* plugin = mjp_getPluginAtSlot(d->plugin[i]);
src/engine/engine_io.h
+17
View File
@@ -18,6 +18,7 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjxmacro.h>
#ifdef __cplusplus
#include <cstddef>
@@ -105,6 +106,12 @@ MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
// mjData arena allocate
MJAPI void* mj_arenaAlloc(mjData* d, size_t bytes, size_t alignment);
// mjData mark stack frame
MJAPI void mj_markStack(mjData* d);
// mjData free stack frame
MJAPI void mj_freeStack(mjData* d);
// mjData stack allocate
MJAPI void* mj_stackAlloc(mjData* d, size_t bytes, size_t alignment);
@@ -117,6 +124,16 @@ MJAPI int* mj_stackAllocInt(mjData* d, int size);
// de-allocate data
MJAPI void mj_deleteData(mjData* d);
// clear arena pointers in mjData
static inline void mj_clearEfc(mjData* d) {
#define X(type, name, nr, nc) d->name = NULL;
MJDATA_ARENA_POINTERS
#undef X
d->nefc = 0;
d->nisland = 0;
d->contact = (mjContact*) d->arena;
}
#ifdef __cplusplus
}
#endif
src/engine/engine_island.c
+125 -72
View File
@@ -16,17 +16,15 @@
#include <stdio.h>
#include <stddef.h>
#include <string.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"
#include "engine/engine_util_misc.h"
#ifdef MEMORY_SANITIZER
#include <sanitizer/msan_interface.h>
@@ -75,7 +73,7 @@ int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, cons
island[v] = nisland;
// push adjacent vertices onto stack
memcpy(stack + nstack, colind + rowadr[v], rownnz[v]*sizeof(int));
mju_copyInt(stack + nstack, colind + rowadr[v], rownnz[v]);
nstack += rownnz[v];
}
@@ -160,13 +158,12 @@ static int countMaxEdge(const mjModel* m, const mjData* d) {
// return id of next tree in Jacobian row i that is different from tree, -1 if not found
// write the index of the found tree to *index if given
// start search from *index if given, otherwise 0
// if J is (dense/sparse) *index is the (column/nonzro) index, respectively
// start search from *index
// write the index of the found tree to *index
// if J is (dense/sparse) *index is the (column/nonzero) index, respectively
static int treeNext(const mjModel* m, const mjData* d, int tree, int i, int *index) {
int tree_next = -1;
int j0 = index ? *index : 0; // start searching at *index if given, otherwise 0
int j; // loop variable, saved to *index
int j; // local loop variable, saved to *index
// sparse
if (mj_isSparse(m)) {
@@ -174,7 +171,7 @@ static int treeNext(const mjModel* m, const mjData* d, int tree, int i, int *ind
int* colind = d->efc_J_colind + d->efc_J_rowadr[i];
// loop over remaining nonzeros, look for different tree
for (j=j0; j < rownnz; j++) {
for (j=(*index); j < rownnz; j++) {
int tree_j = m->dof_treeid[colind[j]];
if (tree_j != tree) {
// found different tree
@@ -189,7 +186,7 @@ static int treeNext(const mjModel* m, const mjData* d, int tree, int i, int *ind
int nv = m->nv;
// scan row, look for different tree
for (j=j0; j < nv; j++) {
for (j=(*index); j < nv; j++) {
if (d->efc_J[nv*i + j]) {
int tree_j = m->dof_treeid[j];
if (tree_j != tree) {
@@ -202,13 +199,94 @@ static int treeNext(const mjModel* m, const mjData* d, int tree, int i, int *ind
}
// save last index
if (index) *index = j;
*index = j;
return tree_next;
}
// find first and possibly second nonegative tree ids in Jacobian row i
// if row i is special-cased (no more trees), return -1
// otherwise call treeNext, starting scan at index 0, return index
static int treeFirst(const mjModel* m, const mjData* d, int tree[2], int i) {
int efc_type = d->efc_type[i];
int efc_id = d->efc_id[i];
// clear outputs
tree[0] = -1;
tree[1] = -1;
// ==== fast handling of special cases
// joint friction
if (efc_type == mjCNSTR_FRICTION_DOF) {
tree[0] = m->dof_treeid[efc_id];
return -1;
}
// joint limit
if (efc_type == mjCNSTR_LIMIT_JOINT) {
tree[0] = m->dof_treeid[m->jnt_dofadr[efc_id]];
return -1;
}
// contact
if (efc_type == mjCNSTR_CONTACT_FRICTIONLESS ||
efc_type == mjCNSTR_CONTACT_PYRAMIDAL ||
efc_type == mjCNSTR_CONTACT_ELLIPTIC) {
tree[0] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom1]];
tree[1] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom2]];
// handle static bodies
if (tree[0] < 0) {
if (tree[1] < 0) {
mjERROR("contact %d is between two static bodies", efc_id); // SHOULD NOT OCCUR
} else {
int tmp = tree[0];
tree[0] = tree[1];
tree[1] = tmp;
}
}
return -1;
}
// connect or weld constraints
if (efc_type == mjCNSTR_EQUALITY) {
mjtEq eq_type = m->eq_type[efc_id];
if (eq_type == mjEQ_CONNECT || eq_type == mjEQ_WELD) {
tree[0] = m->body_treeid[m->eq_obj1id[efc_id]];
tree[1] = m->body_treeid[m->eq_obj2id[efc_id]];
// handle static bodies
if (tree[0] < 0) {
if (tree[1] < 0) {
mjERROR("equality %d is between two static bodies", efc_id); // SHOULD NOT OCCUR
} else {
int tmp = tree[0];
tree[0] = tree[1];
tree[1] = tmp;
}
}
return -1;
}
}
// ==== generic case: scan Jacobian
int index = 0;
tree[0] = treeNext(m, d, -1, i, &index);
if (tree[0] < 0) {
mjERROR("no tree found for constraint %d", i); // SHOULD NOT OCCUR
}
return index;
}
// add 0 edges, 1 self-edge or 2 flipped edges to array, increment treenedge
// return current number of edges
static int addEdge(int* treenedge, int* edge, int nedge, int tree1, int tree2, int nedge_max) {
@@ -273,10 +351,9 @@ static int findEdges(const mjModel* m, const mjData* d, int* treenedge, int* edg
int nefc = d->nefc;
int efc_type = -1;
int efc_id = -1;
int tree1, tree2;
// clear treenedge
memset(treenedge, 0, m->ntree*sizeof(int));
mju_zeroInt(treenedge, m->ntree);
int nedge = 0;
for (int i=0; i < nefc; i++) {
@@ -287,60 +364,34 @@ static int findEdges(const mjModel* m, const mjData* d, int* treenedge, int* edg
efc_type = d->efc_type[i];
efc_id = d->efc_id[i];
// ==== fast handling of special cases
int tree[2];
int index = treeFirst(m, d, tree, i);
int tree1 = tree[0];
int tree2 = tree[1];
// joint friction
if (efc_type == mjCNSTR_FRICTION_DOF) {
tree1 = m->dof_treeid[efc_id];
nedge = addEdge(treenedge, edge, nedge, tree1, tree1, nedge_max);
// no more edges to find, add and continue
if (index == -1) {
nedge = addEdge(treenedge, edge, nedge, tree1, tree2 == -1 ? tree1 : tree2, nedge_max);
continue;
}
// joint limit
if (efc_type == mjCNSTR_LIMIT_JOINT) {
tree1 = m->dof_treeid[m->jnt_dofadr[efc_id]];
nedge = addEdge(treenedge, edge, nedge, tree1, tree1, nedge_max);
continue;
}
// possibly more edges, scan Jacobian row
else {
tree2 = treeNext(m, d, tree1, i, &index);
// contact
if (efc_type == mjCNSTR_CONTACT_FRICTIONLESS ||
efc_type == mjCNSTR_CONTACT_PYRAMIDAL ||
efc_type == mjCNSTR_CONTACT_ELLIPTIC) {
tree1 = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom1]];
tree2 = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom2]];
nedge = addEdge(treenedge, edge, nedge, tree1, tree2, nedge_max);
continue;
}
// connect or weld constraints
if (efc_type == mjCNSTR_EQUALITY) {
mjtEq eq_type = m->eq_type[efc_id];
if (eq_type == mjEQ_CONNECT || eq_type == mjEQ_WELD) {
tree1 = m->body_treeid[m->eq_obj1id[efc_id]];
tree2 = m->body_treeid[m->eq_obj2id[efc_id]];
if (tree2 == -1) {
// 1 tree found: add self-edge
nedge = addEdge(treenedge, edge, nedge, tree1, tree1, nedge_max);
} else {
// 2 trees found: add edge, keep scanning and adding until no more trees
nedge = addEdge(treenedge, edge, nedge, tree1, tree2, nedge_max);
continue;
}
}
// ==== generic case: scan Jacobian
int index = 0;
tree1 = treeNext(m, d, -1, i, &index);
tree2 = treeNext(m, d, tree1, i, &index);
if (tree2 == -1) {
// 1 tree found: add self-edge
nedge = addEdge(treenedge, edge, nedge, tree1, tree1, nedge_max);
} else {
// 2 trees found: add edge, keep scanning and adding until no more trees
nedge = addEdge(treenedge, edge, nedge, tree1, tree2, nedge_max);
int tree3 = treeNext(m, d, tree2, i, &index);
while (tree3 > -1 && tree3 != tree2) {
tree1 = tree2;
tree2 = tree3;
nedge = addEdge(treenedge, edge, nedge, tree1, tree2, nedge_max);
tree3 = treeNext(m, d, tree2, i, &index);
int tree3 = treeNext(m, d, tree2, i, &index);
while (tree3 > -1 && tree3 != tree2) {
tree1 = tree2;
tree2 = tree3;
nedge = addEdge(treenedge, edge, nedge, tree1, tree2, nedge_max);
tree3 = treeNext(m, d, tree2, i, &index);
}
}
}
}
@@ -361,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);
@@ -395,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;
}
@@ -403,10 +454,10 @@ void mj_island(const mjModel* m, mjData* d) {
// compute dof_island, island_dofnum
int num_dof_unc = 0; // number of unconstrained dofs
memset(d->island_dofnum, 0, nisland*sizeof(int));
mju_zeroInt(d->island_dofnum, nisland);
for (int i=0; i < nv; i++) {
// dof_island
int island = tree_island[m->dof_treeid[i]];;
int island = tree_island[m->dof_treeid[i]];
d->dof_island[i] = island;
// island_dofnum
@@ -424,7 +475,7 @@ void mj_island(const mjModel* m, mjData* d) {
}
// reset island_dofnum
memset(d->island_dofnum, 0, nisland*sizeof(int));
mju_zeroInt(d->island_dofnum, nisland);
// compute dof_islandind, island_dofind
int num_dof_island = 0;
@@ -444,15 +495,17 @@ void mj_island(const mjModel* m, mjData* d) {
mjERROR("not all islands assigned to dofs");
}
// finalize dof_islandind: set remaning indices to -1
// finalize dof_islandind: set remaining indices to -1
for (int i=num_dof_island; i < nv; i++) {
d->island_dofind[i] = -1;
}
// compute efc_island, island_efcnum
memset(d->island_efcnum, 0, nisland*sizeof(int));
mju_zeroInt(d->island_efcnum, nisland);
for (int i=0; i < nefc; i++) {
int island = tree_island[treeNext(m, d, -1, i, NULL)];
int tree[2];
treeFirst(m, d, tree, i);
int island = tree_island[tree[0]];
d->efc_island[i] = island;
d->island_efcnum[island]++;
}
@@ -464,7 +517,7 @@ void mj_island(const mjModel* m, mjData* d) {
}
// reset island_efcnum
memset(d->island_efcnum, 0, nisland*sizeof(int));
mju_zeroInt(d->island_efcnum, nisland);
// compute efc_islandind
for (int i=0; i < nefc; i++) {
@@ -472,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_macro.h
+3 -3
View File
@@ -24,9 +24,9 @@
// thread local macro
#ifdef _MSC_VER
#define mjTHREADLOCAL __declspec(thread)
#define mjTHREADLOCAL __declspec(thread)
#else
#define mjTHREADLOCAL _Thread_local
#define mjTHREADLOCAL _Thread_local
#endif
@@ -41,7 +41,7 @@
//-------------------------- compiler builtin ------------------------------------------------------
#ifndef __has_builtin
#define __has_builtin(x) 0
#define __has_builtin(x) 0
#endif
//-------------------------- pointer arithmetic ----------------------------------------------------
src/engine/engine_plugin.cc
+17 -102
View File
@@ -24,6 +24,7 @@
#include <cctype>
#include <cstddef>
#include <cstdlib>
#include <cstdio>
#include <cstring>
#include <memory>
#include <mutex>
@@ -57,9 +58,6 @@ constexpr int kMaxAttributes = 255;
constexpr int kCacheLine = 256;
// vfs prefix
constexpr const char* kVfsPrefix = mjVFS_PREFIX;
// A table of registered plugins, implemented as a linked list of array "blocks".
// This is a compromise that maintains a good degree of memory locality while not invalidating
// existing pointers when growing the table. It is expected that for most users, the number of
@@ -270,6 +268,8 @@ bool ResourceProvidersAreIdentical(const mjpResourceProvider* p1, const mjpResou
p1->open == p2->open &&
p1->read == p2->read &&
p1->close == p2->close &&
p1->getdir == p2->getdir &&
p1->modified == p2->modified &&
p1->data == p2->data);
}
@@ -532,18 +532,7 @@ void mjp_defaultResourceProvider(mjpResourceProvider* provider) {
// globally register a resource provider (thread-safe), return new slot id
int mjp_registerResourceProvider(const mjpResourceProvider* provider) {
// check against reserved prefixes
if (PrefixesAreIdentical(kVfsPrefix, provider->prefix)) {
mju_warning("provider->prefix is '%s' which is reserved", provider->prefix);
return -1;
}
return mjp_registerResourceProviderInternal(provider);
}
// internal version of mjp_registerResourceProvider without prechecks on reserved prefixes
int mjp_registerResourceProviderInternal(const mjpResourceProvider* provider) {
// check if prefix is valid URI scheme format
// check if prefix is valid URI scheme format
if (!IsValidURISchemeFormat(provider->prefix)) {
mju_warning("provider->prefix is '%s' which is not a valid URI scheme format",
provider->prefix);
@@ -562,35 +551,25 @@ int mjp_registerResourceProviderInternal(const mjpResourceProvider* provider) {
// Do not handle objects with nontrivial destructors outside of this lambda.
// Do not call mju_error inside this lambda.
int slot = [&]() -> int {
bool vfs_provider = false;
std::unique_ptr<char[]> prefix;
// check if this is a VFS provider
if (PrefixesAreIdentical(mjVFS_PREFIX, provider->prefix)) {
vfs_provider = true;
}
// copy prefix
if (!vfs_provider) {
prefix = CopyName(provider->prefix);
if (!prefix) {
if (strklen(provider->prefix) == -1) {
std::snprintf(err, sizeof(err),
"provider->prefix length exceeds the maximum limit of %d", kMaxNameLength);
} else {
std::snprintf(err, sizeof(err), "failed to allocate memory for resource provider prefix");
}
return -1;
prefix = CopyName(provider->prefix);
if (!prefix) {
if (strklen(provider->prefix) == -1) {
std::snprintf(err, sizeof(err),
"provider->prefix length exceeds the maximum limit of %d", kMaxNameLength);
} else {
std::snprintf(err, sizeof(err), "failed to allocate memory for resource provider prefix");
}
return -1;
}
Global<mjpResourceProvider>& global = GetGlobal<mjpResourceProvider>();
auto lock = global.lock_mutex_exclusively();
int count = global.count().load(std::memory_order_acquire);
int local_idx = 0;
int free_idx = -1, free_local_idx = -1;
PluginTable<mjpResourceProvider>* table = &global.table();
PluginTable<mjpResourceProvider>* free_table = nullptr;
// check if a non-identical provider with the same name has already been registered
for (int i = 0; i < count; ++i, ++local_idx) {
@@ -600,17 +579,7 @@ int mjp_registerResourceProviderInternal(const mjpResourceProvider* provider) {
}
mjpResourceProvider& existing = table->plugins[local_idx];
// VFS providers can safely go in open slots
if (vfs_provider && existing.prefix == nullptr && free_idx == -1) {
free_table = table;
free_idx = i;
free_local_idx = local_idx;
// can skip the rest
break;
}
if (!vfs_provider && existing.prefix != nullptr) {
if (existing.prefix != nullptr) {
// if identical then return slot number
if (PrefixesAreIdentical(provider->prefix, existing.prefix)) {
if (ResourceProvidersAreIdentical(provider, &existing)) {
@@ -626,7 +595,7 @@ int mjp_registerResourceProviderInternal(const mjpResourceProvider* provider) {
}
// allocate a new block of PluginTable if the last allocated block is full
if (free_local_idx == -1 && local_idx == PluginTable<mjpResourceProvider>::kBlockSize) {
if (local_idx == PluginTable<mjpResourceProvider>::kBlockSize) {
local_idx = 0;
table = AddNewTableBlock<mjpResourceProvider>(table);
if (!table) {
@@ -636,19 +605,12 @@ int mjp_registerResourceProviderInternal(const mjpResourceProvider* provider) {
// all checks passed, register the plugin into the global table
mjpResourceProvider& registered_provider = table->plugins[local_idx];
if (free_local_idx != -1) {
registered_provider = free_table->plugins[free_local_idx];
}
registered_provider = *provider;
registered_provider.prefix = (!vfs_provider) ? prefix.release() : kVfsPrefix;
registered_provider.prefix = prefix.release();
// increment the global plugin count with a release memory barrier
if (free_idx == -1) {
free_idx = count;
global.count().store(count + 1, std::memory_order_release);
}
return free_idx;
global.count().store(count + 1, std::memory_order_release);
return count;
}();
// ========= ATTENTION! ==========================================================================
@@ -663,46 +625,6 @@ int mjp_registerResourceProviderInternal(const mjpResourceProvider* provider) {
return slot+1;
}
// globally unregister resource provider (thread-safe)
// only used for VFS resource providers
void mjp_unregisterResourceProvider(int slot) {
// shift slot to zero-index
slot--;
if (slot < 0) {
return;
}
// get global table, acquire lock
Global<mjpResourceProvider>& global = GetGlobal<mjpResourceProvider>();
auto lock = global.lock_mutex_exclusively();
int count = global.count().load(std::memory_order_acquire);
if (slot >= count) {
return;
}
PluginTable<mjpResourceProvider>* table = &global.table();
// iterate over blocks in the global table until the local index is less than the block size
int local_idx = slot;
while (local_idx >= PluginTable<mjpResourceProvider>::kBlockSize) {
local_idx -= PluginTable<mjpResourceProvider>::kBlockSize;
table = table->next;
if (!table) {
return;
}
}
// local_idx is now a valid index into the current block
mjpResourceProvider& provider = table->plugins[local_idx];
// no-op for anything other than VFS resource providers
if (provider.prefix == kVfsPrefix) {
provider.prefix = nullptr;
}
}
// return the number of globally registered resource providers
int mjp_resourceProviderCount() {
return GetGlobal<mjpResourceProvider>().count().load(std::memory_order_acquire);
@@ -728,12 +650,6 @@ const mjpResourceProvider* mjp_getResourceProvider(const char* resource_name) {
return nullptr;
}
// since multiple VFS resource providers can be registered with the same
// prefix, it doesn't make sense to try to match against them
if (PrefixesAreIdentical(kVfsPrefix, file_prefix.c_str())) {
return nullptr;
}
Global<mjpResourceProvider>& global = GetGlobal<mjpResourceProvider>();
auto lock = global.lock_mutex_exclusively();
PluginTable<mjpResourceProvider>* table = &global.table();
@@ -743,7 +659,6 @@ const mjpResourceProvider* mjp_getResourceProvider(const char* resource_name) {
for (int i = 0;
i < PluginTable<mjpPlugin>::kBlockSize && found_slot < count;
++i, ++found_slot) {
const mjpResourceProvider& provider = table->plugins[i];
const char *prefix = provider.prefix;
src/engine/engine_plugin.h
+2 -5
View File
@@ -31,14 +31,11 @@ MJAPI int mjp_registerPlugin(const mjpPlugin* plugin);
// globally register a resource provider (thread-safe), return new slot id
MJAPI int mjp_registerResourceProvider(const mjpResourceProvider* provider);
// globally unregister a resource provider (thread-safe)
MJAPI void mjp_unregisterResourceProvider(int slot);
// return the number of globally registered plugins
MJAPI int mjp_pluginCount();
MJAPI int mjp_pluginCount(void);
// return the number of globally registered resource providers
MJAPI int mjp_resourceProviderCount();
MJAPI int mjp_resourceProviderCount(void);
// look up a plugin by name, optionally also get its registered slot number
MJAPI const mjpPlugin* mjp_getPlugin(const char* name, int* slot);
src/engine/engine_print.c
+22 -9
View File
@@ -730,8 +730,13 @@ void mj_printModel(const mjModel* m, const char* filename) {
// valid printf-style format string for a single float value
void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
const char* float_format) {
// stack in use, SHOULD NOT OCCUR
if (d->pstack) {
mjERROR("attempting to print mjData when stack is in use");
}
mjtNum *M;
mjMARKSTACK;
mj_markStack(d);
// check format string
if (!validateFloatFormat(float_format)) {
@@ -739,11 +744,6 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
float_format = FLOAT_FORMAT;
}
// stack in use, SHOULD NOT OCCUR
if (d->pstack) {
mjERROR("attempting to print mjData when stack is in use");
}
// get file
FILE* fp;
if (filename) {
@@ -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;
}
@@ -776,7 +776,10 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
fprintf(fp, "SIZES\n");
#define X(type, name) \
if (strcmp(#name, "pstack") != 0 && strcmp(#name, "parena") != 0) { \
if (strcmp(#name, "pstack") != 0 && \
strcmp(#name, "pbase") != 0 && \
strcmp(#name, "parena") != 0 && \
strcmp(#name, "threadpool") != 0) { \
const char* format = _Generic( \
d->name, \
int : INT_FORMAT, \
@@ -792,6 +795,16 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
MJDATA_SCALAR
#undef X
int threadpool = 0;
if (d->threadpool) {
threadpool = 1;
}
fprintf(fp, " ");
fprintf(fp, NAME_FORMAT, "threadpool");
fprintf(fp, INT_FORMAT, threadpool);
fprintf(fp, "\n");
fprintf(fp, "\n");
// WARNING
@@ -1132,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_resource.c
+92 -82
View File
@@ -16,40 +16,35 @@
#include <limits.h>
#include <stddef.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <time.h>
#if defined (__unix__) || (defined (__APPLE__) && defined (__MACH__))
#include <unistd.h>
#endif
#ifdef _WIN32
#define stat _stat
#endif
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include "engine/engine_plugin.h"
#include "engine/engine_util_errmem.h"
// file buffer used internally for the OS filesystem
typedef struct {
void* buffer;
int nbuffer;
uint8_t* buffer; // raw bytes from file
size_t nbuffer; // size of buffer in bytes
time_t mtime; // last modified time
} file_buffer;
// helper function to fill data from resource provider into provider
static void fillResource(const mjpResourceProvider* provider, mjResource* resource) {
if (provider == NULL) {
resource->read = NULL;
resource->close = NULL;
resource->getdir = NULL;
resource->provider_data = NULL;
} else {
resource->read = provider->read;
resource->close = provider->close;
resource->getdir = provider->getdir;
resource->provider_data = provider->data;
}
}
// open the given resource; if the name doesn't have a prefix matching with a
// resource provider, then the default_provider is used
// if default_provider non-positive, then the OS filesystem is used
mjResource* mju_openResource(const char* name, int default_provider) {
// resource provider, then the OS filesystem is used
mjResource* mju_openResource(const char* name) {
mjResource* resource = (mjResource*) mju_malloc(sizeof(mjResource));
const mjpResourceProvider* provider = NULL;
if (resource == NULL) {
@@ -57,19 +52,22 @@ mjResource* mju_openResource(const char* name, int default_provider) {
return NULL;
}
// clear out resource
memset(resource, 0, sizeof(mjResource));
// copy name
resource->name = mju_malloc(sizeof(char) * (strlen(name) + 1));
if (resource->name == NULL) {
mju_free(resource);
mju_closeResource(resource);
mjERROR("could not allocate memory");
return NULL;
}
strcpy(resource->name, name);
memcpy(resource->name, name, sizeof(char) * (strlen(name) + 1));
// find provider based off prefix of name
provider = mjp_getResourceProvider(name);
if (provider != NULL) {
fillResource(provider, resource);
resource->provider = provider;
if (provider->open(resource)) {
return resource;
}
@@ -77,49 +75,27 @@ mjResource* mju_openResource(const char* name, int default_provider) {
mju_warning("mju_openResource: could not open resource '%s' "
"using a resource provider matching prefix '%s'",
name, provider->prefix);
mju_free(resource->name);
mju_free(resource);
return NULL;
}
// fallback to default provider
if (default_provider > 0) {
provider = mjp_getResourceProviderAtSlot(default_provider);
if (provider == NULL) {
mju_warning("mju_openResource: unknown resource provider at slot %d",
default_provider);
mju_free(resource->name);
mju_free(resource);
return NULL;
}
fillResource(provider, resource);
if (provider->open(resource)) {
return resource;
}
mju_warning("mju_openResource: could not open resource '%s' "
"with default provider at slot %d",
name, default_provider);
mju_free(resource->name);
mju_free(resource);
mju_closeResource(resource);
return NULL;
}
// lastly fallback to OS filesystem
else {
fillResource(NULL, resource);
resource->data = mju_malloc(sizeof(file_buffer));
file_buffer* fb = (file_buffer*) resource->data;
fb->buffer = mju_fileToMemory(name, &(fb->nbuffer));
if (fb->buffer == NULL) {
mju_warning("mju_openResource: unknown file '%s'", name);
mju_free(fb);
mju_free(resource->name);
mju_free(resource);
return NULL;
}
resource->provider = NULL;
resource->data = mju_malloc(sizeof(file_buffer));
file_buffer* fb = (file_buffer*) resource->data;
fb->buffer = mju_fileToMemory(name, &(fb->nbuffer));
if (fb->buffer == NULL) {
mju_warning("mju_openResource: unknown file '%s'", name);
mju_closeResource(resource);
return NULL;
}
struct stat file_stat;
if (stat(name, &file_stat) == 0) {
memcpy(&fb->mtime, &file_stat.st_mtime, sizeof(time_t));
} else {
memset(&fb->mtime, 0, sizeof(time_t));
}
return resource;
}
@@ -131,20 +107,20 @@ void mju_closeResource(mjResource* resource) {
return;
}
// use the resource provider to close resource
if (resource->close) {
resource->close(resource);
}
// if provider is NULL, then OS filesystem is used
else {
// use the resource provider close callback
if (resource->provider && resource->provider->close) {
resource->provider->close(resource);
} else {
// clear OS filesystem if present
file_buffer* fb = (file_buffer*) resource->data;
mju_free(fb->buffer);
mju_free(fb);
if (fb) {
if (fb->buffer) mju_free(fb->buffer);
mju_free(fb);
}
}
// free name and resource
mju_free(resource->name);
// free resource
if (resource->name) mju_free(resource->name);
mju_free(resource);
}
@@ -157,12 +133,12 @@ int mju_readResource(mjResource* resource, const void** buffer) {
return 0;
}
if (resource->read) {
return resource->read(resource, buffer);
if (resource->provider) {
return resource->provider->read(resource, buffer);
}
// if provider read callback is NULL, then OS filesystem is used
// if provider is NULL, then OS filesystem is used
const file_buffer* fb = (file_buffer*) resource->data;
*buffer = fb->buffer;
return fb->nbuffer;
@@ -175,14 +151,14 @@ void mju_getResourceDir(mjResource* resource, const char** dir, int* ndir) {
*dir = NULL;
*ndir = 0;
if (!resource) {
if (resource == NULL) {
return;
}
// provider is not OS filesystem
if (resource->read) {
if (resource->getdir) {
resource->getdir(resource, dir, ndir);
if (resource->provider) {
if (resource->provider->getdir) {
resource->provider->getdir(resource, dir, ndir);
}
} else {
*dir = resource->name;
@@ -192,6 +168,40 @@ void mju_getResourceDir(mjResource* resource, const char** dir, int* ndir) {
// modified callback for OS filesystem
static int mju_isModifiedFile(const char* name, const file_buffer* fb) {
if (fb != NULL) {
struct stat file_stat;
if (stat(name, &file_stat) == 0) {
return difftime(fb->mtime, file_stat.st_mtime) < 0;
}
return -1;
}
return -2;
}
// Returns > 0 if resource has been modified since last read, 0 if not, and < 0
// if inconclusive
int mju_isModifiedResource(const mjResource* resource) {
if (resource == NULL) {
return -2;
}
// provider is not OS filesystem
if (resource->provider) {
if (resource->provider->modified) {
return resource->provider->modified(resource);
}
return 1; // default (modified)
}
return mju_isModifiedFile(resource->name, (file_buffer*) resource->data);
}
// get the length of the dirname portion of a given path
int mju_dirnamelen(const char* path) {
if (!path) {
@@ -211,7 +221,7 @@ int mju_dirnamelen(const char* path) {
// read file into memory buffer (allocated here with mju_malloc)
void* mju_fileToMemory(const char* filename, int* filesize) {
void* mju_fileToMemory(const char* filename, size_t* filesize) {
// open file
*filesize = 0;
FILE* fp = fopen(filename, "rb");
src/engine/engine_resource.h
+9 -4
View File
@@ -15,6 +15,8 @@
#ifndef MUJOCO_SRC_ENGINE_ENGINE_RESOURCE_H_
#define MUJOCO_SRC_ENGINE_ENGINE_RESOURCE_H_
#include <stddef.h>
#include <mujoco/mjexport.h>
#include "engine/engine_plugin.h"
@@ -23,9 +25,8 @@ extern "C" {
#endif
// open the given resource; if the name doesn't have a prefix matching with a
// resource provider, then the default_provider is used
// if default_provider non-positive, then the OS filesystem is used
MJAPI mjResource* mju_openResource(const char* name, int default_provider);
// resource provider, then the OS filesystem is used
MJAPI mjResource* mju_openResource(const char* name);
// close the given resource; no-op if resource is NULL
MJAPI void mju_closeResource(mjResource* resource);
@@ -37,11 +38,15 @@ MJAPI int mju_readResource(mjResource* resource, const void** buffer);
// sets for a resource with a name partitioned as {dir}{filename}, the dir and ndir pointers
MJAPI void mju_getResourceDir(mjResource* resource, const char** dir, int* ndir);
// Returns > 0 if resource has been modified since last read, 0 if not, and < 0
// if inconclusive
MJAPI int mju_isModifiedResource(const mjResource* resource);
// get the length of the dirname portion of a given path
int mju_dirnamelen(const char* path);
// read file into memory buffer (allocated here with mju_malloc)
void* mju_fileToMemory(const char* filename, int* filesize);
void* mju_fileToMemory(const char* filename, size_t* filesize);
#ifdef __cplusplus
}
src/engine/engine_sensor.c
+2 -2
View File
@@ -854,18 +854,18 @@ void mj_energyPos(const mjModel* m, mjData* d) {
// velocity-dependent energy (kinetic)
void mj_energyVel(const mjModel* m, mjData* d) {
mjtNum *vec;
mjMARKSTACK;
// return if disabled (already cleared in potential)
if (!mjENABLED(mjENBL_ENERGY)) {
return;
}
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
+24 -26
View File
@@ -168,7 +168,8 @@ static void residual(const mjModel* m, mjData* d, mjtNum* res, int i, int dim, i
for (int j=0; j < dim; j++) {
res[j] = d->efc_b[i+j] + mju_dotSparse(d->efc_AR + d->efc_AR_rowadr[i+j],
d->efc_force, d->efc_AR_rownnz[i+j],
d->efc_AR_colind + d->efc_AR_rowadr[i+j]);
d->efc_AR_colind + d->efc_AR_rowadr[i+j],
/*flg_unc1=*/0);
}
}
@@ -308,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);
@@ -463,7 +464,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
}
// process state
memcpy(oldstate, d->efc_state, nefc*sizeof(int));
mju_copyInt(oldstate, d->efc_state, nefc);
int nactive = dualState(m, d);
int nchange = 0;
for (int i=0; i < nefc; i++) {
@@ -496,7 +497,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
// map to joint space
dualFinish(m, d);
mjFREESTACK;
mj_freeStack(d);
}
@@ -510,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);
@@ -679,7 +680,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
}
// process state
memcpy(oldstate, d->efc_state, nefc*sizeof(int));
mju_copyInt(oldstate, d->efc_state, nefc);
int nactive = dualState(m, d);
int nchange = 0;
for (int i=0; i < nefc; i++) {
@@ -702,7 +703,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
// map to joint space
dualFinish(m, d);
mjFREESTACK;
mj_freeStack(d);
}
@@ -1092,9 +1093,6 @@ static int updateBracket(const mjModel* m, mjData* d, mjCGContext* ctx,
static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
mjCGPnt p0, p1, p2, pmid, p1next, p2next;
const int LSmaxiter = 50;
const mjtNum LStolscl = 0.01;
// clear results
ctx->LSiter = 0;
ctx->LSresult = 0;
@@ -1108,7 +1106,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
}
// compute scaled gradtol and slope scaling
mjtNum gtol = m->opt.tolerance * LStolscl * snorm * m->stat.meaninertia * mjMAX(1, m->nv);
mjtNum gtol = m->opt.tolerance * m->opt.ls_tolerance * snorm * m->stat.meaninertia * mjMAX(1, m->nv);
mjtNum slopescl = 1 / (snorm * m->stat.meaninertia * mjMAX(1, m->nv));
// compute Mv, Jv
@@ -1179,7 +1177,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
// one-sided search
int p2update = 0;
while (p1.deriv[0]*dir <= -gtol && ctx->LSiter < LSmaxiter) {
while (p1.deriv[0]*dir <= -gtol && ctx->LSiter < m->opt.ls_iterations) {
// save current
p2 = p1;
p2update = 1;
@@ -1196,7 +1194,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
}
// check for failure to bracket
if (ctx->LSiter >= LSmaxiter) {
if (ctx->LSiter >= m->opt.ls_iterations) {
ctx->LSresult = 3; // could not bracket
ctx->LSslope = mju_abs(p1.deriv[0])*slopescl;
return p1.alpha;
@@ -1215,7 +1213,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) {
CGeval(m, d, ctx, &p1next);
// bracketed search
while (ctx->LSiter < LSmaxiter) {
while (ctx->LSiter < m->opt.ls_iterations) {
// evaluate at midpoint
pmid.alpha = 0.5*(p1.alpha + p2.alpha);
CGeval(m, d, ctx, &pmid);
@@ -1276,7 +1274,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);
@@ -1313,7 +1311,7 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) {
for (int r=0; r < dim; r++) {
// copy data for this row
mju_copy(LTJ_row, LTJ+r*nnz, nnz);
memcpy(LTJ_ind, d->efc_J_colind+d->efc_J_rowadr[i+r], nnz*sizeof(int));
mju_copyInt(LTJ_ind, d->efc_J_colind+d->efc_J_rowadr[i+r], nnz);
// update
mju_cholUpdateSparse(ctx->Hcone, LTJ_row, nv, 1,
@@ -1346,7 +1344,7 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) {
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -1354,7 +1352,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);
@@ -1426,7 +1424,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) {
@@ -1443,7 +1441,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);
@@ -1475,7 +1473,7 @@ static void HessianIncremental(const mjModel* m, mjData* d,
// scale vec, copy colind
mju_scl(vec, d->efc_J+adr, mju_sqrt(d->efc_D[i]), nnz);
memcpy(vec_ind, d->efc_J_colind+adr, nnz*sizeof(int));
mju_copyInt(vec_ind, d->efc_J_colind+adr, nnz);
// sparse update
rank = mju_cholUpdateSparse(ctx->H, vec, nv, flag_update,
@@ -1489,7 +1487,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
@@ -1503,7 +1501,7 @@ static void HessianIncremental(const mjModel* m, mjData* d,
HessianCone(m, d, ctx);
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -1514,7 +1512,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);
@@ -1562,7 +1560,7 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
mju_copy(gradold, ctx.grad, nv);
mju_copy(Mgradold, ctx.Mgrad, nv);
}
memcpy(oldstate, d->efc_state, nefc*sizeof(int));
mju_copyInt(oldstate, d->efc_state, nefc);
mjtNum oldcost = ctx.cost;
// update
@@ -1624,7 +1622,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
+103 -41
View File
@@ -14,12 +14,13 @@
#include "engine/engine_support.h"
#include <stddef.h>
#include <stdint.h>
#include <string.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include "engine/engine_array_safety.h"
#include "engine/engine_core_constraint.h"
#include "engine/engine_crossplatform.h"
#include "engine/engine_io.h"
@@ -278,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
@@ -299,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);
}
@@ -324,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);
@@ -338,7 +339,7 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -827,25 +828,27 @@ void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M) {
// multiply vector by inertia matrix
void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) {
int adr, nv = m->nv;
int nv = m->nv;
const mjtNum* M = d->qM;
const int* dofMadr = m->dof_Madr;
const int* Madr = m->dof_Madr;
const int* parentid = m->dof_parentid;
const int* simplenum = m->dof_simplenum;
mju_zero(res, nv);
for (int i=0; i < nv; i++) {
#ifdef mjUSEAVX
// simple: diagonal division, AVX
if (m->dof_simplenum[i] >= 4) {
// simple: diagonal multiplication, AVX
if (simplenum[i] >= 4) {
// init
__m256d result, val1, val2;
// parallel computation
val1 = _mm256_loadu_pd(vec+i);
val2 = _mm256_set_pd(M[dofMadr[i+3]],
M[dofMadr[i+2]],
M[dofMadr[i+1]],
M[dofMadr[i+0]]);
val2 = _mm256_set_pd(M[Madr[i+3]],
M[Madr[i+2]],
M[Madr[i+1]],
M[Madr[i+0]]);
result = _mm256_mul_pd(val1, val2);
// store result
@@ -856,29 +859,88 @@ void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec)
continue;
}
#endif
// address in M
int adr = Madr[i];
// simple: diagonal multiplication
if (m->dof_simplenum[i]) {
res[i] = M[dofMadr[i]]*vec[i];
// compute diagonal
res[i] = M[adr]*vec[i];
// simple dof: continue
if (simplenum[i]) {
continue;
}
// regular: full multiplication
else {
// diagonal
adr = dofMadr[i];
res[i] += M[adr]*vec[i];
// off-diagonal
int j = m->dof_parentid[i];
// compute off-diagonals
int j = parentid[i];
while (j >= 0) {
adr++;
while (j >= 0) {
res[i] += M[adr]*vec[j];
res[j] += M[adr]*vec[i];
res[i] += M[adr]*vec[j];
res[j] += M[adr]*vec[i];
// advance to next element
j = m->dof_parentid[j];
adr++;
// advance to parent
j = parentid[j];
}
}
}
// multiply vector by inertia matrix for one dof island
void mj_mulM_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec,
int island, int flg_vecunc) {
// if no island, call regular function
if (island < 0) {
mj_mulM(m, d, res, vec);
return;
}
// local constants: general
const mjtNum* M = d->qM;
const int* Madr = m->dof_Madr;
const int* parentid = m->dof_parentid;
const int* simplenum = m->dof_simplenum;
// local constants: island specific
int ndof = d->island_dofnum[island];
const int* dofind = d->island_dofind + d->island_dofadr[island];
const int* islandind = d->dof_islandind;
mju_zero(res, ndof);
for (int k=0; k < ndof; k++) {
// address in full dof vector
int i = dofind[k];
// address in M
int adr = Madr[i];
// diagonal
if (flg_vecunc) {
res[k] = M[adr]*vec[i];
} else {
res[k] = M[adr]*vec[k];
}
// simple dof: continue
if (simplenum[i]) {
continue;
}
// off-diagonal
int j = parentid[i];
while (j >= 0) {
adr++;
int l = islandind[j];
if (flg_vecunc) {
res[k] += M[adr]*vec[j];
res[l] += M[adr]*vec[i];
} else {
res[k] += M[adr]*vec[l];
res[l] += M[adr]*vec[k];
}
// advance to parent
j = parentid[j];
}
}
}
@@ -952,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
@@ -962,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
@@ -997,7 +1059,7 @@ void mj_makeMSparse(const mjModel* m, mjData* d, mjtNum* M,
}
// backward pass over dofs: construct M_row(i) in reverse order
int col = M_rowadr[i]; // current column in row i
int col = M_rowadr[i]; // current column in row i
for (int j = i; j >= 0; j = m->dof_parentid[j]) {
M[col] = d->qM[Madr++];
M_colind[col++] = j;
@@ -1049,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);
@@ -1059,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);
}
@@ -1096,11 +1158,11 @@ 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);
memcpy(remaining, d->D_rownnz, nv * sizeof(int));
mju_copyInt(remaining, d->D_rownnz, nv);
// copy data
for (int i = nv - 1; i >= 0; i--) {
@@ -1123,7 +1185,7 @@ void mj_copyM2DSparse(const mjModel* m, mjData* d, mjtNum* dst, const mjtNum* sr
}
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -1161,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);
@@ -1184,7 +1246,7 @@ void mj_applyFT(const mjModel* m, mjData* d,
mju_addTo(qfrc_target, qforce, nv);
}
mjFREESTACK;
mj_freeStack(d);
}
@@ -1525,7 +1587,7 @@ int mj_version(void) {
// current version of MuJoCo as a null-terminated string
const char* mj_versionString() {
const char* mj_versionString(void) {
static const char versionstring[] = mjVERSIONSTRING;
return versionstring;
}
src/engine/engine_support.h
+8 -1
View File
@@ -15,9 +15,12 @@
#ifndef MUJOCO_SRC_ENGINE_ENGINE_SUPPORT_H_
#define MUJOCO_SRC_ENGINE_ENGINE_SUPPORT_H_
#include <stdint.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#ifdef __cplusplus
extern "C" {
@@ -108,6 +111,10 @@ MJAPI void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M);
// multiply vector by inertia matrix
MJAPI void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
// multiply vector by inertia matrix for one dof island
MJAPI void mj_mulM_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec,
int island, int flg_vecunc);
// multiply vector by (inertia matrix)^(1/2)
MJAPI void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
@@ -193,7 +200,7 @@ MJAPI void mj_warning(mjData* d, int warning, int info);
MJAPI int mj_version(void);
// current version of MuJoCo as a null-terminated string
MJAPI const char* mj_versionString();
MJAPI const char* mj_versionString(void);
#ifdef __cplusplus
}
#endif
src/engine/engine_util_blas.c
+2 -7
View File
@@ -16,7 +16,6 @@
#include <string.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mjtnum.h>
#ifdef mjUSEPLATFORMSIMD
@@ -238,9 +237,7 @@ mjtNum mju_normalize4(mjtNum vec[4]) {
// res = 0
void mju_zero(mjtNum* res, int n) {
if (n > 0) {
memset(res, 0, n*sizeof(mjtNum));
}
memset(res, 0, n*sizeof(mjtNum));
}
@@ -256,9 +253,7 @@ void mju_fill(mjtNum* res, mjtNum val, int n) {
// res = vec
void mju_copy(mjtNum* res, const mjtNum* vec, int n) {
if (n > 0) {
memcpy(res, vec, n*sizeof(mjtNum));
}
memcpy(res, vec, n*sizeof(mjtNum));
}
src/engine/engine_util_errmem.c
+18 -11
View File
@@ -70,7 +70,7 @@ typedef void (*callback_fn)(const char*);
static mjTHREADLOCAL callback_fn _mjPRIVATE_tls_error_fn = NULL;
static mjTHREADLOCAL callback_fn _mjPRIVATE_tls_warning_fn = NULL;
callback_fn _mjPRIVATE__get_tls_error_fn() {
callback_fn _mjPRIVATE__get_tls_error_fn(void) {
return _mjPRIVATE_tls_error_fn;
}
@@ -78,7 +78,7 @@ void _mjPRIVATE__set_tls_error_fn(callback_fn h) {
_mjPRIVATE_tls_error_fn = h;
}
callback_fn _mjPRIVATE__get_tls_warning_fn() {
callback_fn _mjPRIVATE__get_tls_warning_fn(void) {
return _mjPRIVATE_tls_warning_fn;
}
@@ -113,20 +113,17 @@ void mju_writeLog(const char* type, const char* msg) {
}
}
void mju_error_v(const char* msg, va_list args) {
char errmsg[1000];
// Format msg into errmsg
vsnprintf(errmsg, mjSIZEOFARRAY(errmsg), msg, args);
void mju_error_raw(const char* msg) {
if (_mjPRIVATE_tls_error_fn) {
_mjPRIVATE_tls_error_fn(errmsg);
_mjPRIVATE_tls_error_fn(msg);
} else if (mju_user_error) {
mju_user_error(errmsg);
mju_user_error(msg);
} else {
// write to log and console
mju_writeLog("ERROR", errmsg);
printf("ERROR: %s\n\nPress Enter to exit ...", errmsg);
mju_writeLog("ERROR", msg);
printf("ERROR: %s\n\nPress Enter to exit ...", msg);
// pause, exit
getchar();
@@ -135,6 +132,16 @@ void mju_error_v(const char* msg, va_list args) {
}
void mju_error_v(const char* msg, va_list args) {
// Format msg into errmsg
char errmsg[1024];
vsnprintf(errmsg, mjSIZEOFARRAY(errmsg), msg, args);
mju_error_raw(errmsg);
}
// write message to logfile and console, pause and exit
void mju_error(const char* msg, ...) {
va_list args;
@@ -147,7 +154,7 @@ void mju_error(const char* msg, ...) {
// write message to logfile and console
void mju_warning(const char* msg, ...) {
char wrnmsg[1000];
char wrnmsg[1024];
// Format msg into wrnmsg
va_list args;
src/engine/engine_util_errmem.h
+13 -38
View File
@@ -18,6 +18,7 @@
#include <stdarg.h>
#include <stddef.h>
#include <stdio.h>
#include <string.h>
#include <mujoco/mjexport.h>
#include <mujoco/mjmacro.h>
@@ -31,8 +32,8 @@ extern "C" {
#define mjPRINTFLIKE(n, m) __attribute__((format(printf, n, m)))
#else
#define mjPRINTFLIKE(n, m)
#endif // __GNUC__
#endif // mjPRINTFLIKE
#endif // __GNUC__
#endif // mjPRINTFLIKE
//------------------------------ user handlers -----------------------------------------------------
@@ -54,6 +55,7 @@ MJAPI void _mjPRIVATE__set_tls_warning_fn(void (*h)(const char*));
//------------------------------ errors and warnings -----------------------------------------------
// errors
MJAPI void mju_error_raw(const char* msg);
MJAPI void mju_error(const char* msg, ...) mjPRINTFLIKE(1, 2);
MJAPI void mju_error_v(const char* msg, va_list args);
MJAPI void mju_error_i(const char* msg, int i);
@@ -69,42 +71,15 @@ MJAPI void mju_writeLog(const char* type, const char* msg);
//------------------------------ internal error macros --------------------------------------------
// need at least c99 or c++11
#if (defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L) || \
(defined(__cplusplus) && __cplusplus >= 201103L)
// macro to get the first argument
#define _GET_MSG(msg, ...) msg
// helper function for the mjERROR macro
// formats buf as '{prefix}: {msg}' and passes along to mju_error_v
static inline void _mju_error_prefix(char *buf, size_t nbuf, const char* prefix,
const char* msg, ...) mjPRINTFLIKE(4, 5);
static inline void _mju_error_prefix(char *buf, size_t nbuf, const char* prefix,
const char* msg, ...) {
snprintf(buf, nbuf, "%s: %s", prefix, msg);
va_list args;
va_start(args, msg);
mju_error_v(buf, args);
va_end(args);
}
// macro to get first argument
#define _GET_MSG(msg, ...) msg
// internal macro to prepend the calling function name to the error message
// standard support for variadic macros with zero arguments is only now
// supported in C23 and C++20 so we rely on a helper function to get around this
// in a portable way
#define mjERROR(...) { \
char _buf[sizeof(_GET_MSG(__VA_ARGS__)) + sizeof(__func__) + 1]; \
_mju_error_prefix(_buf, sizeof(_buf), __func__, __VA_ARGS__); \
}
#else
#define mjERROR mju_error
#endif // c99 or c++11
// internal macro to prepend the calling function name to the error message
#define mjERROR(...) \
{ \
char _errbuf[1024]; \
size_t _funclen = strlen(__func__); \
strncpy(_errbuf, __func__, sizeof(_errbuf)); \
snprintf(_errbuf + _funclen, sizeof(_errbuf) - _funclen, ": " __VA_ARGS__); \
mju_error_raw(_errbuf); \
}
//------------------------------ malloc and free ---------------------------------------------------
src/engine/engine_util_misc.c
+14
View File
@@ -1207,6 +1207,20 @@ int mju_isZero(mjtNum* vec, int n) {
// set integer vector to 0
void mju_zeroInt(int* res, int n) {
memset(res, 0, n*sizeof(int));
}
// copy int vector vec into res
void mju_copyInt(int* res, const int* vec, int n) {
memcpy(res, vec, n*sizeof(int));
}
// standard normal random number generator (optional second number)
mjtNum mju_standardNormal(mjtNum* num2) {
const mjtNum scale = 2.0/((mjtNum)RAND_MAX);
src/engine/engine_util_misc.h
+6
View File
@@ -119,6 +119,12 @@ MJAPI int mju_isBad(mjtNum x);
// return 1 if all elements are 0
MJAPI int mju_isZero(mjtNum* vec, int n);
// set integer vector to 0
MJAPI void mju_zeroInt(int* res, int n);
// copy int vector vec into res
MJAPI void mju_copyInt(int* res, const int* vec, int n);
// standard normal random number generator (optional second number)
MJAPI mjtNum mju_standardNormal(mjtNum* num2);
src/engine/engine_util_solve.c
+6 -7
View File
@@ -16,7 +16,6 @@
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjmacro.h>
@@ -148,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);
@@ -199,7 +198,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag,
}
}
mjFREESTACK;
mj_freeStack(d);
return rank;
}
@@ -235,7 +234,7 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int
// x(i) -= sum_j L(i,j)*x(j), j=0:i-1
if (nnz > 1) {
res[i] -= mju_dotSparse(mat+adr, res, nnz-1, colind+adr);
res[i] -= mju_dotSparse(mat+adr, res, nnz-1, colind+adr, /*flg_unc1=*/0);
// modulo AVX, the above line does
// for (int j=0; j<nnz-1; j++)
// res[i] -= mat[adr+j]*res[colind[adr+j]];
@@ -254,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);
@@ -295,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;
}
@@ -592,7 +591,7 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
int* remaining = scratch;
// set remaining = rownnz
memcpy(remaining, rownnz, n*sizeof(int));
mju_copyInt(remaining, rownnz, n);
// diagonal elements (i,i)
for (int i=n-1; i >= 0; i--) {
src/engine/engine_util_sparse.c
+45 -23
View File
@@ -13,24 +13,25 @@
// 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"
#include "engine/engine_util_misc.h"
//------------------------------ sparse operations -------------------------------------------------
// dot-product, first vector is sparse
// flg_unc1: is vec1 memory layout uncompressed
mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2,
const int nnz1, const int* ind1) {
const int nnz1, const int* ind1, int flg_unc1) {
#ifdef mjUSEAVX
return mju_dotSparse_avx(vec1, vec2, nnz1, ind1);
return mju_dotSparse_avx(vec1, vec2, nnz1, ind1, flg_unc1);
#else
int i = 0;
mjtNum res = 0;
@@ -40,17 +41,33 @@ mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2,
mjtNum res2 = 0;
mjtNum res3 = 0;
for (; i <= n_4; i+=4) {
res0 += vec1[i+0] * vec2[ind1[i+0]];
res1 += vec1[i+1] * vec2[ind1[i+1]];
res2 += vec1[i+2] * vec2[ind1[i+2]];
res3 += vec1[i+3] * vec2[ind1[i+3]];
if (flg_unc1) {
for (; i <= n_4; i+=4) {
res0 += vec1[ind1[i+0]] * vec2[ind1[i+0]];
res1 += vec1[ind1[i+1]] * vec2[ind1[i+1]];
res2 += vec1[ind1[i+2]] * vec2[ind1[i+2]];
res3 += vec1[ind1[i+3]] * vec2[ind1[i+3]];
}
} else {
for (; i <= n_4; i+=4) {
res0 += vec1[i+0] * vec2[ind1[i+0]];
res1 += vec1[i+1] * vec2[ind1[i+1]];
res2 += vec1[i+2] * vec2[ind1[i+2]];
res3 += vec1[i+3] * vec2[ind1[i+3]];
}
}
res = (res0 + res2) + (res1 + res3);
// scalar part
for (; i < nnz1; i++) {
res += vec1[i] * vec2[ind1[i]];
if (flg_unc1) {
for (; i < nnz1; i++) {
res += vec1[ind1[i]] * vec2[ind1[i]];
}
} else {
for (; i < nnz1; i++) {
res += vec1[i] * vec2[ind1[i]];
}
}
return res;
@@ -91,9 +108,10 @@ void mju_dotSparseX3(mjtNum* res0, mjtNum* res1, mjtNum* res2,
// dot-product, both vectors are sparse
// flg_unc2: is vec2 memory layout uncompressed
mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2,
const int nnz1, const int* ind1,
const int nnz2, const int* ind2) {
const int nnz2, const int* ind2, int flg_unc2) {
int i1 = 0, i2 = 0;
mjtNum res = 0;
@@ -108,7 +126,12 @@ mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2,
// match: accumulate result, advance both
if (adr1 == adr2) {
res += vec1[i1++] * vec2[i2++];
if (flg_unc2) {
res += vec1[i1++] * vec2[adr2];
i2++;
} else {
res += vec1[i1++] * vec2[i2++];
}
}
// otherwise advance smaller
@@ -176,7 +199,7 @@ void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
#else
// regular sparse dot-product
for (int r=0; r < nr; r++) {
res[r] = mju_dotSparse(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
res[r] = mju_dotSparse(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r], /*flg_unc1=*/0);
}
#endif // mjUSEAVX
}
@@ -249,8 +272,8 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b,
// copy dst into buf
if (dst_nnz) {
memcpy(buf, dst, dst_nnz*sizeof(mjtNum));
memcpy(buf_ind, dst_ind, dst_nnz*sizeof(int));
mju_copy(buf, dst, dst_nnz);
mju_copyInt(buf_ind, dst_ind, dst_nnz);
}
// prepare to merge buf and src into dst
@@ -370,7 +393,7 @@ void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc,
int* res_rownnz, int* res_rowadr, int* res_colind,
const int* rownnz, const int* rowadr, const int* colind) {
// clear number of non-zeros for each row of transposed
memset(res_rownnz, 0, nc*sizeof(int));
mju_zeroInt(res_rownnz, nc);
// total number of non-zeros of mat
int nnz = rowadr[nr-1] + rownnz[nr-1];
@@ -450,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];
@@ -532,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);
}
@@ -556,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);
@@ -575,7 +597,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
// if rowsuper, use the previous row sparsity structure
if (rowsuperT && i > 0 && rowsuperT[i-1]) {
res_rownnz[i] = res_rownnz[i-1];
memcpy(cols, res_colind+res_rowadr[i-1], res_rownnz[i]*sizeof(int));
mju_copyInt(cols, res_colind+res_rowadr[i-1], res_rownnz[i]);
}
// iterate through each row of M'
@@ -665,5 +687,5 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
}
}
mjFREESTACK;
mj_freeStack(d);
}
src/engine/engine_util_sparse.h
+6 -6
View File
@@ -25,14 +25,14 @@ extern "C" {
//------------------------------ sparse operations -------------------------------------------------
// dot-product, first vector is sparse
// dot-product, vec1 is sparse, can be uncompressed
MJAPI mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2,
const int nnz1, const int* ind1);
const int nnz1, const int* ind1, int flg_unc1);
// dot-product, both vectors are sparse
mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2,
const int nnz1, const int* ind1,
const int nnz2, const int* ind2);
// dot-product, both vectors are sparse, vec2 can be uncompressed
MJAPI mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2,
const int nnz1, const int* ind1,
const int nnz2, const int* ind2, int flg_unc2);
// convert matrix from dense to sparse
MJAPI void mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc,
src/engine/engine_util_sparse_avx.h
+46 -16
View File
@@ -30,9 +30,10 @@
//------------------------------ sparse operations using avx ---------------------------------------
// dot-product, first vector is sparse
// flg_unc1: is vec1 memory layout uncompressed
static inline
mjtNum mju_dotSparse_avx(const mjtNum* vec1, const mjtNum* vec2,
const int nnz1, const int* ind1) {
const int nnz1, const int* ind1, int flg_unc1) {
int i = 0;
mjtNum res = 0;
int nnz1_4 = nnz1 - 4;
@@ -47,20 +48,43 @@ mjtNum mju_dotSparse_avx(const mjtNum* vec1, const mjtNum* vec2,
vec2[ind1[2]],
vec2[ind1[1]],
vec2[ind1[0]]);
val1 = _mm256_loadu_pd(vec1);
if (flg_unc1) {
val1 = _mm256_set_pd(vec1[ind1[3]],
vec1[ind1[2]],
vec1[ind1[1]],
vec1[ind1[0]]);
} else {
val1 = _mm256_loadu_pd(vec1);
}
sum = _mm256_mul_pd(val1, val2);
i = 4;
// parallel computation
while (i<=nnz1_4) {
val1 = _mm256_loadu_pd(vec1+i);
val2 = _mm256_set_pd(vec2[ind1[i+3]],
vec2[ind1[i+2]],
vec2[ind1[i+1]],
vec2[ind1[i+0]]);
prod = _mm256_mul_pd(val1, val2);
sum = _mm256_add_pd(sum, prod);
i += 4;
if (flg_unc1) {
while (i<=nnz1_4) {
val1 = _mm256_set_pd(vec1[ind1[i+3]],
vec1[ind1[i+2]],
vec1[ind1[i+1]],
vec1[ind1[i+0]]);
val2 = _mm256_set_pd(vec2[ind1[i+3]],
vec2[ind1[i+2]],
vec2[ind1[i+1]],
vec2[ind1[i+0]]);
prod = _mm256_mul_pd(val1, val2);
sum = _mm256_add_pd(sum, prod);
i += 4;
}
} else {
while (i<=nnz1_4) {
val1 = _mm256_loadu_pd(vec1+i);
val2 = _mm256_set_pd(vec2[ind1[i+3]],
vec2[ind1[i+2]],
vec2[ind1[i+1]],
vec2[ind1[i+0]]);
prod = _mm256_mul_pd(val1, val2);
sum = _mm256_add_pd(sum, prod);
i += 4;
}
}
// reduce
@@ -72,8 +96,14 @@ mjtNum mju_dotSparse_avx(const mjtNum* vec1, const mjtNum* vec2,
}
// scalar part
for (; i<nnz1; i++) {
res += vec1[i] * vec2[ind1[i]];
if (flg_unc1) {
for (; i < nnz1; i++) {
res += vec1[ind1[i]] * vec2[ind1[i]];
}
} else {
for (; i < nnz1; i++) {
res += vec1[i] * vec2[ind1[i]];
}
}
return res;
@@ -179,7 +209,7 @@ void mju_mulMatVecSparse_avx(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
if (!rowsuper) {
// regular sparse dot-product
for (int r=0; r<nr; r++) {
res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r], /*flg_unc2=*/0);
}
return;
@@ -202,7 +232,7 @@ void mju_mulMatVecSparse_avx(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
// handle remaining rows
while (rs>0) {
res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r], /*flg_unc2=*/0);
r++;
rs--;
@@ -213,7 +243,7 @@ void mju_mulMatVecSparse_avx(mjtNum* res, const mjtNum* mat, const mjtNum* vec,
}
else {
res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r]);
res[r] = mju_dotSparse_avx(mat+rowadr[r], vec, rownnz[r], colind+rowadr[r], /*flg_unc2=*/0);
}
}
}
src/engine/engine_vfs.c
+50 -16
View File
@@ -14,10 +14,10 @@
#include "engine/engine_vfs.h"
#include <stddef.h>
#include <string.h>
#include "engine/engine_array_safety.h"
#include "engine/engine_plugin.h"
#include "engine/engine_resource.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
@@ -90,7 +90,7 @@ int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename) {
mjSTRNCPY(vfs->filename[vfs->nfile], newname);
// allocate and read
int filesize = 0;
size_t filesize = 0;
vfs->filedata[vfs->nfile] = mju_fileToMemory(fullname, &filesize);
if (!vfs->filedata[vfs->nfile]) {
return -1;
@@ -211,11 +211,11 @@ void mj_deleteVFS(mjVFS* vfs) {
// open callback for the VFS resource provider
static int vfs_open_callback(mjResource* resource) {
if (!resource || !resource->provider_data || !resource->name) {
if (!resource || !resource->name || !resource->data) {
return 0;
}
const mjVFS* vfs = (const mjVFS*) resource->provider_data;
const mjVFS* vfs = (const mjVFS*) resource->data;
return mj_findFileVFS(vfs, resource->name) >= 0;
}
@@ -223,12 +223,12 @@ static int vfs_open_callback(mjResource* resource) {
// read callback for the VFS resource provider
static int vfs_read_callback(mjResource* resource, const void** buffer) {
if (!resource || !resource->provider_data) {
if (!resource || !resource->name || !resource->data) {
*buffer = NULL;
return -1;
}
const mjVFS* vfs = (const mjVFS*) resource->provider_data;
const mjVFS* vfs = (const mjVFS*) resource->data;
int i = mj_findFileVFS(vfs, resource->name);
if (i < 0) {
*buffer = NULL;
@@ -260,16 +260,50 @@ static void vfs_getdir_callback(mjResource* resource, const char** dir, int* ndi
// registers a VFS resource provider; returns the index of the provider
int mj_registerVfsProvider(const mjVFS* vfs) {
mjpResourceProvider provider = {
.prefix = mjVFS_PREFIX,
.open = &vfs_open_callback,
.read = &vfs_read_callback,
.close = &vfs_close_callback,
.getdir = &vfs_getdir_callback,
.data = (void*) vfs
// open VFS resource
mjResource* mju_openVfsResource(const char* name, const mjVFS* vfs) {
if (vfs == NULL) {
return NULL;
}
// VFS provider
static struct mjpResourceProvider provider = {
.prefix = NULL,
.data = NULL,
.open = &vfs_open_callback,
.read = &vfs_read_callback,
.close = &vfs_close_callback,
.getdir = &vfs_getdir_callback,
.modified = NULL
};
return mjp_registerResourceProviderInternal(&provider);
// create resource
mjResource* resource = (mjResource*) mju_malloc(sizeof(mjResource));
if (resource == NULL) {
mjERROR("could not allocate memory");
return NULL;
}
// clear out resource
memset(resource, 0, sizeof(mjResource));
// copy name
resource->name = mju_malloc(sizeof(char) * (strlen(name) + 1));
if (resource->name == NULL) {
mju_closeResource(resource);
mjERROR("could not allocate memory");
return NULL;
}
memcpy(resource->name, name, sizeof(char) * (strlen(name) + 1));
resource->data = (void*) vfs;
// open resource
resource->provider = &provider;
if (provider.open(resource)) {
return resource;
}
// not found in VFS
mju_closeResource(resource);
return NULL;
}
src/engine/engine_vfs.h
+3 -2
View File
@@ -17,6 +17,7 @@
#include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#ifdef __cplusplus
extern "C" {
@@ -40,8 +41,8 @@ MJAPI int mj_deleteFileVFS(mjVFS* vfs, const char* filename);
// delete all files from VFS
MJAPI void mj_deleteVFS(mjVFS* vfs);
// registers a VFS resource provider; returns the index of the provider
MJAPI int mj_registerVfsProvider(const mjVFS* vfs);
// open VFS resource
MJAPI mjResource* mju_openVfsResource(const char* name, const mjVFS* vfs);
#ifdef __cplusplus
}
src/engine/engine_vis_interact.c
+3 -3
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,6 +528,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur
// invalid selected body: return
if (sel <= 0 || sel >= m->nbody) {
mj_freeStack(d);
return;
}
@@ -557,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);
}
src/engine/engine_vis_state.c
+3 -3
View File
@@ -263,9 +263,9 @@ void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt,
mjvSceneState* scnstate) {
// Check that mjModel sizes haven't changed.
#define X(var)
#define XMJV(var) \
if (scnstate->model.var != m->var) { \
mjERROR("m->%s changed", #var); \
#define XMJV(var) \
if (scnstate->model.var != m->var) { \
mjERROR("m->%s changed: %d vs %d", #var, scnstate->model.var, m->var); \
}
MJMODEL_INTS
#undef XMJV
src/render/glad/glad.c
+13 -4
View File
@@ -36,6 +36,11 @@
// Online:
// https://glad.dav1d.de/#profile=compatibility&language=c&specification=gl&loader=on&api=gl%3D1.5&extensions=GL_ARB_framebuffer_object&extensions=GL_ARB_seamless_cube_map&extensions=GL_ARB_vertex_buffer_object&extensions=GL_KHR_debug
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#endif
#if !defined(_WIN32) && !defined(__CYGWIN__) && !defined(__APPLE__) && \
!defined(__HAIKU__) && !defined(_GNU_SOURCE)
#define _GNU_SOURCE
@@ -1471,12 +1476,12 @@ static void mjGlad_find_coreGL(void) {
}
}
int mjGladLoadGLUnsafe() {
if(mjGlad_open_gl()) {
int mjGladLoadGLUnsafe(void) {
if (mjGlad_open_gl()) {
mjGLVersion.major = 0; mjGLVersion.minor = 0;
glGetString = (PFNGLGETSTRINGPROC)mjGlad_get_proc("glGetString");
if(glGetString == NULL) return 0;
if(glGetString(GL_VERSION) == NULL) return 0;
if (glGetString == NULL) return 0;
if (glGetString(GL_VERSION) == NULL) return 0;
mjGlad_find_coreGL();
mjGlad_load_GL_VERSION_1_0(mjGlad_get_proc);
mjGlad_load_GL_VERSION_1_1(mjGlad_get_proc);
@@ -1496,3 +1501,7 @@ int mjGladLoadGLUnsafe() {
return 0;
}
}
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif
src/render/render_context.c
+2 -2
View File
@@ -140,7 +140,7 @@ static void makePlane(const mjModel* m, mjrContext* con) {
// record
grid[k][x] = left;
grid[k][x+1] = mjMAX(left, right); // just in case
grid[k][x+1] = mjMAX(left, right); // just in case
}
}
@@ -1458,7 +1458,7 @@ void GLAPIENTRY debugCallback(GLenum source,
// returns 1 if MUJOCO_GL_DEBUG environment variable is set to 1
static int glDebugEnabled() {
static int glDebugEnabled(void) {
char* debug = getenv("MUJOCO_GL_DEBUG");
return debug && strcmp(debug, "1") == 0;
}
src/thread/CMakeLists.txt
+23
View File
@@ -0,0 +1,23 @@
# Copyright 2023 DeepMind Technologies Limited
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
set(MUJOCO_THREAD_SRCS
lockless_queue.h
task.cc
task.h
thread_pool.cc
thread_pool.h
)
target_sources(mujoco PRIVATE ${MUJOCO_THREAD_SRCS})
src/thread/lockless_queue.h
+154
View File
@@ -0,0 +1,154 @@
// Copyright 2023 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// IWYU pragma: private, include "third_party/mujoco/include/mujoco.h"
// IWYU pragma: friend "third_party/(py/)?mujoco/.*"
#ifndef MUJOCO_SRC_THREAD_LOCKLESS_QUEUE_H_
#define MUJOCO_SRC_THREAD_LOCKLESS_QUEUE_H_
#include <atomic>
#include <climits>
#include <cstddef>
#include <thread>
namespace mujoco {
// A Lockless Queue allows for sending information quickly between different
// threads. This is a Multi-Producer Multi-Consumer Lockless Queue allowing for
// multiple threads to be adding items to the queue while multiple threads are
// consuming items from the queue. Internally it uses a Ring Buffer for storage
// so it will not grow as items are added. Push will block if the Queue is full
// and Pop will block if it is empty.
//
// For a basic overview of this category of structures:
// https://www.linuxjournal.com/content/lock-free-multi-producer-multi-consumer-queue-ring-buffer
template <typename T, size_t buffer_capacity>
class LocklessQueue {
public:
bool full() const {
return full_internal(
convert_to_index(read_cursor_), convert_to_index(write_cursor_));
}
bool empty() const {
return maximum_read_cursor_ == read_cursor_;
}
// Push an element into the queue.
void push(const T& input) {
// Reserve a slot in the queue
size_t current_write_cursor;
size_t dummy_current_write_cursor;
size_t next_write_cursor;
size_t current_write_index;
size_t current_read_index;
do {
// Check if the queue is full.
do {
current_write_cursor = write_cursor_.load();
current_write_index = convert_to_index(current_write_cursor);
next_write_cursor = get_next_cursor(current_write_cursor);
current_read_index = convert_to_index(read_cursor_.load());
} while (full_internal(current_read_index, current_write_index));
// Once it's not full, attempt to grab a slot to write.
dummy_current_write_cursor = current_write_cursor;
} while (!write_cursor_.compare_exchange_weak(
dummy_current_write_cursor, next_write_cursor));
// Write the entry.
buffer_[current_write_index].store(input);
// Increment maximum read cursor. Note here it has to wait if the compare
// and exchange fails as another thread might not have completed its write.
do {
dummy_current_write_cursor = current_write_cursor;
} while (!maximum_read_cursor_.compare_exchange_weak(
dummy_current_write_cursor, next_write_cursor));
}
// Pop an element from the queue.
T pop() {
size_t current_read_cursor;
size_t dummy_current_read_cursor;
size_t current_read_index;
size_t next_read_cursor;
size_t current_maximum_read_cursor;
size_t current_maximum_read_index;
bool empty = false;
T result;
do {
// Wait until the queue has an element
do {
if (empty) {
std::this_thread::yield();
}
current_read_cursor = read_cursor_.load();
current_maximum_read_cursor = maximum_read_cursor_.load();
current_read_index = convert_to_index(current_read_cursor);
current_maximum_read_index = convert_to_index(
current_maximum_read_cursor);
empty = empty_internal(
current_read_index, current_maximum_read_index);
} while (empty);
next_read_cursor = get_next_cursor(current_read_cursor);
// Attempt to grab the element, if unsuccessful then wait for the next
// element to arrive.
result = buffer_[current_read_index].load();
dummy_current_read_cursor = current_read_cursor;
} while (!read_cursor_.compare_exchange_weak(
dummy_current_read_cursor, next_read_cursor));
return result;
}
private:
size_t convert_to_index(size_t input) const {
return input % internal_buffer_capacity_;
}
size_t get_next_cursor(size_t input) const {
return (input + 1) % cursor_max_;
}
size_t get_next_index(size_t input) const {
return convert_to_index(get_next_cursor(input));
}
bool full_internal(size_t read_index, size_t write_index) const {
return get_next_index(write_index) == read_index;
}
bool empty_internal(size_t read_index, size_t write_index) const {
return read_index == write_index;
}
const size_t internal_buffer_capacity_ = buffer_capacity + 1;
const size_t cursor_max_ = UINT_MAX - (UINT_MAX % internal_buffer_capacity_);
std::atomic<size_t> read_cursor_ = 0;
std::atomic<size_t> write_cursor_ = 0;
std::atomic<size_t> maximum_read_cursor_ = 0;
std::atomic<T> buffer_[(buffer_capacity + 1)];
};
} // namespace mujoco
#endif // MUJOCO_SRC_THREAD_LOCKLESS_QUEUE_H_
src/thread/task.cc
+24
View File
@@ -0,0 +1,24 @@
// Copyright 2023 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "thread/task.h"
#include <mujoco/mjthread.h>
#include <mujoco/mujoco.h>
// waits for a task to complete
void mju_taskJoin(mjTask* task) {
mujoco::Task* task_ptr = static_cast<mujoco::Task*>(static_cast<void*>(task));
task_ptr->Join();
}
src/thread/task.h
+70
View File
@@ -0,0 +1,70 @@
// Copyright 2023 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// IWYU pragma: private, include "third_party/mujoco/include/mujoco.h"
// IWYU pragma: friend "third_party/(py/)?mujoco/.*"
#ifndef MUJOCO_SRC_THREAD_TASK_H_
#define MUJOCO_SRC_THREAD_TASK_H_
#ifdef __cplusplus
#include <atomic>
#include <new>
#include <thread>
namespace mujoco {
class Task {
public:
using FunctionPtr = void* (*)(void*);
enum Status {
QUEUED,
COMPLETE,
};
static void Initialize(
Task* task,
FunctionPtr start_routine,
void* args) {
// instantiate a task at the pointer passed in
new(task) Task();
task->start_routine_ = start_routine;
task->args_ = args;
task->status_ = Status::QUEUED;
}
void Execute() {
args_ = start_routine_(args_);
status_ = Status::COMPLETE;
}
void Join() {
while (status_ != Status::COMPLETE) {
std::this_thread::yield();
}
}
private:
FunctionPtr start_routine_;
void* args_;
std::atomic<Status> status_ = Status::QUEUED;
};
} // namespace mujoco
#endif // __cplusplus
#endif // MUJOCO_SRC_THREAD_TASK_H_
src/thread/thread_pool.cc
+52
View File
@@ -0,0 +1,52 @@
// Copyright 2023 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "thread/thread_pool.h"
#include <cstddef>
#include <mujoco/mjthread.h>
#include <mujoco/mujoco.h>
#include "thread/task.h"
static constexpr size_t kMaxThreads = 128;
// create a thread pool
mjThreadPool* mju_threadPoolCreate(size_t number_of_threads) {
mujoco::ThreadPool<kMaxThreads>* thread_pool =
new mujoco::ThreadPool<kMaxThreads>(number_of_threads);
return static_cast<mjThreadPool*>(static_cast<void*>(thread_pool));
}
// start a task in the threadpool
void mju_threadPoolEnqueue(
mjThreadPool* thread_pool, mjTask* task, mjStartRoutine start_routine,
void* args) {
mujoco::ThreadPool<kMaxThreads>* thread_pool_ptr =
static_cast<mujoco::ThreadPool<kMaxThreads>*>(
static_cast<void*>(thread_pool));
thread_pool_ptr->Enqueue(
static_cast<mujoco::Task*>(static_cast<void*>(task)), start_routine,
args);
}
// shutdown the threadpool and free the memory
void mju_threadPoolDestroy(mjThreadPool* thread_pool) {
mujoco::ThreadPool<kMaxThreads>* thread_pool_ptr =
static_cast<mujoco::ThreadPool<kMaxThreads>*>(
static_cast<void*>(thread_pool));
thread_pool_ptr->Shutdown();
delete thread_pool_ptr;
}
src/thread/thread_pool.h
+102
View File
@@ -0,0 +1,102 @@
// Copyright 2023 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// IWYU pragma: private, include "third_party/mujoco/include/mujoco.h"
// IWYU pragma: friend "third_party/(py/)?mujoco/.*"
#ifndef MUJOCO_SRC_THREAD_THREAD_POOL_H_
#define MUJOCO_SRC_THREAD_THREAD_POOL_H_
#ifdef __cplusplus
#include <atomic>
#include <cstddef>
#include <thread>
#include "thread/lockless_queue.h"
#include "thread/task.h"
namespace mujoco {
static constexpr size_t kThreadPoolQueueSize = 640;
template <size_t max_number_of_threads>
class ThreadPool {
public:
ThreadPool(size_t number_of_threads)
: number_of_threads_(number_of_threads) {
for (int i = 0; i < number_of_threads_; ++i) {
threads_[i] = std::thread(ThreadPoolWorker, static_cast<void*>(this));
}
}
// start a task in the threadpool
void Enqueue(
Task* task, Task::FunctionPtr start_routine, void* args) {
Task::Initialize(task, start_routine, args);
lockless_queue_.push(static_cast<void*>(task));
}
// shutdown the threadpool
void Shutdown() {
if (shutdown_) {
return;
}
shutdown_ = true;
Task shutdown_tasks[max_number_of_threads];
for (int i = 0; i < number_of_threads_; ++i) {
Enqueue(&shutdown_tasks[i], ShutdownFunction, nullptr);
}
for (int i = 0; i < number_of_threads_; ++i) {
threads_[i].join();
}
}
~ThreadPool() { Shutdown(); }
private:
// method executed by running threads
static void ThreadPoolWorker(void* arg) {
ThreadPool<max_number_of_threads>* thread_pool =
static_cast<ThreadPool<max_number_of_threads>*>(arg);
while (!thread_pool->shutdown_) {
Task* task = static_cast<Task*>(thread_pool->lockless_queue_.pop());
task->Execute();
}
}
// shutdown function passed to running threads to ensure cleans shutdown
static void* ShutdownFunction(void* args) {
return NULL;
}
// is the thread pool is being shut down
std::atomic<bool> shutdown_ = false;
// actual number of running threads in the threadpool
const size_t number_of_threads_;
// OS threads that are running in this pool
std::thread threads_[max_number_of_threads];
// queue of tasks to execute
LocklessQueue<void*, kThreadPoolQueueSize> lockless_queue_;
};
} // namespace mujoco
#endif // __cplusplus
#endif // MUJOCO_SRC_THREAD_THREAD_POOL_H_
src/user/user_composite.cc
+204 -53
View File
@@ -34,6 +34,7 @@
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_util.h"
#include "xml/xml_util.h"
namespace {
namespace mju = ::mujoco::util;
@@ -235,7 +236,7 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
}
// check spacing
if (type==mjCOMPTYPE_GRID || type==mjCOMPTYPE_PARTICLE) {
if (type==mjCOMPTYPE_GRID || (type==mjCOMPTYPE_PARTICLE && uservert.empty())) {
if (spacing < mju_max(def[0].geom.size[0],
mju_max(def[0].geom.size[1], def[0].geom.size[2]))) {
return comperr(error, "Spacing must be larger than geometry size",
@@ -314,66 +315,178 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
// make particles
bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int error_sz) {
// create bodies and geoms
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
for (int iz=0; iz<count[2]; iz++) {
// create body
char txt[100];
mjCBody* b = body->AddBody(NULL);
mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
b->name = txt;
char txt[100];
std::vector<int> face;
// set body position
b->pos[0] = offset[0] + spacing*(ix - 0.5*count[0]);
b->pos[1] = offset[1] + spacing*(iy - 0.5*count[1]);
b->pos[2] = offset[2] + spacing*(iz - 0.5*count[2]);
// populate vertices and names
if (uservert.empty()) {
if (spacing < mju_max(def[0].geom.size[0],
mju_max(def[0].geom.size[1], def[0].geom.size[2])))
return comperr(error, "Spacing must be larger than geometry size", error_sz);
// add slider joints if none defined
if (!add[mjCOMPKIND_PARTICLE]) {
for (int i=0; i<3; i++) {
mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0], false);
jnt->def = body->def;
jnt->type = mjJNT_SLIDE;
mjuu_setvec(jnt->pos, 0, 0, 0);
mjuu_setvec(jnt->axis, 0, 0, 0);
jnt->axis[i] = 1;
}
}
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
for (int iz=0; iz<count[2]; iz++) {
uservert.push_back(spacing*(ix - 0.5*count[0]));
uservert.push_back(spacing*(iy - 0.5*count[1]));
uservert.push_back(spacing*(iz - 0.5*count[2]));
// add user-specified joints
else {
for (auto defjnt : defjoint[mjCOMPKIND_PARTICLE]) {
mjCJoint* jnt = b->AddJoint(&defjnt, false);
jnt->def = body->def;
}
}
// add geom
mjCGeom* g = b->AddGeom(def);
g->def = body->def;
// add plugin
if (plugin_instance) {
b->is_plugin = true;
b->plugin_name = plugin_name;
b->plugin_instance = plugin_instance;
b->plugin_instance_name = plugin_instance_name;
// propagate attributes
if (plugin_name == "mujoco.elasticity.solid") {
b->plugin_instance->config_attribs["nx"] = std::to_string(count[0]);
b->plugin_instance->config_attribs["ny"] = std::to_string(count[1]);
b->plugin_instance->config_attribs["nz"] = std::to_string(count[2]);
}
mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
username.push_back(std::string(txt));
}
}
}
}
// skin
// create faces
if (userface.empty()) {
if (dim == 3) {
int cube2tets[6][4] = {{0, 3, 1, 7}, {0, 1, 4, 7},
{1, 3, 2, 7}, {1, 2, 6, 7},
{1, 5, 4, 7}, {1, 6, 5, 7}};
for (int ix = 0; ix < count[0]-1; ix++) {
for (int iy = 0; iy < count[1]-1; iy++) {
for (int iz = 0; iz < count[2]-1; iz++) {
int vert[8] = {
count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+0,
count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+0,
count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+0,
count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+0,
count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+1,
count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+1,
count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+1,
count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+1,
};
for (int s = 0; s < 6; s++) {
for (int v = 0; v < 4; v++) {
face.push_back(vert[cube2tets[s][v]]+1);
}
}
}
}
}
} else if (dim == 2) {
int quad2tri[2][3] = {{0, 1, 2}, {0, 2, 3}};
for (int ix = 0; ix < count[0]-1; ix++) {
for (int iy = 0; iy < count[1]-1; iy++) {
int vert[4] = {
count[2]*count[1]*(ix+0) + count[2]*(iy+0),
count[2]*count[1]*(ix+1) + count[2]*(iy+0),
count[2]*count[1]*(ix+1) + count[2]*(iy+1),
count[2]*count[1]*(ix+0) + count[2]*(iy+1),
};
for (int s = 0; s < 2; s++) {
for (int v = 0; v < 3; v++) {
face.push_back(vert[quad2tri[s][v]]+1);
}
}
}
}
}
mjXUtil::Vector2String(userface, face);
} else {
dim = 2; // can only load a surface for now
mjXUtil::String2Vector(userface, face);
}
// compute volume
std::vector<mjtNum> volume(uservert.size()/3);
mjtNum t = 1;
if (dim == 2 && plugin_instance) {
// do nothing for now (until new passive forces are supported)
}
if (!userface.empty()) {
mjXUtil::String2Vector(userface, face);
for (int j=0; j<face.size()/3; j++) {
mjtNum area[3];
mjtNum edge1[3];
mjtNum edge2[3];
for (int i=0; i<3; i++) {
edge1[i] = uservert[3*(face[3*j+1]-1)+i] - uservert[3*(face[3*j]-1)+i];
edge2[i] = uservert[3*(face[3*j+2]-1)+i] - uservert[3*(face[3*j]-1)+i];
}
mjuu_crossvec(area, edge1, edge2);
for (int i=0; i<3; i++) {
volume[face[3*j+i]-1] += sqrt(mjuu_dot3(area, area)) / 2 * t;
}
}
} else {
for (int i=0; i<uservert.size()/3; i++) {
volume[i] = 6 * spacing * spacing / 2 * t;
}
}
// create bodies and geoms
for (int i=0; i<uservert.size()/3; i++) {
// create body
mjCBody* b = body->AddBody(NULL);
if (!username.empty()) {
b->name = username[i];
} else {
mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), i);
b->name = txt;
}
// set body position
b->pos[0] = offset[0] + uservert[3*i];
b->pos[1] = offset[1] + uservert[3*i+1];
b->pos[2] = offset[2] + uservert[3*i+2];
// add slider joints if none defined
if (!add[mjCOMPKIND_PARTICLE]) {
for (int i=0; i<3; i++) {
mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0], false);
jnt->def = body->def;
jnt->type = mjJNT_SLIDE;
mjuu_setvec(jnt->pos, 0, 0, 0);
mjuu_setvec(jnt->axis, 0, 0, 0);
jnt->axis[i] = 1;
}
}
// add user-specified joints
else {
for (auto defjnt : defjoint[mjCOMPKIND_PARTICLE]) {
mjCJoint* jnt = b->AddJoint(&defjnt, false);
jnt->def = body->def;
}
}
// add geom
mjCGeom* g = b->AddGeom(def);
g->def = body->def;
// add site
mjCSite* s = b->AddSite(def);
s->def = body->def;
s->type = mjGEOM_SPHERE;
mju::sprintf_arr(txt, "%sS%d", prefix.c_str(), i);
s->name = txt;
// add plugin
if (plugin_instance) {
b->is_plugin = true;
b->plugin_name = plugin_name;
b->plugin_instance = plugin_instance;
b->plugin_instance_name = plugin_instance_name;
if (i==0 && !plugin_instance->config_attribs["face"].empty()) {
return comperr(error, "Face attribute already exists in plugin", error_sz);
}
b->plugin_instance->config_attribs["face"] = userface;
// update density
if (dim == 2) {
g->density *= volume[i] / (4./3. * mjPI * pow(g->size[0], 3));
}
}
}
if (skin) {
MakeSkin3(model);
}
@@ -2040,8 +2153,46 @@ void mjCComposite::MakeSkin3(mjCModel* model) {
skin->inflate = skininflate;
skin->group = skingroup;
// copy skin from existing mesh
if (type==mjCOMPTYPE_PARTICLE && username.empty()) {
std::vector<int> face;
mjXUtil::String2Vector(userface, face);
int nvert = uservert.size()/3;
for (int j=0; j<2; j++) {
for (int i=0; i<nvert; i++) {
skin->vert.push_back(0);
skin->vert.push_back(0);
skin->vert.push_back(0);
mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), i);
skin->bodyname.push_back(txt);
skin->bindpos.push_back(0);
skin->bindpos.push_back(0);
skin->bindpos.push_back(0);
skin->bindquat.push_back(1);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
skin->bindquat.push_back(0);
vector<int> vertid;
vector<float> vertweight;
vertid.push_back(j*nvert+i);
vertweight.push_back(1);
skin->vertid.push_back(vertid);
skin->vertweight.push_back(vertweight);
}
for (int i=0; i<face.size()/3; i++) {
skin->face.push_back(j*nvert+face[3*i]-1);
skin->face.push_back(j*nvert+face[3*i+(j==0 ? 1 : 2)]-1);
skin->face.push_back(j*nvert+face[3*i+(j==0 ? 2 : 1)]-1);
}
}
}
// box
if (type==mjCOMPTYPE_BOX || type==mjCOMPTYPE_PARTICLE) {
else if (type==mjCOMPTYPE_BOX || type==mjCOMPTYPE_PARTICLE) {
// z-faces
MakeSkin3Box(skin, count[0], count[1], 1, vcnt, "%sB%d_%d_0");
fmt = "%sB%d_%d_" + string(cnt2);
src/user/user_composite.h
+4
View File
@@ -106,9 +106,13 @@ class mjCComposite {
// currently used only for cable
std::string initial; // root boundary type
std::vector<float> uservert; // user-specified vertex positions
std::string userface; // connectivity
mjtNum size[3]; // rope size (meaning depends on the shape)
mjtCompShape curve[3]; // geometric shape
// body names used in the skin
std::vector<std::string> username;
// plugin support
bool is_plugin;
std::string plugin_name;
src/user/user_mesh.cc
+24 -6
View File
@@ -13,12 +13,14 @@
// limitations under the License.
#include <algorithm>
#include <array>
#include <cmath>
#include <csetjmp>
#include <cstddef>
#include <cstdio>
#include <cstring>
#include <memory>
#include <optional>
#include <string>
#include <utility>
#include <vector>
@@ -27,10 +29,27 @@
#define TINYOBJLOADER_IMPLEMENTATION
#endif
#if defined(__clang__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wgnu-anonymous-struct"
#pragma clang diagnostic ignored "-Wnested-anon-types"
#elif defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#endif
#include <MC.h>
#if defined(__clang__)
#pragma clang diagnostic pop
#elif defined(__GNUC__)
#pragma GCC diagnostic pop
#endif
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include "cc/array_safety.h"
#include <mujoco/mjtnum.h>
#include <mujoco/mjplugin.h>
#include "engine/engine_crossplatform.h"
#include "engine/engine_plugin.h"
#include "engine/engine_resource.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
@@ -329,7 +348,7 @@ void mjCMesh::LoadSDF() {
// compiler
void mjCMesh::Compile(int vfs_provider) {
void mjCMesh::Compile(const mjVFS* vfs) {
// load file
if (!file_.empty()) {
// remove path from file if necessary
@@ -348,7 +367,7 @@ void mjCMesh::Compile(int vfs_provider) {
}
string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file_);
mjResource* resource = LoadResource(filename, vfs_provider);
mjResource* resource = LoadResource(filename, vfs);
try {
if (asset_type == "model/stl") {
@@ -1836,8 +1855,7 @@ mjCSkin::~mjCSkin() {
// compiler
void mjCSkin::Compile(int vfs_provider) {
void mjCSkin::Compile(const mjVFS* vfs) {
// load file
if (!file.empty()) {
// make sure data is not present
@@ -1865,7 +1883,7 @@ void mjCSkin::Compile(int vfs_provider) {
}
string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file);
mjResource* resource = LoadResource(filename, vfs_provider);
mjResource* resource = LoadResource(filename, vfs);
try {
LoadSKN(resource);
src/user/user_model.cc
+7 -7
View File
@@ -2423,7 +2423,7 @@ static void warninghandler(const char* msg) {
// compiler
mjModel* mjCModel::Compile(int vfs_provider) {
mjModel* mjCModel::Compile(const mjVFS* vfs) {
// The volatile keyword is necessary to prevent a possible memory leak due to
// an interaction between longjmp and compiler optimization. Specifically, at
// the point where the setjmp takes places, these pointers have never been
@@ -2454,7 +2454,7 @@ mjModel* mjCModel::Compile(int vfs_provider) {
// TryCompile resulted in an mju_error which was converted to a longjmp.
throw mjCError(0, "engine error: %s", errortext);
}
TryCompile(*const_cast<mjModel**>(&m), *const_cast<mjData**>(&data), vfs_provider);
TryCompile(*const_cast<mjModel**>(&m), *const_cast<mjData**>(&data), vfs);
} catch (mjCError err) {
// deallocate everything allocated in Compile
mj_deleteModel(m);
@@ -2480,7 +2480,7 @@ mjModel* mjCModel::Compile(int vfs_provider) {
}
void mjCModel::TryCompile(mjModel*& m, mjData*& d, int vfs_provider) {
void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
// check if nan test works
double test = mjNAN;
if (mjuu_defined(test)) {
@@ -2557,7 +2557,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, int vfs_provider) {
// compile meshes (needed for geom compilation)
for (int i=0; i<meshes.size(); i++) {
meshes[i]->Compile(vfs_provider);
meshes[i]->Compile(vfs);
}
// automatically set nuser fields
@@ -2616,9 +2616,9 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, int vfs_provider) {
}
// compile all other objects except for keyframes
for (int i=0; i<skins.size(); i++) skins[i]->Compile(vfs_provider);
for (int i=0; i<hfields.size(); i++) hfields[i]->Compile(vfs_provider);
for (int i=0; i<textures.size(); i++) textures[i]->Compile(vfs_provider);
for (int i=0; i<skins.size(); i++) skins[i]->Compile(vfs);
for (int i=0; i<hfields.size(); i++) hfields[i]->Compile(vfs);
for (int i=0; i<textures.size(); i++) textures[i]->Compile(vfs);
for (int i=0; i<materials.size(); i++) materials[i]->Compile();
for (int i=0; i<pairs.size(); i++) pairs[i]->Compile();
for (int i=0; i<excludes.size(); i++) excludes[i]->Compile();
src/user/user_model.h
+3 -3
View File
@@ -65,7 +65,7 @@ class mjCModel {
mjCModel(); // constructor
~mjCModel(); // destructor
mjModel* Compile(int vfs_provider = 0); // COMPILER: construct mjModel
mjModel* Compile(const mjVFS* vfs = 0); // COMPILER: construct mjModel
bool CopyBack(const mjModel*); // DECOMPILER: copy numeric back
void FuseStatic(void); // fuse static bodies with parent
void FuseReindex(mjCBody* body); // reindex elements during fuse
@@ -163,8 +163,8 @@ class mjCModel {
int nuser_sensor; // number of mjtNums in sensor_user
private:
void TryCompile(mjModel*& m, mjData*& d, int vfs_provider);
mjModel* _Compile(int vfs_provider);
void TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs);
mjModel* _Compile(const mjVFS* vfs);
void Clear(void); // clear objects allocated by Compile
src/user/user_objects.cc
+24 -29
View File
@@ -14,22 +14,22 @@
#include "user/user_objects.h"
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <sstream>
#include <string>
#include <string_view>
#include <vector>
#include "lodepng.h"
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include "cc/array_safety.h"
#include "engine/engine_core_smooth.h"
#include "engine/engine_crossplatform.h"
#include "engine/engine_resource.h"
#include "engine/engine_io.h"
#include "engine/engine_passive.h"
@@ -39,6 +39,7 @@
#include "engine/engine_util_misc.h"
#include "engine/engine_util_solve.h"
#include "engine/engine_util_spatial.h"
#include "engine/engine_vfs.h"
#include "user/user_model.h"
#include "user/user_util.h"
@@ -130,7 +131,7 @@ mjCError::mjCError(const mjCBase* obj, const char* msg, const char* str, int pos
// constructor
mjCAlternative::mjCAlternative() {
axisangle[0] = xyaxes[0] = zaxis[0] = euler[0] = fullinertia[0] = mjNAN;
};
}
// compute frame orientation given alternative specifications
@@ -495,17 +496,14 @@ mjCBase::mjCBase() {
// load resource if found (fallback to OS filesystem)
mjResource* mjCBase::LoadResource(string filename, int provider) {
mjResource* mjCBase::LoadResource(string filename, const mjVFS* vfs) {
mjResource* r = nullptr;
const char* cname = filename.c_str();
// try reading from given provider
if ((r = mju_openResource(cname, provider)) == nullptr) {
if (!provider) {
throw mjCError(0, "file not found: '%s'", cname);
}
// if provider wasn't the OS filesystem try to fallback to OS filesystem
if ((r = mju_openResource(filename.c_str(), 0)) == nullptr) {
// try reading from provided VFS
if ((r = mju_openVfsResource(cname, vfs)) == nullptr) {
// not in vfs try a provider or fallback to OS filesystem
if ((r = mju_openResource(filename.c_str())) == nullptr) {
throw mjCError(this, "resource not found via provider or OS filesystem: '%s'", cname);
}
}
@@ -1516,7 +1514,6 @@ void mjCGeom::SetFluidCoefs(void) {
// get semiaxes
switch (type) {
case mjGEOM_SPHERE:
dx = size[0];
dy = size[0];
@@ -2194,7 +2191,7 @@ void mjCHField::LoadPNG(mjResource* resource) {
// compiler
void mjCHField::Compile(int vfs_provider) {
void mjCHField::Compile(const mjVFS* vfs) {
// check size parameters
for (int i=0; i<4; i++)
if (size[i]<=0)
@@ -2226,7 +2223,7 @@ void mjCHField::Compile(int vfs_provider) {
}
string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file);
mjResource* resource = LoadResource(filename, vfs_provider);
mjResource* resource = LoadResource(filename, vfs);
try {
if (asset_type == "image/png") {
@@ -2617,7 +2614,7 @@ void mjCTexture::LoadCustom(mjResource* resource,
// load from PNG or custom file, flip if specified
void mjCTexture::LoadFlip(string filename, int vfs_provider,
void mjCTexture::LoadFlip(string filename, const mjVFS* vfs,
std::vector<unsigned char>& image,
unsigned int& w, unsigned int& h) {
std::string asset_type = GetAssetContentType(filename, content_type);
@@ -2631,7 +2628,7 @@ void mjCTexture::LoadFlip(string filename, int vfs_provider,
throw mjCError(this, "unsupported content type: '%s'", asset_type.c_str());
}
mjResource* resource = LoadResource(filename, vfs_provider);
mjResource* resource = LoadResource(filename, vfs);
try {
if (asset_type == "image/png") {
@@ -2693,11 +2690,11 @@ void mjCTexture::LoadFlip(string filename, int vfs_provider,
// load 2D
void mjCTexture::Load2D(string filename, int vfs_provider) {
void mjCTexture::Load2D(string filename, const mjVFS* vfs) {
// load PNG or custom
unsigned int w, h;
std::vector<unsigned char> image;
LoadFlip(filename, vfs_provider, image, w, h);
LoadFlip(filename, vfs, image, w, h);
// assign size
width = w;
@@ -2716,7 +2713,7 @@ void mjCTexture::Load2D(string filename, int vfs_provider) {
// load cube or skybox from single file (repeated or grid)
void mjCTexture::LoadCubeSingle(string filename, int vfs_provider) {
void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
// check gridsize
if (gridsize[0]<1 || gridsize[1]<1 || gridsize[0]*gridsize[1]>12) {
throw mjCError(this,
@@ -2727,7 +2724,7 @@ void mjCTexture::LoadCubeSingle(string filename, int vfs_provider) {
// load PNG or custom
unsigned int w, h;
std::vector<unsigned char> image;
LoadFlip(filename, vfs_provider, image, w, h);
LoadFlip(filename, vfs, image, w, h);
// check gridsize for compatibility
if (w/gridsize[1]!=h/gridsize[0] || (w%gridsize[1]) || (h%gridsize[0])) {
@@ -2816,7 +2813,7 @@ void mjCTexture::LoadCubeSingle(string filename, int vfs_provider) {
// load cube or skybox from separate file
void mjCTexture::LoadCubeSeparate(int vfs_provider) {
void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) {
// keep track of which faces were defined
int loaded[6] = {0, 0, 0, 0, 0, 0};
@@ -2834,7 +2831,7 @@ void mjCTexture::LoadCubeSeparate(int vfs_provider) {
// load PNG or custom
unsigned int w, h;
std::vector<unsigned char> image;
LoadFlip(filename, vfs_provider, image, w, h);
LoadFlip(filename, vfs, image, w, h);
// PNG must be square
if (w!=h) {
@@ -2888,7 +2885,7 @@ void mjCTexture::LoadCubeSeparate(int vfs_provider) {
// compiler
void mjCTexture::Compile(int vfs_provider) {
void mjCTexture::Compile(const mjVFS* vfs) {
// builtin
if (builtin!=mjBUILTIN_NONE) {
// check size
@@ -2931,9 +2928,9 @@ void mjCTexture::Compile(int vfs_provider) {
// dispatch
if (type==mjTEXTURE_2D) {
Load2D(filename, vfs_provider);
Load2D(filename, vfs);
} else {
LoadCubeSingle(filename, vfs_provider);
LoadCubeSingle(filename, vfs);
}
}
@@ -2961,7 +2958,7 @@ void mjCTexture::Compile(int vfs_provider) {
}
// only cube and skybox
LoadCubeSeparate(vfs_provider);
LoadCubeSeparate(vfs);
}
// make sure someone allocated data; SHOULD NOT OCCUR
@@ -4499,7 +4496,6 @@ mjCKey::~mjCKey() {
// compiler
void mjCKey::Compile(const mjModel* m) {
// qpos: allocate or check size
if (qpos.empty()) {
qpos.resize(m->nq);
@@ -4574,7 +4570,6 @@ void mjCKey::Compile(const mjModel* m) {
} else if (ctrl.size()!=m->nu) {
throw mjCError(this, "key %d: invalid ctrl size, expected length %d", nullptr, id, m->nu);
}
}
src/user/user_objects.h
+10 -9
View File
@@ -23,6 +23,7 @@
#include "lodepng.h"
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
// forward declarations of all mjC/X classes
class mjCError;
@@ -173,7 +174,7 @@ class mjCBase {
public:
// load resource if found (fallback to OS filesystem)
mjResource* LoadResource(std::string filename, int provider);
mjResource* LoadResource(std::string filename, const mjVFS* vfs);
// Get and sanitize content type from raw_text if not empty, otherwise parse
// content type from resource_name; throw on failure
@@ -572,7 +573,7 @@ class mjCMesh: public mjCBase {
void set_usertexcoord(std::optional<std::vector<float>>&& usertexcoord);
void set_userface(std::optional<std::vector<int>>&& userface);
void Compile(int vfs_provider); // compiler
void Compile(const mjVFS* vfs); // compiler
double* GetPosPtr(mjtMeshType type); // get position
double* GetQuatPtr(mjtMeshType type); // get orientation
double* GetInertiaBoxPtr(mjtMeshType type); // get inertia box
@@ -696,7 +697,7 @@ class mjCSkin: public mjCBase {
private:
mjCSkin(mjCModel* = 0); // constructor
~mjCSkin(); // destructor
void Compile(int vfs_provider); // compiler
void Compile(const mjVFS* vfs); // compiler
void LoadSKN(mjResource* resource); // load skin in SKN BIN format
int matid; // material id
@@ -723,7 +724,7 @@ class mjCHField : public mjCBase {
private:
mjCHField(mjCModel* model); // constructor
~mjCHField(); // destructor
void Compile(int vfs_provider); // compiler
void Compile(const mjVFS* vfs); // compiler
void LoadCustom(mjResource* resource); // load from custom format
void LoadPNG(mjResource* resource); // load from PNG format
@@ -768,15 +769,15 @@ class mjCTexture : public mjCBase {
private:
mjCTexture(mjCModel*); // constructor
~mjCTexture(); // destructior
void Compile(int vfs_provider); // compiler
void Compile(const mjVFS* vfs); // compiler
void Builtin2D(void); // make builtin 2D
void BuiltinCube(void); // make builtin cube
void Load2D(std::string filename, int vfs_provider); // load 2D from file
void LoadCubeSingle(std::string filename, int vfs_provider); // load cube from single file
void LoadCubeSeparate(int vfs_provider); // load cube from separate files
void Load2D(std::string filename, const mjVFS* vfs); // load 2D from file
void LoadCubeSingle(std::string filename, const mjVFS* vfs); // load cube from single file
void LoadCubeSeparate(const mjVFS* vfs); // load cube from separate files
void LoadFlip(std::string filename, int vfs_provider, // load and flip
void LoadFlip(std::string filename, const mjVFS* vfs, // load and flip
std::vector<unsigned char>& image,
unsigned int& w, unsigned int& h);
src/xml/xml.cc
+13 -11
View File
@@ -22,9 +22,11 @@
#include <string>
#include <mujoco/mjmodel.h>
#include "cc/array_safety.h"
#include "engine/engine_crossplatform.h"
#include "engine/engine_resource.h"
#include "engine/engine_vfs.h"
#include "user/user_model.h"
#include "user/user_util.h"
#include "xml/xml_native_reader.h"
@@ -110,7 +112,7 @@ string mjWriteXML(mjCModel* model, char* error, int error_sz) {
// find include elements recursively, replace them with subtree from xml file
static XMLElement* mjIncludeXML(XMLElement* elem, string dir,
int vfs_provider, vector<string>& included) {
const mjVFS* vfs, vector<string>& included) {
// include element: process
if (!strcasecmp(elem->Value(), "include")) {
// make sure include has no children
@@ -133,9 +135,9 @@ static XMLElement* mjIncludeXML(XMLElement* elem, string dir,
// get data source
mjResource *resource = nullptr;
const char* xmlstring = nullptr;
if ((resource = mju_openResource(filename.c_str(), vfs_provider)) == nullptr) {
// load from OS filesystem
if (!vfs_provider || (resource = mju_openResource(filename.c_str(), 0)) == nullptr) {
if ((resource = mju_openVfsResource(filename.c_str(), vfs)) == nullptr) {
// load from provider or OS filesystem
if ((resource = mju_openResource(filename.c_str())) == nullptr) {
throw mjXError(elem, "Could not open file '%s'", filename.c_str());
}
}
@@ -196,14 +198,14 @@ static XMLElement* mjIncludeXML(XMLElement* elem, string dir,
}
// run XMLInclude on first new child
return mjIncludeXML(first->ToElement(), dir, vfs_provider, included);
return mjIncludeXML(first->ToElement(), dir, vfs, included);
}
// otherwise check all child elements, return self
else {
XMLElement* child = elem->FirstChildElement();
while (child) {
child = mjIncludeXML(child, dir, vfs_provider, included);
child = mjIncludeXML(child, dir, vfs, included);
if (child) {
child = child->NextSiblingElement();
}
@@ -215,7 +217,7 @@ static XMLElement* mjIncludeXML(XMLElement* elem, string dir,
// Main parser function
mjCModel* mjParseXML(const char* filename, int vfs_provider, char* error, int error_sz) {
mjCModel* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz) {
LocaleOverride locale_override;
// check arguments
@@ -235,9 +237,9 @@ mjCModel* mjParseXML(const char* filename, int vfs_provider, char* error, int er
// get data source
mjResource* resource = nullptr;
const char* xmlstring = nullptr;
if ((resource = mju_openResource(filename, vfs_provider)) == nullptr) {
// load from OS filesystem
if (!vfs_provider || (resource = mju_openResource(filename, 0)) == nullptr) {
if ((resource = mju_openVfsResource(filename, vfs)) == nullptr) {
// load from provider or fallback to OS filesystem
if ((resource = mju_openResource(filename)) == nullptr) {
if (error) {
snprintf(error, error_sz, "mjParseXML: could not open file '%s'", filename);
}
@@ -303,7 +305,7 @@ mjCModel* mjParseXML(const char* filename, int vfs_provider, char* error, int er
// find include elements, replace them with subtree from xml file
vector<string> included;
included.push_back(filename);
mjIncludeXML(root, model->modelfiledir, vfs_provider, included);
mjIncludeXML(root, model->modelfiledir, vfs, included);
// parse MuJoCo model
mjXReader parser;
src/xml/xml.h
+1 -1
View File
@@ -26,7 +26,7 @@
std::string mjWriteXML(mjCModel* model, char* error, int error_sz);
// Main parser function
mjCModel* mjParseXML(const char* filename, int vfs_provider, char* error, int error_sz);
mjCModel* mjParseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz);
#endif // MUJOCO_SRC_XML_XML_H_
src/xml/xml_api.cc
+8 -30
View File
@@ -70,20 +70,23 @@ static std::mutex themutex;
//---------------------------------- Functions -----------------------------------------------------
// mj_loadXML helper function
mjModel* _loadXML(const char* filename, int vfs_provider,
char* error, int error_sz) {
// parse XML file in MJCF or URDF format, compile it, return low-level model
// if vfs is not NULL, look up files in vfs before reading from disk
// error can be NULL; otherwise assumed to have size error_sz
mjModel* mj_loadXML(const char* filename, const mjVFS* vfs,
char* error, int error_sz) {
// serialize access to themodel
std::lock_guard<std::mutex> lock(themutex);
// parse new model
mjCModel* newmodel = mjParseXML(filename, vfs_provider, error, error_sz);
mjCModel* newmodel = mjParseXML(filename, vfs, error, error_sz);
if (!newmodel) {
return nullptr;
}
// compile new model
mjModel* m = newmodel->Compile(vfs_provider);
mjModel* m = newmodel->Compile(vfs);
if (!m) {
mjCopyError(error, newmodel->GetError().message, error_sz);
delete newmodel;
@@ -106,31 +109,6 @@ mjModel* _loadXML(const char* filename, int vfs_provider,
// parse XML file in MJCF or URDF format, compile it, return low-level model
// if vfs is not NULL, look up files in vfs before reading from disk
// error can be NULL; otherwise assumed to have size error_sz
mjModel* mj_loadXML(const char* filename, const mjVFS* vfs,
char* error, int error_sz) {
if (vfs == nullptr) {
return _loadXML(filename, 0, error, error_sz);
}
int index = mj_registerVfsProvider(vfs);
if (index < 1) {
if (error) {
snprintf(error, error_sz, "mj_loadXML: could not register VFS");
}
return nullptr;
}
mjModel* model = _loadXML(filename, index, error, error_sz);
mjp_unregisterResourceProvider(index);
return model;
}
// update XML data structures with info from low-level model, save as MJCF
// returns 1 if successful, 0 otherwise
// error can be NULL; otherwise assumed to have size error_sz
src/xml/xml_native_reader.cc
+11 -6
View File
@@ -95,12 +95,12 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"inttotal", "interval", "tolrange"},
{">"},
{"option", "*", "24",
"timestep", "apirate", "impratio", "tolerance", "noslip_tolerance", "mpr_tolerance",
"gravity", "wind", "magnetic", "density", "viscosity",
{"option", "*", "26",
"timestep", "apirate", "impratio", "tolerance", "ls_tolerance", "noslip_tolerance",
"mpr_tolerance", "gravity", "wind", "magnetic", "density", "viscosity",
"o_margin", "o_solref", "o_solimp",
"integrator", "collision", "cone", "jacobian",
"solver", "iterations", "noslip_iterations", "mpr_iterations",
"solver", "iterations", "ls_iterations", "noslip_iterations", "mpr_iterations",
"sdf_iterations", "sdf_initpoints"},
{"<"},
{"flag", "?", "22", "constraint", "equality", "frictionloss", "limit", "contact",
@@ -270,8 +270,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"light", "*", "15", "name", "class", "directional", "castshadow", "active",
"pos", "dir", "attenuation", "cutoff", "exponent", "ambient", "diffuse", "specular",
"mode", "target"},
{"composite", "*", "12", "prefix", "type", "count", "spacing", "offset",
"flatinertia", "solrefsmooth", "solimpsmooth", "vertex",
{"composite", "*", "13", "prefix", "type", "count", "spacing", "offset",
"flatinertia", "solrefsmooth", "solimpsmooth", "vertex", "face",
"initial", "curve", "size"},
{"<"},
{"plugin", "*", "2", "plugin", "instance"},
@@ -958,6 +958,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
ReadAttr(section, "apirate", 1, &opt->apirate, text);
ReadAttr(section, "impratio", 1, &opt->impratio, text);
ReadAttr(section, "tolerance", 1, &opt->tolerance, text);
ReadAttr(section, "ls_tolerance", 1, &opt->ls_tolerance, text);
ReadAttr(section, "noslip_tolerance", 1, &opt->noslip_tolerance, text);
ReadAttr(section, "mpr_tolerance", 1, &opt->mpr_tolerance, text);
ReadAttr(section, "gravity", 3, opt->gravity, text);
@@ -976,6 +977,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
MapValue(section, "jacobian", &opt->jacobian, jac_map, jac_sz);
MapValue(section, "solver", &opt->solver, solver_map, solver_sz);
ReadAttrInt(section, "iterations", &opt->iterations);
ReadAttrInt(section, "ls_iterations", &opt->ls_iterations);
ReadAttrInt(section, "noslip_iterations", &opt->noslip_iterations);
ReadAttrInt(section, "mpr_iterations", &opt->mpr_iterations);
ReadAttrInt(section, "sdf_iterations", &opt->sdf_iterations);
@@ -1943,6 +1945,9 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) {
String2Vector(text, comp.uservert);
}
// shell
ReadAttrTxt(elem, "face", comp.userface);
// process curve string
std::istringstream iss(curves);
int i = 0;
src/xml/xml_native_writer.cc
+2
View File
@@ -793,6 +793,7 @@ void mjXWriter::Option(XMLElement* root) {
WriteAttr(section, "apirate", 1, &model->option.apirate, &opt.apirate);
WriteAttr(section, "impratio", 1, &model->option.impratio, &opt.impratio);
WriteAttr(section, "tolerance", 1, &model->option.tolerance, &opt.tolerance);
WriteAttr(section, "ls_tolerance", 1, &model->option.ls_tolerance, &opt.ls_tolerance);
WriteAttr(section, "noslip_tolerance", 1, &model->option.noslip_tolerance, &opt.noslip_tolerance);
WriteAttr(section, "mpr_tolerance", 1, &model->option.mpr_tolerance, &opt.mpr_tolerance);
WriteAttr(section, "gravity", 3, model->option.gravity, opt.gravity);
@@ -816,6 +817,7 @@ void mjXWriter::Option(XMLElement* root) {
WriteAttrKey(section, "solver", solver_map, solver_sz,
model->option.solver, opt.solver);
WriteAttrInt(section, "iterations", model->option.iterations, opt.iterations);
WriteAttrInt(section, "ls_iterations", model->option.ls_iterations, opt.ls_iterations);
WriteAttrInt(section, "noslip_iterations", model->option.noslip_iterations, opt.noslip_iterations);
WriteAttrInt(section, "mpr_iterations", model->option.mpr_iterations, opt.mpr_iterations);
WriteAttrInt(section, "sdf_iterations", model->option.sdf_iterations, opt.sdf_iterations);

Some files were not shown because too many files have changed in this diff Show More