From 215433f5f7b8ffa0e11ddfde0a3f0f74836f4a71 Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Wed, 30 Aug 2023 04:28:17 -0700 Subject: [PATCH 01/38] Implement mj_stackAlloc and mj_arenaAlloc alignment using bitwise-and. This implementation explicitly avoids calling division instructions (which are generally expensive) even when link-time optimization is not active or if the alloc functions aren't inlined for some reason. Also make mj_stackAlloc cheaper for external callers who may be linking against MuJoCo dynamically (and therefore cannot benefit from inlining). PiperOrigin-RevId: 561299565 Change-Id: Iea4be5bf6d351ddf0c9d1fd8bd89a0c207b39309 --- doc/APIreference/functions.rst | 3 ++- include/mujoco/mujoco.h | 3 ++- introspect/functions.py | 2 +- src/engine/engine_io.c | 9 +++++++-- src/engine/engine_io.h | 4 ++-- 5 files changed, 14 insertions(+), 7 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 963bc409..c80c8358 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -1239,7 +1239,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 which must be a power of 2. +Call mju_error on stack overflow. .. _mj_stackAllocNum: diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 706def25..19c37f67 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -188,7 +188,8 @@ 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. +// Allocate a number of bytes on mjData stack at a specific alignment which must be a power of 2. +// 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. diff --git a/introspect/functions.py b/introspect/functions.py index 87b1bde2..a5bbf038 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -699,7 +699,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 which must be a power of 2. Call mju_error on stack overflow.', # pylint: disable=line-too-long )), ('mj_stackAllocNum', FunctionDecl( diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index d62044e7..5b13811f 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -45,6 +45,11 @@ static const int MAX_ARRAY_SIZE = INT_MAX / 4; +// compute a % b assuming that the second argument is a power of 2 +static inline size_t modpow2(size_t a, size_t b) { + return a & (b - 1); +} + //------------------------------ mjLROpt ----------------------------------------------------------- // set default options for length range computation @@ -1200,7 +1205,7 @@ 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 = modpow2(d->parena, alignment); size_t padding = misalignment ? alignment - misalignment : 0; // check size @@ -1255,7 +1260,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 -= modpow2(start_ptr, alignment); // new top of the stack uintptr_t new_pstack_ptr = start_ptr - mjREDZONE; diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index 0916d769..137a00f4 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -102,10 +102,10 @@ 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); -// mjData arena allocate +// mjData arena allocate (alignment must be a power of 2) MJAPI void* mj_arenaAlloc(mjData* d, size_t bytes, size_t alignment); -// mjData stack allocate +// mjData stack allocate (alignment must be a power of 2) MJAPI void* mj_stackAlloc(mjData* d, size_t bytes, size_t alignment); // mjData stack allocate for array of mjtNums From f86e8b449fdef29d276cd78ddd9d36e90681f742 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Wed, 30 Aug 2023 04:46:32 -0700 Subject: [PATCH 02/38] Refactor VFS logic out of ResourceProvider plugin code. PiperOrigin-RevId: 561302536 Change-Id: I952f9e76e85a7e3a111e0338f307378a6ff58197 --- doc/includes/references.h | 47 ++++------ include/mujoco/mjmodel.h | 31 ++----- include/mujoco/mjplugin.h | 23 +++-- introspect/structs.py | 2 +- src/engine/engine_io.c | 30 ++---- src/engine/engine_plugin.cc | 118 ++++-------------------- src/engine/engine_plugin.h | 3 - src/engine/engine_resource.c | 121 ++++++++----------------- src/engine/engine_resource.h | 13 ++- src/engine/engine_vfs.c | 65 +++++++++---- src/engine/engine_vfs.h | 5 +- src/user/user_mesh.cc | 9 +- src/user/user_model.cc | 14 +-- src/user/user_model.h | 6 +- src/user/user_objects.cc | 42 ++++----- src/user/user_objects.h | 19 ++-- src/xml/xml.cc | 24 ++--- src/xml/xml.h | 2 +- src/xml/xml_api.cc | 38 ++------ test/engine/engine_resource_test.cc | 131 +++++++++++++++------------ test/fixture.cc | 10 +- unity/Runtime/Bindings/MjBindings.cs | 2 +- 22 files changed, 310 insertions(+), 445 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 3a4f910f..35aaa77c 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -655,28 +655,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 @@ -1262,15 +1247,21 @@ 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) + 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 diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 61f70100..38f28f44 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -17,6 +17,7 @@ #include + #include // 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 diff --git a/include/mujoco/mjplugin.h b/include/mujoco/mjplugin.h index a6936e20..aa23bc44 100644 --- a/include/mujoco/mjplugin.h +++ b/include/mujoco/mjplugin.h @@ -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); @@ -39,15 +44,15 @@ typedef void (*mjfCloseResource)(mjResource* resource); typedef void (*mjfGetResourceDir)(mjResource* resource, const char** dir, int* ndir); // 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) + void* data; // opaque data pointer (resource invariant) }; -typedef struct mjpResourceProvider_ mjpResourceProvider; +typedef struct mjpResourceProvider mjpResourceProvider; //---------------------------------- Plugins ------------------------------------------------------- diff --git a/introspect/structs.py b/introspect/structs.py index f0cc95d9..5ac4330a 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -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', diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 5b13811f..159ab555 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -661,7 +661,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]; @@ -670,8 +670,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; @@ -785,27 +788,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) { diff --git a/src/engine/engine_plugin.cc b/src/engine/engine_plugin.cc index 1b314b5d..52e86455 100644 --- a/src/engine/engine_plugin.cc +++ b/src/engine/engine_plugin.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -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,7 @@ bool ResourceProvidersAreIdentical(const mjpResourceProvider* p1, const mjpResou p1->open == p2->open && p1->read == p2->read && p1->close == p2->close && + p1->getdir == p2->getdir && p1->data == p2->data); } @@ -532,18 +531,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 +550,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 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& global = GetGlobal(); 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* table = &global.table(); - PluginTable* 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 +578,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 +594,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::kBlockSize) { + if (local_idx == PluginTable::kBlockSize) { local_idx = 0; table = AddNewTableBlock(table); if (!table) { @@ -636,19 +604,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 +624,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& global = GetGlobal(); - auto lock = global.lock_mutex_exclusively(); - int count = global.count().load(std::memory_order_acquire); - - if (slot >= count) { - return; - } - - PluginTable* 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::kBlockSize) { - local_idx -= PluginTable::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().count().load(std::memory_order_acquire); @@ -728,12 +649,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& global = GetGlobal(); auto lock = global.lock_mutex_exclusively(); PluginTable* table = &global.table(); @@ -743,7 +658,6 @@ const mjpResourceProvider* mjp_getResourceProvider(const char* resource_name) { for (int i = 0; i < PluginTable::kBlockSize && found_slot < count; ++i, ++found_slot) { - const mjpResourceProvider& provider = table->plugins[i]; const char *prefix = provider.prefix; diff --git a/src/engine/engine_plugin.h b/src/engine/engine_plugin.h index 83fbef83..13068a4a 100644 --- a/src/engine/engine_plugin.h +++ b/src/engine/engine_plugin.h @@ -31,9 +31,6 @@ 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(); diff --git a/src/engine/engine_resource.c b/src/engine/engine_resource.c index eba0ad62..bd7280fa 100644 --- a/src/engine/engine_resource.c +++ b/src/engine/engine_resource.c @@ -16,40 +16,23 @@ #include #include +#include #include #include -#include #include #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 } 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 +40,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,48 +63,19 @@ 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; } return resource; } @@ -131,20 +88,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 +114,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 +132,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; @@ -211,7 +168,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"); diff --git a/src/engine/engine_resource.h b/src/engine/engine_resource.h index 7c659d0b..b45b3a82 100644 --- a/src/engine/engine_resource.h +++ b/src/engine/engine_resource.h @@ -15,6 +15,8 @@ #ifndef MUJOCO_SRC_ENGINE_ENGINE_RESOURCE_H_ #define MUJOCO_SRC_ENGINE_ENGINE_RESOURCE_H_ +#include + #include #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 } diff --git a/src/engine/engine_vfs.c b/src/engine/engine_vfs.c index e4246bb7..0535111e 100644 --- a/src/engine/engine_vfs.c +++ b/src/engine/engine_vfs.c @@ -14,10 +14,10 @@ #include "engine/engine_vfs.h" +#include #include #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,49 @@ 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, }; - 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; } diff --git a/src/engine/engine_vfs.h b/src/engine/engine_vfs.h index 7fa0cd40..b9b578a1 100644 --- a/src/engine/engine_vfs.h +++ b/src/engine/engine_vfs.h @@ -17,6 +17,7 @@ #include #include +#include #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 } diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 6a04f4b3..548819ab 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -29,6 +29,7 @@ #include #include +#include #include "cc/array_safety.h" #include "engine/engine_crossplatform.h" #include "engine/engine_resource.h" @@ -329,7 +330,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 +349,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,7 +1837,7 @@ mjCSkin::~mjCSkin() { // compiler -void mjCSkin::Compile(int vfs_provider) { +void mjCSkin::Compile(const mjVFS* vfs) { // load file if (!file.empty()) { @@ -1865,7 +1866,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); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 3bdde0ed..8b782f08 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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(&m), *const_cast(&data), vfs_provider); + TryCompile(*const_cast(&m), *const_cast(&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; iCompile(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; iCompile(vfs_provider); - for (int i=0; iCompile(vfs_provider); - for (int i=0; iCompile(vfs_provider); + for (int i=0; iCompile(vfs); + for (int i=0; iCompile(vfs); + for (int i=0; iCompile(vfs); for (int i=0; iCompile(); for (int i=0; iCompile(); for (int i=0; iCompile(); diff --git a/src/user/user_model.h b/src/user/user_model.h index 4ac38f3a..f4a3ed68 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -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 diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 853d5a70..75eca814 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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" @@ -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); } } @@ -2194,7 +2192,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 +2224,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 +2615,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& image, unsigned int& w, unsigned int& h) { std::string asset_type = GetAssetContentType(filename, content_type); @@ -2631,7 +2629,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 +2691,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 image; - LoadFlip(filename, vfs_provider, image, w, h); + LoadFlip(filename, vfs, image, w, h); // assign size width = w; @@ -2716,7 +2714,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 +2725,7 @@ void mjCTexture::LoadCubeSingle(string filename, int vfs_provider) { // load PNG or custom unsigned int w, h; std::vector 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 +2814,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 +2832,7 @@ void mjCTexture::LoadCubeSeparate(int vfs_provider) { // load PNG or custom unsigned int w, h; std::vector image; - LoadFlip(filename, vfs_provider, image, w, h); + LoadFlip(filename, vfs, image, w, h); // PNG must be square if (w!=h) { @@ -2888,7 +2886,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 +2929,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 +2959,7 @@ void mjCTexture::Compile(int vfs_provider) { } // only cube and skybox - LoadCubeSeparate(vfs_provider); + LoadCubeSeparate(vfs); } // make sure someone allocated data; SHOULD NOT OCCUR diff --git a/src/user/user_objects.h b/src/user/user_objects.h index e5bbd58a..148b3997 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -23,6 +23,7 @@ #include "lodepng.h" #include +#include // 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>&& usertexcoord); void set_userface(std::optional>&& 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& image, unsigned int& w, unsigned int& h); diff --git a/src/xml/xml.cc b/src/xml/xml.cc index 7b8ec3cd..fda3cde8 100644 --- a/src/xml/xml.cc +++ b/src/xml/xml.cc @@ -22,9 +22,11 @@ #include +#include #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& included) { + const mjVFS* vfs, vector& 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 included; included.push_back(filename); - mjIncludeXML(root, model->modelfiledir, vfs_provider, included); + mjIncludeXML(root, model->modelfiledir, vfs, included); // parse MuJoCo model mjXReader parser; diff --git a/src/xml/xml.h b/src/xml/xml.h index acbe81ce..84ef9ae3 100644 --- a/src/xml/xml.h +++ b/src/xml/xml.h @@ -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_ diff --git a/src/xml/xml_api.cc b/src/xml/xml_api.cc index bad07392..4aa468eb 100644 --- a/src/xml/xml_api.cc +++ b/src/xml/xml_api.cc @@ -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 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 diff --git a/test/engine/engine_resource_test.cc b/test/engine/engine_resource_test.cc index 686c297d..c3287375 100644 --- a/test/engine/engine_resource_test.cc +++ b/test/engine/engine_resource_test.cc @@ -19,12 +19,10 @@ #include #include #include "src/cc/array_safety.h" -#include #include #include #include "src/engine/engine_plugin.h" #include "src/engine/engine_resource.h" -#include "src/engine/engine_vfs.h" #include "test/fixture.h" namespace mujoco { @@ -68,20 +66,48 @@ void close_str(mjResource* resource) { } TEST_F(ResourceTest, RegisterProviderSuccess) { - mjpResourceProvider provider = {"my-prefix.123+45", open_nop, read_nop, close_nop, - nullptr}; + mjpResourceProvider provider = { + "my-prefix.123+45", open_nop, read_nop, close_nop + }; int count1 = mjp_resourceProviderCount(); int i = mjp_registerResourceProvider(&provider); int count2 = mjp_resourceProviderCount(); EXPECT_GT(i, 0); - EXPECT_EQ(count1+1, count2); + EXPECT_EQ(count2 - count1, 1); +} + +TEST_F(ResourceTest, RegisterProviderMultipleSuccess) { + mjpResourceProvider provider = { + "my-prefix.123+44", open_nop, read_nop, close_nop + }; + + mjpResourceProvider provider2 = { + "my-prefix.123+46", open_nop, read_nop, close_nop + }; + + + mjpResourceProvider provider3 = { + "my-prefix.123+41", open_nop, read_nop, close_nop + }; + + int count1 = mjp_resourceProviderCount(); + int i = mjp_registerResourceProvider(&provider); + int i2 = mjp_registerResourceProvider(&provider2); + int i3 = mjp_registerResourceProvider(&provider3); + int count2 = mjp_resourceProviderCount(); + + EXPECT_GT(i, 0); + EXPECT_GT(i2, 0); + EXPECT_GT(i3, 0); + EXPECT_EQ(count2 - count1, 3); } TEST_F(ResourceTest, RegisterProviderMissingCallbacks) { - mjpResourceProvider provider = {"myprefix", nullptr, nullptr, nullptr, - nullptr}; + mjpResourceProvider provider = { + "myprefix" + }; // install warning handler static char warning[1024]; @@ -98,7 +124,9 @@ TEST_F(ResourceTest, RegisterProviderMissingCallbacks) { } TEST_F(ResourceTest, RegisterProviderMissingPrefix) { - mjpResourceProvider provider = {"", open_nop, read_nop, close_nop, nullptr, nullptr}; + mjpResourceProvider provider = { + "", open_nop, read_nop, close_nop + }; // install warning handler static char warning[1024]; @@ -115,7 +143,9 @@ TEST_F(ResourceTest, RegisterProviderMissingPrefix) { } TEST_F(ResourceTest, RegisterProviderInvalidPrefix1) { - mjpResourceProvider provider = {"1invalid", open_nop, read_nop, close_nop, nullptr, nullptr}; + mjpResourceProvider provider = { + "1invalid", open_nop, read_nop, close_nop + }; // install warning handler static char warning[1024]; @@ -132,7 +162,9 @@ TEST_F(ResourceTest, RegisterProviderInvalidPrefix1) { } TEST_F(ResourceTest, RegisterProviderInvalidPrefix2) { - mjpResourceProvider provider = {"invalid:", open_nop, read_nop, close_nop, nullptr, nullptr}; + mjpResourceProvider provider = { + "invalid:", open_nop, read_nop, close_nop + }; // install warning handler static char warning[1024]; @@ -149,11 +181,13 @@ TEST_F(ResourceTest, RegisterProviderInvalidPrefix2) { } TEST_F(ResourceTest, RegisterProviderSame) { - mjpResourceProvider provider = {"prefix", open_nop, read_nop, close_nop, - nullptr, nullptr}; + mjpResourceProvider provider = { + "prefix", open_nop, read_nop, close_nop + }; - mjpResourceProvider provider2 = {"prefix", open_nop, read_nop, close_nop, - nullptr, nullptr}; + mjpResourceProvider provider2 = { + "prefix", open_nop, read_nop, close_nop + }; int i1 = mjp_registerResourceProvider(&provider); int count1 = mjp_resourceProviderCount(); @@ -166,11 +200,13 @@ TEST_F(ResourceTest, RegisterProviderSame) { } TEST_F(ResourceTest, RegisterProviderSameCase) { - mjpResourceProvider provider = {"prefix", open_nop, read_nop, close_nop, - nullptr, nullptr}; + mjpResourceProvider provider = { + "prefix", open_nop, read_nop, close_nop + }; - mjpResourceProvider provider2 = {"PREFIX", open_nop, read_nop, close_nop, - nullptr, nullptr}; + mjpResourceProvider provider2 = { + "PREFIX", open_nop, read_nop, close_nop + }; int i1 = mjp_registerResourceProvider(&provider); int count1 = mjp_resourceProviderCount(); @@ -183,15 +219,16 @@ TEST_F(ResourceTest, RegisterProviderSameCase) { } TEST_F(ResourceTest, GeneralTest) { - mjpResourceProvider provider = {"str", open_str, read_str, close_str, - nullptr, nullptr}; + mjpResourceProvider provider = { + "str", open_str, read_str, close_str + }; // register resource provider int i = mjp_registerResourceProvider(&provider); EXPECT_GT(i, 0); // open resource - mjResource* resource = mju_openResource("str:file", 0); + mjResource* resource = mju_openResource("str:file"); ASSERT_THAT(resource, NotNull()); const char* buffer = NULL; @@ -203,8 +240,9 @@ TEST_F(ResourceTest, GeneralTest) { } TEST_F(ResourceTest, GeneralTestFailure) { - mjpResourceProvider provider = {"str", open_str, read_str, close_str, - nullptr, nullptr}; + mjpResourceProvider provider = { + "str", open_str, read_str, close_str + }; // register resource provider int i = mjp_registerResourceProvider(&provider); @@ -219,15 +257,16 @@ TEST_F(ResourceTest, GeneralTestFailure) { }; // open resource - mjResource* resource = mju_openResource("str:notfound", 0); + mjResource* resource = mju_openResource("str:notfound"); ASSERT_THAT(resource, IsNull()); EXPECT_THAT(warning, HasSubstr("could not open")); } TEST_F(ResourceTest, NameWithValidPrefix) { - mjpResourceProvider provider = {"nop", open_nop, read_nop, close_nop, - nullptr, nullptr}; + mjpResourceProvider provider = { + "nop", open_nop, read_nop, close_nop + }; // register resource provider int i = mjp_registerResourceProvider(&provider); @@ -242,14 +281,15 @@ TEST_F(ResourceTest, NameWithValidPrefix) { }; // open resource - mjResource* resource = mju_openResource("nop:found", 0); + mjResource* resource = mju_openResource("nop:found"); ASSERT_THAT(resource, NotNull()); mju_closeResource(resource); } TEST_F(ResourceTest, NameWithUpperCasePrefix) { - mjpResourceProvider provider = {"nop", open_nop, read_nop, close_nop, - nullptr, nullptr}; + mjpResourceProvider provider = { + "nop", open_nop, read_nop, close_nop, + }; // register resource provider int i = mjp_registerResourceProvider(&provider); @@ -264,14 +304,15 @@ TEST_F(ResourceTest, NameWithUpperCasePrefix) { }; // open resource - mjResource* resource = mju_openResource("NOP:found", 0); + mjResource* resource = mju_openResource("NOP:found"); ASSERT_THAT(resource, NotNull()); mju_closeResource(resource); } TEST_F(ResourceTest, NameWithInvalidPrefix) { - mjpResourceProvider provider = {"nop", open_nop, read_nop, close_nop, - nullptr, nullptr}; + mjpResourceProvider provider = { + "nop", open_nop, read_nop, close_nop + }; // register resource provider int i = mjp_registerResourceProvider(&provider); @@ -286,33 +327,9 @@ TEST_F(ResourceTest, NameWithInvalidPrefix) { }; // open resource - mjResource* resource = mju_openResource("nopfound", 0); + mjResource* resource = mju_openResource("nopfound"); ASSERT_THAT(resource, IsNull()); } -TEST_F(ResourceTest, VFSProvider) { - // load VFS on the heap - auto vfs = std::make_unique(); - mj_defaultVFS(vfs.get()); - mj_makeEmptyFileVFS(vfs.get(), "file", 1); - - // register resource provider - int i = mj_registerVfsProvider(vfs.get()); - EXPECT_GT(i, 0); - - - // install warning handler - static char warning[1024]; - warning[0] = '\0'; - mju_user_warning = [](const char* msg) { - util::strcpy_arr(warning, msg); - }; - - // open resource - mjResource* resource = mju_openResource("vfs:file", 0); - ASSERT_THAT(resource, IsNull()); - mj_deleteFileVFS(vfs.get(), "file"); -} - } // namespace } // namespace mujoco diff --git a/test/fixture.cc b/test/fixture.cc index 10432236..4c451b65 100644 --- a/test/fixture.cc +++ b/test/fixture.cc @@ -14,6 +14,7 @@ #include "test/fixture.h" +#include #include #include #include @@ -79,10 +80,11 @@ mjModel* LoadModelFromString(std::string_view xml, char* error, resource->data = &(resource->name[strlen("LoadModelFromString:")]); return 1; }; - resourceProvider.read = +[](mjResource* resource, const void** buffer) { - *buffer = resource->data; - return (int) strlen((const char*) resource->data); - }; + resourceProvider.read = + +[](mjResource* resource, const void** buffer) { + *buffer = resource->data; + return (int) strlen((const char*) resource->data); + }; resourceProvider.close = +[](mjResource* resource) {}; mjp_registerResourceProvider(&resourceProvider); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index b4f9f6a1..7b318b97 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1756,7 +1756,7 @@ public unsafe struct _mjVFS { public int nfile; [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000 * 1000)] public char[] filename; - [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public int[] filesize; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public UIntPtr[] filesize; [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public IntPtr[] filedata; } From 273c23038cae4695bcbe16fe6e4f29bfd9bb7897 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 30 Aug 2023 05:39:42 -0700 Subject: [PATCH 03/38] MakeParticle and elasticity plugin refactoring. - Removed the explicit dependency on a plugin name. - Creation of particles from user-defined vertices (if a plugin is present). - Add skin support for an irregular mesh - Fixes #642 and #674. PiperOrigin-RevId: 561313771 Change-Id: I450f91f0d3b578cc18d97ed7180a0a8944b7dd41 --- plugin/elasticity/CMakeLists.txt | 2 + plugin/elasticity/elasticity.cc | 51 ++++- plugin/elasticity/elasticity.h | 50 +++++ plugin/elasticity/register.cc | 26 +++ plugin/elasticity/solid.cc | 102 ++------- plugin/elasticity/solid.h | 6 +- src/user/user_composite.cc | 253 +++++++++++++++++----- src/user/user_composite.h | 4 + test/plugin/elasticity/elasticity_test.cc | 3 - 9 files changed, 355 insertions(+), 142 deletions(-) create mode 100644 plugin/elasticity/elasticity.h create mode 100644 plugin/elasticity/register.cc diff --git a/plugin/elasticity/CMakeLists.txt b/plugin/elasticity/CMakeLists.txt index f35cf2d3..81946e9a 100644 --- a/plugin/elasticity/CMakeLists.txt +++ b/plugin/elasticity/CMakeLists.txt @@ -20,6 +20,8 @@ set(MUJOCO_ELASTICITY_SRCS cable.cc cable.h elasticity.cc + elasticity.h + register.cc solid.cc solid.h ) diff --git a/plugin/elasticity/elasticity.cc b/plugin/elasticity/elasticity.cc index e349379f..35c0e0fc 100644 --- a/plugin/elasticity/elasticity.cc +++ b/plugin/elasticity/elasticity.cc @@ -12,15 +12,54 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include "cable.h" -#include "solid.h" +#include "elasticity.h" +#include +#include +#include +#include +#include +#include +#include +#include namespace mujoco::plugin::elasticity { -mjPLUGIN_LIB_INIT { - Cable::RegisterPlugin(); - Solid::RegisterPlugin(); +void String2Vector(const std::string& txt, std::vector& 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& len, + const std::vector >& 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 diff --git a/plugin/elasticity/elasticity.h b/plugin/elasticity/elasticity.h new file mode 100644 index 00000000..54b0fb14 --- /dev/null +++ b/plugin/elasticity/elasticity.h @@ -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 +#include +#include + +#include + +namespace mujoco::plugin::elasticity { + +struct PairHash +{ + template + std::size_t operator() (const std::pair& pair) const { + return std::hash()(pair.first) ^ std::hash()(pair.second); + } +}; + +// copied from mjXUtil +void String2Vector(const std::string& txt, std::vector& 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& len, + const std::vector >& edges, + const mjtNum* x); + +} // namespace mujoco::plugin::elasticity + +#endif // MUJOCO_PLUGIN_ELASTICITY_ELASTICITY_H_ diff --git a/plugin/elasticity/register.cc b/plugin/elasticity/register.cc new file mode 100644 index 00000000..e349379f --- /dev/null +++ b/plugin/elasticity/register.cc @@ -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 +#include "cable.h" +#include "solid.h" + +namespace mujoco::plugin::elasticity { + +mjPLUGIN_LIB_INIT { + Cable::RegisterPlugin(); + Solid::RegisterPlugin(); +} + +} // namespace mujoco::plugin::elasticity diff --git a/plugin/elasticity/solid.cc b/plugin/elasticity/solid.cc index 83777613..84130bff 100644 --- a/plugin/elasticity/solid.cc +++ b/plugin/elasticity/solid.cc @@ -13,15 +13,17 @@ // limitations under the License. #include -#include -#include -#include +#include +#include #include #include +#include +#include #include #include #include +#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& len, - const std::vector >& 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 - std::size_t operator() (const std::pair& pair) const { - return std::hash()(pair.first) ^ std::hash()(pair.second); - } -}; - } // namespace // factory function std::optional 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 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::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& 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& 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; }; diff --git a/plugin/elasticity/solid.h b/plugin/elasticity/solid.h index 96527a1d..7f4ee95d 100644 --- a/plugin/elasticity/solid.h +++ b/plugin/elasticity/solid.h @@ -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& simplex); - void CreateStencils(int nx, int ny, int nz); + void CreateStencils(const std::vector& simplex); }; } // namespace mujoco::plugin::elasticity diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 256f9a95..b44b1600 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -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; @@ -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; ixAddBody(NULL); - mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz); - b->name = txt; + char txt[100]; + std::vector 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; ixAddJoint(&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 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; jAddBody(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,6 +2153,44 @@ void mjCComposite::MakeSkin3(mjCModel* model) { skin->inflate = skininflate; skin->group = skingroup; + // copy skin from existing mesh + if (type==mjCOMPTYPE_PARTICLE && username.empty()) { + std::vector face; + mjXUtil::String2Vector(userface, face); + int nvert = uservert.size()/3; + + for (int j=0; j<2; j++) { + for (int i=0; ivert.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 vertid; + vector 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; iface.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) { // z-faces diff --git a/src/user/user_composite.h b/src/user/user_composite.h index af6e6807..0d60b97e 100644 --- a/src/user/user_composite.h +++ b/src/user/user_composite.h @@ -106,9 +106,13 @@ class mjCComposite { // currently used only for cable std::string initial; // root boundary type std::vector 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 username; + // plugin support bool is_plugin; std::string plugin_name; diff --git a/test/plugin/elasticity/elasticity_test.cc b/test/plugin/elasticity/elasticity_test.cc index d6a03d2e..d2e36025 100644 --- a/test/plugin/elasticity/elasticity_test.cc +++ b/test/plugin/elasticity/elasticity_test.cc @@ -41,9 +41,6 @@ TEST_F(PluginTest, ElasticEnergy) { - - - From 16ee0dfde545a51e809d2793a8de6bce390700b8 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Wed, 30 Aug 2023 06:52:36 -0700 Subject: [PATCH 04/38] Add modified callback to resource providers (for caching purposes). PiperOrigin-RevId: 561327623 Change-Id: Id6683f6d6d32ee91019aba46599c28cbffe29df6 --- doc/includes/references.h | 1 + doc/programming/extension.rst | 9 ++++-- include/mujoco/mjplugin.h | 6 ++++ src/engine/engine_plugin.cc | 1 + src/engine/engine_resource.c | 53 +++++++++++++++++++++++++++++++++++ src/engine/engine_vfs.c | 1 + 6 files changed, 68 insertions(+), 3 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 35aaa77c..d6223101 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1259,6 +1259,7 @@ struct mjpResourceProvider { 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; diff --git a/doc/programming/extension.rst b/doc/programming/extension.rst index e5e22846..7ab8f7e2 100644 --- a/doc/programming/extension.rst +++ b/doc/programming/extension.rst @@ -330,8 +330,9 @@ Resource prefix Callbacks There are three callbacks that a resource provider is required to implement: :ref:`open`, - :ref:`read`, and :ref:`close`. A fourth callback :ref:`getdir` - is optional. More details on these callbacks are given below. + :ref:`read`, and :ref:`close`. The other two callback + :ref:`getdir` and :ref:`modified` 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`: 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:`mjfModifiedResource`: 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: - + ... diff --git a/include/mujoco/mjplugin.h b/include/mujoco/mjplugin.h index aa23bc44..2c071195 100644 --- a/include/mujoco/mjplugin.h +++ b/include/mujoco/mjplugin.h @@ -43,6 +43,11 @@ 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 @@ -50,6 +55,7 @@ struct mjpResourceProvider { 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; diff --git a/src/engine/engine_plugin.cc b/src/engine/engine_plugin.cc index 52e86455..50b93d61 100644 --- a/src/engine/engine_plugin.cc +++ b/src/engine/engine_plugin.cc @@ -269,6 +269,7 @@ bool ResourceProvidersAreIdentical(const mjpResourceProvider* p1, const mjpResou p1->read == p2->read && p1->close == p2->close && p1->getdir == p2->getdir && + p1->modified == p2->modified && p1->data == p2->data); } diff --git a/src/engine/engine_resource.c b/src/engine/engine_resource.c index bd7280fa..4718df50 100644 --- a/src/engine/engine_resource.c +++ b/src/engine/engine_resource.c @@ -19,6 +19,17 @@ #include #include #include +#include +#include +#include + +#if defined (__unix__) || (defined (__APPLE__) && defined (__MACH__)) + #include +#endif + +#ifdef _WIN32 + #define stat _stat +#endif #include #include "engine/engine_plugin.h" @@ -28,6 +39,7 @@ typedef struct { uint8_t* buffer; // raw bytes from file size_t nbuffer; // size of buffer in bytes + time_t mtime; // last modified time } file_buffer; // open the given resource; if the name doesn't have a prefix matching with a @@ -77,6 +89,13 @@ mjResource* mju_openResource(const char* 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; } @@ -149,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) { diff --git a/src/engine/engine_vfs.c b/src/engine/engine_vfs.c index 0535111e..26db392e 100644 --- a/src/engine/engine_vfs.c +++ b/src/engine/engine_vfs.c @@ -274,6 +274,7 @@ mjResource* mju_openVfsResource(const char* name, const mjVFS* vfs) { .read = &vfs_read_callback, .close = &vfs_close_callback, .getdir = &vfs_getdir_callback, + .modified = NULL }; // create resource From f371ca7a4e64e112966444de33d4afc71626e9a5 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 30 Aug 2023 07:07:21 -0700 Subject: [PATCH 05/38] Add missing `face` attribute to the XML schema. PiperOrigin-RevId: 561330536 Change-Id: I693fc5228591e4a26ea0749435a6ce9869afd457 --- doc/XMLreference.rst | 5 +++++ doc/XMLschema.rst | 4 +++- src/user/user_composite.cc | 4 ++-- src/xml/xml_native_reader.cc | 7 +++++-- 4 files changed, 15 insertions(+), 5 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 62c0c191..6ce1ce42 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3319,6 +3319,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"` diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index ee7ad48c..2210168f 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -390,7 +390,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`offset` | :ref:`flatinertia` | :ref:`solrefsmooth` | :ref:`solimpsmooth` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`vertex` | :ref:`initial` | :ref:`curve` | :ref:`size` | | +| | | | :ref:`vertex` | :ref:`face` | :ref:`initial` | :ref:`curve` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`size` | | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| composite |br| |_2| |L| | | .. table:: | diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index b44b1600..7a8558c6 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -236,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", @@ -2192,7 +2192,7 @@ void mjCComposite::MakeSkin3(mjCModel* model) { } // 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); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 3f04b6e5..3026dfb5 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -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"}, @@ -1943,6 +1943,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; From ac150f751d28ce22a5f99c8d3555d9a997d9530b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 30 Aug 2023 11:46:10 -0700 Subject: [PATCH 06/38] Clean up mj_mulM. PiperOrigin-RevId: 561404770 Change-Id: Ie802f5d7f1df6bf32d4549d3db6809d0f507d1e1 --- src/engine/engine_support.c | 51 ++++++++++++++++++------------------- 1 file changed, 25 insertions(+), 26 deletions(-) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index e959ac00..7e57a21a 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -827,25 +827,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 +858,26 @@ 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]; } } } From 43a0bd36461319a81d4308999ca2efece7450c42 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 31 Aug 2023 01:42:55 -0700 Subject: [PATCH 07/38] Formatting improvements to docs Overview chapter. PiperOrigin-RevId: 561586361 Change-Id: Ib529cbd49cebf26d802d023d12d973491e41d3ad --- doc/changelog.rst | 8 +- doc/overview.rst | 401 ++++++++++++++++++++++++++-------------------- 2 files changed, 231 insertions(+), 178 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index a77cf944..cebd8ce5 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -38,11 +38,11 @@ General Euler integrator. See the :ref:`Numerical Integration` section for more details. #. Added the flag :ref:`invdiscrete`, which enables discrete-time inverse dynamics for all :ref:`integrators` other than ``RK4``. See the flag documentation for more details. -#. Changed the function ``mj_stackAlloc`` to allocate an arbitrary number of bytes, rather than in multiples of +#. Changed the function :ref:`mj_stackAlloc` to allocate an arbitrary number of bytes, rather than in multiples of ``sizeof(mjtNum)``, and add an additional argument for specifying the alignment of the returned pointer. The existing - functionality of allocating ``mjtNum`` arrays is still available through the new function ``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. + functionality of allocating ``mjtNum`` arrays is still available through the new function :ref:`mj_stackAllocNum`. +#. Renamed the ``nstack`` field in :ref:`mjModel` and :ref:`mjData` to ``narena``. Changed ``narena``, ``pstack``, and + ``maxuse_stack`` to count number of bytes rather than number of :ref:`mjtNum` |-| s. Python bindings ^^^^^^^^^^^^^^^ diff --git a/doc/overview.rst b/doc/overview.rst index c011f47e..583556ce 100644 --- a/doc/overview.rst +++ b/doc/overview.rst @@ -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 - `__ 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 +`__ 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 `. 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 `__ 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 `__ 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 `. 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 `. +^^^^^^ + +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 `. 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: From ba5cf4d54d63b815f7e70ee21bc318c3a47a866d Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Thu, 31 Aug 2023 03:24:01 -0700 Subject: [PATCH 08/38] Address -Wint-conversion issues. PiperOrigin-RevId: 561604684 Change-Id: I31af8621f8506d2e029c4ca630982f82ea4cfca9 --- src/engine/engine_io.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 159ab555..9ba2674d 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1272,12 +1272,12 @@ 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 From 129abc6bca232d204ec8ae835befcdd2c64aac18 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 31 Aug 2023 05:10:14 -0700 Subject: [PATCH 09/38] Add private function `mj_solveM_island` for mass matrix solve with sub indices corresponding to one island. PiperOrigin-RevId: 561622962 Change-Id: Ib43f03b3dd0c7456c57d4ba45a68e0708bf1c91c --- src/engine/engine_core_smooth.c | 62 +++++++++++++++++++++++++- src/engine/engine_core_smooth.h | 3 ++ test/engine/engine_core_smooth_test.cc | 59 ++++++++++++++++++++++++ 3 files changed, 123 insertions(+), 1 deletion(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 9b4affe7..b8a667d6 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -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) { diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index edaa4133..48776db5 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -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); diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 7f672263..6d123d1a 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -14,8 +14,11 @@ // Tests for engine/engine_core_smooth.c. +#include "src/engine/engine_core_smooth.h" + #include #include +#include #include #include @@ -291,5 +294,61 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) { mj_deleteModel(model); } +static const char* const kIlslandEfcPath = + "engine/testdata/island/island_efc.xml"; + +TEST_F(CoreSmoothTest, SolveMIsland) { + const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + mjData* data = mj_makeData(model); + int nv = model->nv; + + // allocate vec, fill with arbitrary values, copy to sol + mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum) * nv); + mjtNum* res = (mjtNum*) mju_malloc(sizeof(mjtNum) * nv); + for (int i=0; i < nv; i++) { + vec[i] = 0.2 + 0.3*i; + } + mju_copy(res, vec, nv); + + // simulate for 0.3 seconds + mj_resetData(model, data); + while (data->time < 0.3) { + mj_step(model, data); + } + mj_forward(model, data); + + // divide by mass matrix: sol = M^-1 * vec + mj_solveM(model, data, res, res, 1); + + // iterate over islands + for (int i=0; i < data->nisland; i++) { + // allocate dof vectors for island + int dofnum = data->island_dofnum[i]; + mjtNum* res_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum); + + // copy values into sol_i + int* dofind = data->island_dofind + data->island_dofadr[i]; + for (int j=0; j < dofnum; j++) { + res_i[j] = vec[dofind[j]]; + } + + // divide by mass matrix, for this island + mj_solveM_island(model, data, res_i, i); + + // expect corresponding values to match + for (int j=0; j < dofnum; j++) { + EXPECT_THAT(res_i[j], DoubleNear(res[dofind[j]], 1e-12)); + } + + mju_free(res_i); + } + + mju_free(res); + mju_free(vec); + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco From 9fd186ac2bce133eb042fb9a6c116d26a7ca8276 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 31 Aug 2023 06:41:55 -0700 Subject: [PATCH 10/38] Add private function `mj_mulM_island` for mass matrix multiplication with sub indices corresponding to one island. PiperOrigin-RevId: 561639405 Change-Id: I3777cea51ef22f6847b3cf3e17bdac5cf621c019 --- src/engine/engine_support.c | 52 +++++++++++++++++++++++++++ src/engine/engine_support.h | 4 +++ test/engine/engine_support_test.cc | 58 ++++++++++++++++++++++++++++++ 3 files changed, 114 insertions(+) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 7e57a21a..c58e5e06 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -884,6 +884,58 @@ void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) +// 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) { + // 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 + 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]; + res[k] += M[adr]*vec[l]; + res[l] += M[adr]*vec[k]; + + // advance to parent + j = parentid[j]; + } + } +} + + + // multiply vector by M^(1/2) void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) { int adr, nv = m->nv; diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index ce601a06..3b10fbe7 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -108,6 +108,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); + // multiply vector by (inertia matrix)^(1/2) MJAPI void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec); diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index b2a1a59a..a33b3f28 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -14,6 +14,8 @@ // Tests for engine/engine_support.c. +#include "src/engine/engine_support.h" + #include #include #include @@ -453,5 +455,61 @@ TEST_F(AddMTest, DenseSameAsSparse) { mj_deleteModel(m); } +static const char* const kIlslandEfcPath = + "engine/testdata/island/island_efc.xml"; + +TEST_F(SupportTest, MulMIsland) { + const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + mjData* data = mj_makeData(model); + + // allocate vec, fill with arbitrary values + mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nv); + for (int i=0; i < model->nv; i++) { + vec[i] = 0.2 + 0.3*i; + } + + // simulate for 0.3 seconds + mj_resetData(model, data); + while (data->time < 0.3) { + mj_step(model, data); + } + mj_forward(model, data); + + // multiply by Mass matrix: Mvec = M * vec + mjtNum* Mvec = (mjtNum*) mju_malloc(sizeof(mjtNum)*data->nefc); + mj_mulM(model, data, Mvec, vec); + + // iterate over islands + for (int i=0; i < data->nisland; i++) { + // allocate dof vectors for island + int dofnum = data->island_dofnum[i]; + mjtNum* vec_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum); + mjtNum* Mvec_i = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum); + + // copy values into vec_i + int* dofind = data->island_dofind + data->island_dofadr[i]; + for (int j=0; j < dofnum; j++) { + vec_i[j] = vec[dofind[j]]; + } + + // multiply by Jacobian, for this island + mj_mulM_island(model, data, Mvec_i, vec_i, i); + + // expect corresponding values to match + for (int j=0; j < dofnum; j++) { + EXPECT_THAT(Mvec_i[j], DoubleNear(Mvec[dofind[j]], 1e-12)); + } + + mju_free(vec_i); + mju_free(Mvec_i); + } + + mju_free(Mvec); + mju_free(vec); + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco From 85fd922b376081345daa5e2eb1a11a7866542f0e Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 31 Aug 2023 07:27:34 -0700 Subject: [PATCH 11/38] Add private function `mj_mulJacVec_island` for Jacobian multiplication with sub indices corresponding to one island. PiperOrigin-RevId: 561648540 Change-Id: I8080a620807e824b40b5d6ba484beb06d081d444 --- src/engine/engine_core_constraint.c | 39 +++++++++++++ src/engine/engine_core_constraint.h | 4 ++ test/engine/engine_core_constraint_test.cc | 65 +++++++++++++++++++++- 3 files changed, 107 insertions(+), 1 deletion(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 0de536ad..34b2892e 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -395,6 +395,45 @@ void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec) { +// multiply Jacobian by vector, for one island +void mj_mulJacVec_island(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, int island) { + // 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; + res[i] = mju_dotSparse2(vec, J, vecnnz, vecind, Jnnz, Jind); + } + } + + // dense Jacobian + else { + int nv = m->nv; + for (int i=0; i < resnnz; i++) { + res[i] = mju_dotSparse(vec, d->efc_J + nv*resind[i], vecnnz, vecind); + } + } +} + + + // multiply JacobianT by vector void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec) { // exit if no constraints diff --git a/src/engine/engine_core_constraint.h b/src/engine/engine_core_constraint.h index f90d1322..afa3447c 100644 --- a/src/engine/engine_core_constraint.h +++ b/src/engine/engine_core_constraint.h @@ -38,6 +38,10 @@ 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); +// multiply Jacobian by vector, for one island +MJAPI void mj_mulJacVec_island(const mjModel* m, mjData* d, + mjtNum* res, const mjtNum* vec, int island); + // multiply JacobianT by vector MJAPI void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec); diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 8022f163..af2df04c 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -14,7 +14,6 @@ // Tests for engine/engine_core_constraint.c. -#include #include #include #include @@ -198,5 +197,69 @@ TEST_F(CoreConstraintTest, JacobianPreAllocate) { } } +static const char* const kIlslandEfcPath = + "engine/testdata/island/island_efc.xml"; + +TEST_F(CoreConstraintTest, MulJacVecIsland) { + const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + mjData* data = mj_makeData(model); + + // allocate vec_nv, fill with arbitrary values + mjtNum* vec_nv = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nv); + for (int i=0; i < model->nv; i++) { + vec_nv[i] = 0.2 + 0.3*i; + } + + // iterate through dense and sparse + for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) { + model->opt.jacobian = sparsity; + + // simulate for 0.3 seconds + mj_resetData(model, data); + while (data->time < 0.3) { + mj_step(model, data); + } + mj_forward(model, data); + + // multiply by Jacobian: vec_nefc = J * vec_nv + mjtNum* vec_nefc = (mjtNum*) mju_malloc(sizeof(mjtNum)*data->nefc); + mj_mulJacVec(model, data, vec_nefc, vec_nv); + + // iterate over islands + for (int i=0; i < data->nisland; i++) { + // allocate dof and efc vectors for island + int dofnum = data->island_dofnum[i]; + mjtNum* vec_nvi = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum); + int efcnum = data->island_efcnum[i]; + mjtNum* vec_nefci = (mjtNum*)mju_malloc(sizeof(mjtNum) * efcnum); + + // copy values into vec_nvi + int* dofind = data->island_dofind + data->island_dofadr[i]; + for (int j=0; j < dofnum; j++) { + vec_nvi[j] = vec_nv[dofind[j]]; + } + + // multiply by Jacobian, for this island + mj_mulJacVec_island(model, data, vec_nefci, vec_nvi, i); + + // expect corresponding values to match + int* efcind = data->island_efcind + data->island_efcadr[i]; + for (int j=0; j < efcnum; j++) { + EXPECT_THAT(vec_nefci[j], DoubleNear(vec_nefc[efcind[j]], 1e-12)); + } + + mju_free(vec_nvi); + mju_free(vec_nefci); + } + + mju_free(vec_nefc); + } + + mju_free(vec_nv); + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco From eaa2572a8b87e96f74b7bd9a5523eff0524ad274 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 31 Aug 2023 09:11:47 -0700 Subject: [PATCH 12/38] Add private function `mj_mulJacTVec_island` for Jacobian transpose multiplication with sub indices corresponding to one island. PiperOrigin-RevId: 561672392 Change-Id: Ic72fd7de2a542603cc42e1c15ecb4edd27a9a986 --- src/engine/engine_core_constraint.c | 43 +++++++++++++++ src/engine/engine_core_constraint.h | 4 ++ test/engine/engine_core_constraint_test.cc | 62 ++++++++++++++++++++++ 3 files changed, 109 insertions(+) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 34b2892e..d0d14caa 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -455,6 +455,45 @@ void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec) +// multiply Jacobian transpose by vector, for one island +void mj_mulJacTVec_island(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, int island) { + // 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; + res[i] = mju_dotSparse2(vec, JT, vecnnz, vecind, JTnnz, JTind); + } + } + + // dense Jacobian + else { + int nefc = d->nefc; + for (int i=0; i < resnnz; i++) { + res[i] = mju_dotSparse(vec, d->efc_JT + nefc*resind[i], vecnnz, vecind); + } + } +} + + + //--------------------- instantiate constraints by type -------------------------------------------- // equality constraints @@ -1772,6 +1811,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 diff --git a/src/engine/engine_core_constraint.h b/src/engine/engine_core_constraint.h index afa3447c..3104a339 100644 --- a/src/engine/engine_core_constraint.h +++ b/src/engine/engine_core_constraint.h @@ -42,6 +42,10 @@ MJAPI void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* MJAPI void mj_mulJacVec_island(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, int island); +// multiply Jacobian transposed by vector, for one island +MJAPI void mj_mulJacTVec_island(const mjModel* m, mjData* d, + mjtNum* res, const mjtNum* vec, int island); + // multiply JacobianT by vector MJAPI void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec); diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index af2df04c..a0c2a477 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -261,5 +261,67 @@ TEST_F(CoreConstraintTest, MulJacVecIsland) { mj_deleteModel(model); } +TEST_F(CoreConstraintTest, MulJacTVecIsland) { + const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + mjData* data = mj_makeData(model); + + // allocate vec_nv + mjtNum* vec_nv = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nv); + + // iterate through dense and sparse + for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) { + model->opt.jacobian = sparsity; + + // simulate for 0.3 seconds + mj_resetData(model, data); + while (data->time < 0.3) { + mj_step(model, data); + } + mj_forward(model, data); + + // allocate vec_nefc, fill with arbitrary values + mjtNum* vec_nefc = (mjtNum*) mju_malloc(sizeof(mjtNum)*data->nefc); + for (int i=0; i < data->nefc; i++) { + vec_nefc[i] = 0.2 + 0.3*i; + } + + // multiply by Jacobian: vec_nv = J^T * vec_nefc + mj_mulJacTVec(model, data, vec_nv, vec_nefc); + + // iterate over islands + for (int i=0; i < data->nisland; i++) { + // allocate dof and efc vectors for island + int dofnum = data->island_dofnum[i]; + mjtNum* vec_nvi = (mjtNum*)mju_malloc(sizeof(mjtNum) * dofnum); + int efcnum = data->island_efcnum[i]; + mjtNum* vec_nefci = (mjtNum*)mju_malloc(sizeof(mjtNum) * efcnum); + + // copy values into vec_nefci + int* efcind = data->island_efcind + data->island_efcadr[i]; + for (int j=0; j < efcnum; j++) { + vec_nefci[j] = vec_nefc[efcind[j]]; + } + + // multiply by Jacobian, for this island + mj_mulJacTVec_island(model, data, vec_nvi, vec_nefci, i); + + // expect corresponding values to match + int* dofind = data->island_dofind + data->island_dofadr[i]; + for (int j=0; j < dofnum; j++) { + EXPECT_THAT(vec_nvi[j], DoubleNear(vec_nv[dofind[j]], 1e-12)); + } + + mju_free(vec_nvi); + mju_free(vec_nefci); + } + mju_free(vec_nefc); + } + + mju_free(vec_nv); + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco From f9c57f6213246f7c6fbf0952306e2d19e12b1e74 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 31 Aug 2023 09:58:41 -0700 Subject: [PATCH 13/38] Refactor a loop in mj_constraintUpdate. PiperOrigin-RevId: 561684763 Change-Id: Ifbca1793760ecb49f4b48a1a236bd7c1c6e73d0e --- src/engine/engine_core_constraint.c | 81 +++++++++++++++-------------- 1 file changed, 42 insertions(+), 39 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index d0d14caa..52ebec3b 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -2021,51 +2021,54 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar, force[i] = -D[i]*jar[i]; } - // equality - for (int i=0; i < ne; i++) { - if (cost) { - s += 0.5*D[i]*jar[i]*jar[i]; - } - - d->efc_state[i] = mjCNSTRSTATE_QUADRATIC; - } - - // friction - for (int i=ne; i < ne+nf; i++) { - // linear negative - 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_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 { + // update constraints + for (int i=0; i < nefc; i++) { + // ==== equality + if (i < ne) { 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[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_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 + // non-negative constraint if (d->efc_type[i] != mjCNSTR_CONTACT_ELLIPTIC) { // constraint is satisfied: no cost From 02e819e76a505857cf79cca9ec4183489ceb8341 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 31 Aug 2023 11:26:30 -0700 Subject: [PATCH 14/38] Tighten const qualifiers of `mjData` in Jacobian multiplication functions. PiperOrigin-RevId: 561711229 Change-Id: I66dc1d2fbcfbf8e935b244fe496ada314d53b135 --- doc/includes/references.h | 4 ++-- include/mujoco/mujoco.h | 4 ++-- introspect/functions.py | 4 ++-- src/engine/engine_core_constraint.c | 10 ++++++---- src/engine/engine_core_constraint.h | 13 ++++++------- 5 files changed, 18 insertions(+), 17 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index d6223101..e6f81619 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2268,8 +2268,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); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 19c37f67..c289b78a 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -386,10 +386,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, diff --git a/introspect/functions.py b/introspect/functions.py index a5bbf038..a8db3f87 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -1987,7 +1987,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 +2019,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( diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 52ebec3b..42b4ebf8 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -375,7 +375,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; @@ -396,7 +396,8 @@ void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec) { // multiply Jacobian by vector, for one island -void mj_mulJacVec_island(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, int island) { +void mj_mulJacVec_island(const mjModel* m, const mjData* d, + mjtNum* res, const mjtNum* vec, int island) { // no island, call regular function if (island < 0) { mj_mulJacVec(m, d, res, vec); @@ -435,7 +436,7 @@ void mj_mulJacVec_island(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* // 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; @@ -456,7 +457,8 @@ void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec) // multiply Jacobian transpose by vector, for one island -void mj_mulJacTVec_island(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, int island) { +void mj_mulJacTVec_island(const mjModel* m, const mjData* d, + mjtNum* res, const mjtNum* vec, int island) { // no island, call regular function if (island < 0) { mj_mulJacTVec(m, d, res, vec); diff --git a/src/engine/engine_core_constraint.h b/src/engine/engine_core_constraint.h index 3104a339..2dcc42fb 100644 --- a/src/engine/engine_core_constraint.h +++ b/src/engine/engine_core_constraint.h @@ -36,19 +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, mjData* d, +MJAPI void mj_mulJacVec_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec, int island); -// multiply Jacobian transposed by vector, for one island -MJAPI void mj_mulJacTVec_island(const mjModel* m, mjData* d, - mjtNum* res, const mjtNum* vec, int island); - // 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); //-------------------------- utility functions ----------------------------------------------------- From 5279ce6f6f3168a2444a68ff5d8f3d5c3bdb3782 Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Fri, 1 Sep 2023 04:55:27 -0700 Subject: [PATCH 15/38] Fix comment typo in simulate.cc PiperOrigin-RevId: 561921946 Change-Id: I423378df566be7a6ad88fb3526769d298ac32a3b --- simulate/simulate.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 77edbf1a..404004dd 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -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; ifigtimer.linedata[n][2*i] = -i; sim->figsize.linedata[n][2*i] = -i; } + } } // update profiler figures @@ -2237,7 +2238,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()); From 70c5fa50f21ada3f9a1bdc57df084cde966359ba Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 1 Sep 2023 05:51:26 -0700 Subject: [PATCH 16/38] Improve island-constraint testing model. PiperOrigin-RevId: 561931744 Change-Id: I2826d4c5b987cc6edd761026f6734fe40fb31278 --- test/engine/engine_core_constraint_test.cc | 4 +-- test/engine/engine_core_smooth_test.cc | 4 +-- test/engine/engine_island_test.cc | 28 +++++++++++---------- test/engine/engine_support_test.cc | 4 +-- test/engine/testdata/island/island_efc.xml | 29 ++++++++++++++++++---- 5 files changed, 45 insertions(+), 24 deletions(-) diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index a0c2a477..fee8ba7d 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -215,9 +215,9 @@ TEST_F(CoreConstraintTest, MulJacVecIsland) { for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) { model->opt.jacobian = sparsity; - // simulate for 0.3 seconds + // simulate for 0.2 seconds mj_resetData(model, data); - while (data->time < 0.3) { + while (data->time < 0.2) { mj_step(model, data); } mj_forward(model, data); diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 6d123d1a..52f95e50 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -311,9 +311,9 @@ TEST_F(CoreSmoothTest, SolveMIsland) { } mju_copy(res, vec, nv); - // simulate for 0.3 seconds + // simulate for 0.2 seconds mj_resetData(model, data); - while (data->time < 0.3) { + while (data->time < 0.2) { mj_step(model, data); } mj_forward(model, data); diff --git a/test/engine/engine_island_test.cc b/test/engine/engine_island_test.cc index 1c631254..2e74bfe3 100644 --- a/test/engine/engine_island_test.cc +++ b/test/engine/engine_island_test.cc @@ -346,7 +346,7 @@ TEST_F(IslandTest, IslandEfc) { mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); mjData* data = mj_makeData(model); - while (data->time < 0.3) { + while (data->time < 0.2) { mj_step(model, data); } @@ -356,25 +356,27 @@ TEST_F(IslandTest, IslandEfc) { int nisland = data->nisland; // expect island structure to correspond to comment at top of xml - EXPECT_EQ(nisland, 2); + EXPECT_EQ(nisland, 3); EXPECT_EQ(data->ne, 1); - EXPECT_EQ(data->nf, 1); + EXPECT_EQ(data->nf, 2); EXPECT_EQ(data->nl, 1); - EXPECT_EQ(nefc, 7); + EXPECT_EQ(nefc, 24); EXPECT_THAT(AsVector(data->dof_island, nv), - ElementsAre(0, -1, 0, 0, 0, 1, 0)); - EXPECT_THAT(AsVector(data->island_dofnum, nisland), ElementsAre(5, 1)); - EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 5)); + ElementsAre(0, -1, 0, 0, 0, 1, 2, 0, 1, 1, 1, 1, 1, 1)); + EXPECT_THAT(AsVector(data->island_dofnum, nisland), ElementsAre(5, 7, 1)); + EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 5, 12)); EXPECT_THAT(AsVector(data->island_dofind, nv), - ElementsAre(0, 2, 3, 4, 6, 5, -1)); + ElementsAre(0, 2, 3, 4, 7, 5, 8, 9, 10, 11, 12, 13, 6, -1)); EXPECT_THAT(AsVector(data->dof_islandind, nv), - ElementsAre(0, -1, 1, 2, 3, 0, 4)); + ElementsAre(0, -1, 1, 2, 3, 0, 0, 4, 1, 2, 3, 4, 5, 6)); EXPECT_THAT(AsVector(data->efc_island, nefc), - ElementsAre(0, 1, 0, 0, 0, 0, 0)); - EXPECT_THAT(AsVector(data->island_efcnum, nisland), ElementsAre(6, 1)); - EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 6)); + ElementsAre(0, 1, 2, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1)); + EXPECT_THAT(AsVector(data->island_efcnum, nisland), ElementsAre(6, 17, 1)); + EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 6, 23)); EXPECT_THAT(AsVector(data->island_efcind, nefc), - ElementsAre(0, 2, 3, 4, 5, 6, 1)); + ElementsAre(0, 3, 4, 5, 6, 7, 1, 8, 9, 10, 11, 12, 13, 14, 15, 16, + 17, 18, 19, 20, 21, 22, 23, 2)); mj_deleteData(data); mj_deleteModel(model); diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index a33b3f28..1d7b8511 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -469,9 +469,9 @@ TEST_F(SupportTest, MulMIsland) { vec[i] = 0.2 + 0.3*i; } - // simulate for 0.3 seconds + // simulate for 0.2 seconds mj_resetData(model, data); - while (data->time < 0.3) { + while (data->time < 0.2) { mj_step(model, data); } mj_forward(model, data); diff --git a/test/engine/testdata/island/island_efc.xml b/test/engine/testdata/island/island_efc.xml index 0be5e69e..3f30d9ec 100644 --- a/test/engine/testdata/island/island_efc.xml +++ b/test/engine/testdata/island/island_efc.xml @@ -1,10 +1,10 @@ @@ -16,15 +16,22 @@ + + + + + + + @@ -37,14 +44,26 @@ + + + + + + + + + + + + From 29aa5e4a4115e9deae3db1425cb42deb248dd26c Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 1 Sep 2023 07:39:27 -0700 Subject: [PATCH 17/38] Allow dot products with sparse vectors to specify that one vector uses uncompressed memory. PiperOrigin-RevId: 561951337 Change-Id: I1310d7c09d9a85ad9b43877155844a9d0ce6edab --- src/engine/engine_core_constraint.c | 8 ++-- src/engine/engine_solver.c | 3 +- src/engine/engine_util_solve.c | 2 +- src/engine/engine_util_sparse.c | 47 ++++++++++++++----- src/engine/engine_util_sparse.h | 12 ++--- src/engine/engine_util_sparse_avx.h | 62 +++++++++++++++++++------- test/engine/engine_util_sparse_test.cc | 59 +++++++++++++++++++----- 7 files changed, 143 insertions(+), 50 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 42b4ebf8..c96a67a7 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -420,7 +420,7 @@ void mj_mulJacVec_island(const mjModel* m, const mjData* d, int Jrowadr = d->efc_J_rowadr[row]; int* Jind = d->efc_J_colind + Jrowadr; mjtNum* J = d->efc_J + Jrowadr; - res[i] = mju_dotSparse2(vec, J, vecnnz, vecind, Jnnz, Jind); + res[i] = mju_dotSparse2(vec, J, vecnnz, vecind, Jnnz, Jind, /*flg_unc2=*/0); } } @@ -428,7 +428,7 @@ void mj_mulJacVec_island(const mjModel* m, const mjData* d, else { int nv = m->nv; for (int i=0; i < resnnz; i++) { - res[i] = mju_dotSparse(vec, d->efc_J + nv*resind[i], vecnnz, vecind); + res[i] = mju_dotSparse(vec, d->efc_J + nv*resind[i], vecnnz, vecind, /*flg_unc1=*/0); } } } @@ -481,7 +481,7 @@ void mj_mulJacTVec_island(const mjModel* m, const mjData* d, int JTrowadr = d->efc_JT_rowadr[row]; int* JTind = d->efc_JT_colind + JTrowadr; mjtNum* JT = d->efc_JT + JTrowadr; - res[i] = mju_dotSparse2(vec, JT, vecnnz, vecind, JTnnz, JTind); + res[i] = mju_dotSparse2(vec, JT, vecnnz, vecind, JTnnz, JTind, /*flg_unc2=*/0); } } @@ -489,7 +489,7 @@ void mj_mulJacTVec_island(const mjModel* m, const mjData* d, else { int nefc = d->nefc; for (int i=0; i < resnnz; i++) { - res[i] = mju_dotSparse(vec, d->efc_JT + nefc*resind[i], vecnnz, vecind); + res[i] = mju_dotSparse(vec, d->efc_JT + nefc*resind[i], vecnnz, vecind, /*flg_unc1=*/0); } } } diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 2884616d..dd058a04 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -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); } } diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 4fdc82a9..933b2d90 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -235,7 +235,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; j0) { - 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); } } } diff --git a/test/engine/engine_util_sparse_test.cc b/test/engine/engine_util_sparse_test.cc index 668fda6f..495a45c9 100644 --- a/test/engine/engine_util_sparse_test.cc +++ b/test/engine/engine_util_sparse_test.cc @@ -30,19 +30,58 @@ using ::testing::ElementsAre; using EngineUtilSparseTest = MujocoTest; TEST_F(EngineUtilSparseTest, MjuDot) { - mjtNum a[] = {1, 2, 3, 4, 5, 6, 7}; - mjtNum b[] = {7, 0, 6, 0, 0, 5, 0, 0, 0, 4, 0, 0, 0, 3, 0, 0, 2, 0, 1}; + mjtNum a[] = {2, 3, 4, 5, 6, 7, 8}; + mjtNum u[] = {2, 1, 3, 1, 1, 4, 1, 1, 1, 5, 1, 1, 1, 6, 1, 1, 7, 1, 8}; + mjtNum b[] = {8, 1, 7, 1, 1, 6, 1, 1, 1, 5, 1, 1, 1, 4, 1, 1, 3, 1, 2}; int i[] = {0, 2, 5, 9, 13, 16, 18}; // test various vector lengths as mju_dotSparse adds numbers in groups of four - EXPECT_EQ(mju_dotSparse(a, b, 0, i), 0); - EXPECT_EQ(mju_dotSparse(a, b, 1, i), 7); - EXPECT_EQ(mju_dotSparse(a, b, 2, i), 7 + 2*6); - EXPECT_EQ(mju_dotSparse(a, b, 3, i), 7 + 2*6 + 3*5); - EXPECT_EQ(mju_dotSparse(a, b, 4, i), 7 + 2*6 + 3*5 + 4*4); - EXPECT_EQ(mju_dotSparse(a, b, 5, i), 7 + 2*6 + 3*5 + 4*4 + 5*3); - EXPECT_EQ(mju_dotSparse(a, b, 6, i), 7 + 2*6 + 3*5 + 4*4 + 5*3 + 6*2); - EXPECT_EQ(mju_dotSparse(a, b, 7, i), 7 + 2*6 + 3*5 + 4*4 + 5*3 + 6*2 + 7); + + // a is compressed + int flg_unc1 = 0; + EXPECT_EQ(mju_dotSparse(a, b, 0, i, flg_unc1), 0); + EXPECT_EQ(mju_dotSparse(a, b, 1, i, flg_unc1), 2*8); + EXPECT_EQ(mju_dotSparse(a, b, 2, i, flg_unc1), 2*8 + 3*7); + EXPECT_EQ(mju_dotSparse(a, b, 3, i, flg_unc1), 2*8 + 3*7 + 4*6); + EXPECT_EQ(mju_dotSparse(a, b, 4, i, flg_unc1), 2*8 + 3*7 + 4*6 + 5*5); + EXPECT_EQ(mju_dotSparse(a, b, 5, i, flg_unc1), 2*8 + 3*7 + 4*6 + 5*5 + 6*4); + EXPECT_EQ(mju_dotSparse(a, b, 6, i, flg_unc1), + 2*8 + 3*7 + 4*6 + 5*5 + 6*4 + 7*3); + EXPECT_EQ(mju_dotSparse(a, b, 7, i, flg_unc1), + 2*8 + 3*7 + 4*6 + 5*5 + 6*4 + 7*3 + 8*2); + + // u is compressed + flg_unc1 = 1; + EXPECT_EQ(mju_dotSparse(u, b, 0, i, flg_unc1), 0); + EXPECT_EQ(mju_dotSparse(u, b, 1, i, flg_unc1), 2*8); + EXPECT_EQ(mju_dotSparse(u, b, 2, i, flg_unc1), 2*8 + 3*7); + EXPECT_EQ(mju_dotSparse(u, b, 3, i, flg_unc1), 2*8 + 3*7 + 4*6); + EXPECT_EQ(mju_dotSparse(u, b, 4, i, flg_unc1), 2*8 + 3*7 + 4*6 + 5*5); + EXPECT_EQ(mju_dotSparse(u, b, 5, i, flg_unc1), 2*8 + 3*7 + 4*6 + 5*5 + 6*4); + EXPECT_EQ(mju_dotSparse(u, b, 6, i, flg_unc1), + 2*8 + 3*7 + 4*6 + 5*5 + 6*4 + 7*3); + EXPECT_EQ(mju_dotSparse(u, b, 7, i, flg_unc1), + 2*8 + 3*7 + 4*6 + 5*5 + 6*4 + 7*3 + 8*2); +} + +TEST_F(EngineUtilSparseTest, MjuDot2) { + constexpr int annz = 6; + constexpr int bnnz = 5; + int ia[annz] = {0, 2, 5, 6, 7}; + mjtNum a[annz] = {2, 3, 4, 5, 6}; + int ib[bnnz] = { 1, 2, 3, 5, 7}; + mjtNum b[bnnz] = { 8, 7, 6, 5, 4}; + mjtNum u[] = {1, 8, 7, 6, 1, 5, 1, 4}; + + // test various vector lengths as mju_dotSparse adds numbers in groups of four + + // a is compressed + int flg_unc2 = 0; + EXPECT_EQ(mju_dotSparse2(a, b, annz, ia, bnnz, ib, flg_unc2), 3*7+4*5+6*4); + + // u is uncompressed + flg_unc2 = 1; + EXPECT_EQ(mju_dotSparse2(a, u, annz, ia, bnnz, ib, flg_unc2), 3*7+4*5+6*4); } TEST_F(EngineUtilSparseTest, CombineSparseCount) { From 33f51856c1a552e8cf943f214ec1b9f6f0b450b1 Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Fri, 1 Sep 2023 12:44:53 -0700 Subject: [PATCH 18/38] Check that alignment is power of 2 in `mj_stackAlloc`. PiperOrigin-RevId: 562024496 Change-Id: I9230c6dc05ebb78e935f9b2f606aa77c921d67e5 --- doc/APIreference/functions.rst | 2 +- include/mujoco/mujoco.h | 2 +- introspect/functions.py | 2 +- python/mujoco/bindings_test.py | 2 +- src/engine/engine_crossplatform.h | 22 ++++++++++++++-------- src/engine/engine_io.c | 24 ++++++++++++++---------- src/engine/engine_io.h | 4 ++-- 7 files changed, 34 insertions(+), 24 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index c80c8358..f87ebf2c 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -1239,7 +1239,7 @@ mj_stackAlloc .. mujoco-include:: mj_stackAlloc -Allocate a number of bytes on :ref:`mjData` stack at a specific alignment which must be a power of 2. +Allocate a number of bytes on :ref:`mjData` stack at a specific alignment. Call mju_error on stack overflow. .. _mj_stackAllocNum: diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index c289b78a..d3c1841a 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -188,7 +188,7 @@ 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 number of bytes on mjData stack at a specific alignment which must be a power of 2. +// 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); diff --git a/introspect/functions.py b/introspect/functions.py index a8db3f87..4ee0d35c 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -699,7 +699,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ type=ValueType(name='size_t'), ), ), - doc='Allocate a number of bytes on mjData stack at a specific alignment which must be a power of 2. 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( diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index a19ec73e..4c07c3ff 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -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): diff --git a/src/engine/engine_crossplatform.h b/src/engine/engine_crossplatform.h index db350e9a..e0ab316c 100644 --- a/src/engine/engine_crossplatform.h +++ b/src/engine/engine_crossplatform.h @@ -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 - // 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_ diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 9ba2674d..af13d3e0 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -26,7 +26,7 @@ #include #include #include -#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" @@ -45,9 +45,13 @@ static const int MAX_ARRAY_SIZE = INT_MAX / 4; -// compute a % b assuming that the second argument is a power of 2 -static inline size_t modpow2(size_t a, size_t b) { - return a & (b - 1); +// 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; } //------------------------------ mjLROpt ----------------------------------------------------------- @@ -1187,12 +1191,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 = modpow2(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; } @@ -1218,7 +1222,7 @@ 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; } @@ -1242,7 +1246,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 -= modpow2(start_ptr, alignment); + start_ptr -= fastmod(start_ptr, alignment); // new top of the stack uintptr_t new_pstack_ptr = start_ptr - mjREDZONE; @@ -1255,8 +1259,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); diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index 137a00f4..0916d769 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -102,10 +102,10 @@ 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); -// mjData arena allocate (alignment must be a power of 2) +// mjData arena allocate MJAPI void* mj_arenaAlloc(mjData* d, size_t bytes, size_t alignment); -// mjData stack allocate (alignment must be a power of 2) +// mjData stack allocate MJAPI void* mj_stackAlloc(mjData* d, size_t bytes, size_t alignment); // mjData stack allocate for array of mjtNums From 89f47c33e45c81ff5b8471432c2d39194ed47143 Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Fri, 1 Sep 2023 17:56:43 -0700 Subject: [PATCH 19/38] Fix mjData stack frame leakages. PiperOrigin-RevId: 562091591 Change-Id: Ia7bb8a70ccba6bd40edc06ad8ebfac16e4286836 --- src/engine/engine_core_constraint.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index c96a67a7..7401be26 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -920,12 +920,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; } + mjMARKSTACK; + // allocate Jacobian jac = mj_stackAllocNum(d, 6*NV); jacdifp = mj_stackAllocNum(d, 3*NV); @@ -1831,13 +1832,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; } + mjMARKSTACK; + // space for backsubM2(J')' and its traspose mjtNum* JM2 = mj_stackAllocNum(d, nefc*nv); mjtNum* JM2T = mj_stackAllocNum(d, nv*nefc); From feb8aa2fbb66fdb880206c4e9e0d717649029f3d Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 4 Sep 2023 04:05:46 -0700 Subject: [PATCH 20/38] Fix bugs in engine_island.c PiperOrigin-RevId: 562519702 Change-Id: I3075e539ccc1cef92485152721a9344f180aa4b1 --- src/engine/engine_island.c | 171 ++++++++++++++------- test/engine/engine_island_test.cc | 48 +++--- test/engine/testdata/island/island_efc.xml | 10 +- 3 files changed, 145 insertions(+), 84 deletions(-) diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 3c90f032..65a704ac 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -160,13 +160,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 +173,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 +188,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 +201,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,7 +353,6 @@ 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)); @@ -287,60 +366,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); + } } } } @@ -452,7 +505,9 @@ void mj_island(const mjModel* m, mjData* d) { // compute efc_island, island_efcnum memset(d->island_efcnum, 0, nisland*sizeof(int)); 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]++; } diff --git a/test/engine/engine_island_test.cc b/test/engine/engine_island_test.cc index 2e74bfe3..58027551 100644 --- a/test/engine/engine_island_test.cc +++ b/test/engine/engine_island_test.cc @@ -350,33 +350,33 @@ TEST_F(IslandTest, IslandEfc) { mj_step(model, data); } - // sizes - int nv = model->nv; - int nefc = data->nefc; - int nisland = data->nisland; - // expect island structure to correspond to comment at top of xml - EXPECT_EQ(nisland, 3); - EXPECT_EQ(data->ne, 1); + EXPECT_EQ(data->nisland, 4); + EXPECT_EQ(data->ne, 4); EXPECT_EQ(data->nf, 2); EXPECT_EQ(data->nl, 1); - EXPECT_EQ(nefc, 24); - EXPECT_THAT(AsVector(data->dof_island, nv), - ElementsAre(0, -1, 0, 0, 0, 1, 2, 0, 1, 1, 1, 1, 1, 1)); - EXPECT_THAT(AsVector(data->island_dofnum, nisland), ElementsAre(5, 7, 1)); - EXPECT_THAT(AsVector(data->island_dofadr, nisland), ElementsAre(0, 5, 12)); - EXPECT_THAT(AsVector(data->island_dofind, nv), - ElementsAre(0, 2, 3, 4, 7, 5, 8, 9, 10, 11, 12, 13, 6, -1)); - EXPECT_THAT(AsVector(data->dof_islandind, nv), - ElementsAre(0, -1, 1, 2, 3, 0, 0, 4, 1, 2, 3, 4, 5, 6)); - EXPECT_THAT(AsVector(data->efc_island, nefc), - ElementsAre(0, 1, 2, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1)); - EXPECT_THAT(AsVector(data->island_efcnum, nisland), ElementsAre(6, 17, 1)); - EXPECT_THAT(AsVector(data->island_efcadr, nisland), ElementsAre(0, 6, 23)); - EXPECT_THAT(AsVector(data->island_efcind, nefc), - ElementsAre(0, 3, 4, 5, 6, 7, 1, 8, 9, 10, 11, 12, 13, 14, 15, 16, - 17, 18, 19, 20, 21, 22, 23, 2)); + EXPECT_EQ(data->nefc, 27); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(IslandTest, IslandEfcElliptic) { + const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + mjData* data = mj_makeData(model); + + model->opt.cone = mjCONE_ELLIPTIC; + while (data->time < 0.2) { + mj_step(model, data); + } + mj_forward(model, data); + + EXPECT_EQ(data->nisland, 4); + EXPECT_EQ(data->ne, 4); + EXPECT_EQ(data->nf, 2); + EXPECT_EQ(data->nl, 1); + EXPECT_EQ(data->nefc, 22); mj_deleteData(data); mj_deleteModel(model); diff --git a/test/engine/testdata/island/island_efc.xml b/test/engine/testdata/island/island_efc.xml index 3f30d9ec..b500f57c 100644 --- a/test/engine/testdata/island/island_efc.xml +++ b/test/engine/testdata/island/island_efc.xml @@ -1,10 +1,10 @@ @@ -64,9 +64,15 @@ + + + + + + From 5a21e202edec05429903f4d28adbb28a0d78ad61 Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Mon, 4 Sep 2023 04:45:37 -0700 Subject: [PATCH 21/38] Eliminate mjContact copying during collision detection. This results in approximately 4-5% speedup in collision detection for particle.xml. PiperOrigin-RevId: 562526634 Change-Id: Ia6487b1908618064d858bac484fe6861ed37a402 --- src/engine/engine_collision_driver.c | 75 +++++++++++++++++++--------- src/engine/engine_collision_driver.h | 5 -- src/engine/engine_core_constraint.c | 14 +----- src/engine/engine_io.h | 11 ++++ 4 files changed, 65 insertions(+), 40 deletions(-) diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 37cbdfb2..2a807aaf 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -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; @@ -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; } @@ -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; @@ -374,8 +389,10 @@ void mj_collision(const mjModel* m, mjData* d) { 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; @@ -398,7 +415,7 @@ void mj_collision(const mjModel* m, mjData* d) { 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 +453,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 +477,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 +485,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,7 +497,7 @@ 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); } } } @@ -852,11 +869,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 +945,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 +1128,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); } diff --git a/src/engine/engine_collision_driver.h b/src/engine/engine_collision_driver.h index a4435608..8c84a729 100644 --- a/src/engine/engine_collision_driver.h +++ b/src/engine/engine_collision_driver.h @@ -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); diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 7401be26..c93533f8 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -46,16 +46,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 +68,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 +171,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)); diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index 0916d769..7ad94015 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -18,6 +18,7 @@ #include #include #include +#include #ifdef __cplusplus #include @@ -117,6 +118,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 From 0e36f008578ef4684ae698608066fe3c5a2d273c Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Mon, 4 Sep 2023 06:37:53 -0700 Subject: [PATCH 22/38] Fix more mjData stack leakages. PiperOrigin-RevId: 562545230 Change-Id: I2facf25b00878669dc12d5865767a44229d2ca48 --- src/engine/engine_collision_driver.c | 5 +++-- src/engine/engine_core_constraint.c | 11 +++++++---- src/engine/engine_core_smooth.c | 4 ++-- src/engine/engine_derivative_fd.c | 2 +- src/engine/engine_forward.c | 2 +- src/engine/engine_inverse.c | 3 ++- src/engine/engine_island.c | 4 ++-- src/engine/engine_sensor.c | 2 +- src/engine/engine_vis_interact.c | 1 + 9 files changed, 20 insertions(+), 14 deletions(-) diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 2a807aaf..ede3a6bf 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -387,7 +387,6 @@ 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 and invalidate efc arrays d->ncon = 0; @@ -409,6 +408,8 @@ void mj_collision(const mjModel* m, mjData* d) { return; } + mjMARKSTACK; + // predefined only; ignore exclude if (m->opt.collision == mjCOL_PAIR) { d->nbodypair_broad = npair; @@ -677,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 @@ -750,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) + mjMARKSTACK; aabb = mj_stackAllocNum(d, 6*nbody); mju_zero(aabb, 6); diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index c93533f8..22309c25 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -495,13 +495,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; } + mjMARKSTACK; + // allocate space jac[0] = mj_stackAllocNum(d, 6*nv); jac[1] = mj_stackAllocNum(d, 6*nv); @@ -703,13 +704,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; } + mjMARKSTACK; + // allocate Jacobian jac = mj_stackAllocNum(d, nv); @@ -764,13 +766,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; } + mjMARKSTACK; + // allocate Jacobian jac = mj_stackAllocNum(d, nv); @@ -863,7 +866,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]; diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index b8a667d6..2a5e629e 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -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 + mjMARKSTACK; jac1 = mj_stackAllocNum(d, 3*nv); jac2 = mj_stackAllocNum(d, 3*nv); jacdif = mj_stackAllocNum(d, 3*nv); @@ -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 + mjMARKSTACK; jac = mj_stackAllocNum(d, 3*nv); jacA = mj_stackAllocNum(d, 3*nv); jacS = mj_stackAllocNum(d, 3*nv); diff --git a/src/engine/engine_derivative_fd.c b/src/engine/engine_derivative_fd.c index 3c874207..2cde539b 100644 --- a/src/engine/engine_derivative_fd.c +++ b/src/engine/engine_derivative_fd.c @@ -634,7 +634,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 +647,7 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio int skipsensor = !DsDq && !DsDv && !DsDa; // local vectors + mjMARKSTACK; mjtNum *pos = mj_stackAllocNum(d, nq); // position mjtNum *force = mj_stackAllocNum(d, nv); // force mjtNum *force_plus = mj_stackAllocNum(d, nv); // nudged force diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 38f342ba..0ebe6a72 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -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 + mjMARKSTACK; dX = mj_stackAllocNum(d, 2*nv+na); for (int i=0; i < N; i++) { X[i] = mj_stackAllocNum(d, nq+nv+na); diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index 3cfa3321..1e1fa50c 100644 --- a/src/engine/engine_inverse.c +++ b/src/engine/engine_inverse.c @@ -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) { + mjFREESTACK; return; } @@ -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 + mjMARKSTACK; qforce = mj_stackAllocNum(d, nv); dif = mj_stackAllocNum(d, nv); save_qfrc_constraint = mj_stackAllocNum(d, nv); diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 65a704ac..3dca27da 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -459,7 +459,7 @@ void mj_island(const mjModel* m, mjData* d) { memset(d->island_dofnum, 0, nisland*sizeof(int)); 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 @@ -497,7 +497,7 @@ 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; } diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 38ae6e00..45400bbf 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -854,13 +854,13 @@ 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; } + mjMARKSTACK; vec = mj_stackAllocNum(d, m->nv); // kinetic energy: 0.5 * qvel' * M * qvel diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 5ece1d52..069dbdba 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -529,6 +529,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur // invalid selected body: return if (sel <= 0 || sel >= m->nbody) { + mjFREESTACK; return; } From 5cef4708f267d945fac9065fd90817d8e98f06d8 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 4 Sep 2023 06:50:45 -0700 Subject: [PATCH 23/38] Allow `mj_mulJac(T)Vec_island` to read and write from uncompressed vectors (length nv or nefc). PiperOrigin-RevId: 562547310 Change-Id: Id517c1842e711639f4113d23cc364945839a3759 --- src/engine/engine_core_constraint.c | 24 ++++--- src/engine/engine_core_constraint.h | 8 +-- test/engine/engine_core_constraint_test.cc | 83 +++++++++++++++++++--- 3 files changed, 95 insertions(+), 20 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 22309c25..c95a3d65 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -386,8 +386,9 @@ void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* // multiply Jacobian by vector, for one island -void mj_mulJacVec_island(const mjModel* m, const mjData* d, - mjtNum* res, const mjtNum* vec, int 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); @@ -410,7 +411,8 @@ void mj_mulJacVec_island(const mjModel* m, const mjData* d, int Jrowadr = d->efc_J_rowadr[row]; int* Jind = d->efc_J_colind + Jrowadr; mjtNum* J = d->efc_J + Jrowadr; - res[i] = mju_dotSparse2(vec, J, vecnnz, vecind, Jnnz, Jind, /*flg_unc2=*/0); + int j = flg_resunc ? row : i; + res[j] = mju_dotSparse2(J, vec, Jnnz, Jind, vecnnz, vecind, flg_vecunc); } } @@ -418,7 +420,9 @@ void mj_mulJacVec_island(const mjModel* m, const mjData* d, else { int nv = m->nv; for (int i=0; i < resnnz; i++) { - res[i] = mju_dotSparse(vec, d->efc_J + nv*resind[i], vecnnz, vecind, /*flg_unc1=*/0); + int row = resind[i]; + int j = flg_resunc ? row : i; + res[j] = mju_dotSparse(vec, d->efc_J + nv*row, vecnnz, vecind, flg_vecunc); } } } @@ -447,8 +451,9 @@ void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* // multiply Jacobian transpose by vector, for one island -void mj_mulJacTVec_island(const mjModel* m, const mjData* d, - mjtNum* res, const mjtNum* vec, int 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); @@ -471,7 +476,8 @@ void mj_mulJacTVec_island(const mjModel* m, const mjData* d, int JTrowadr = d->efc_JT_rowadr[row]; int* JTind = d->efc_JT_colind + JTrowadr; mjtNum* JT = d->efc_JT + JTrowadr; - res[i] = mju_dotSparse2(vec, JT, vecnnz, vecind, JTnnz, JTind, /*flg_unc2=*/0); + int j = flg_resunc ? row : i; + res[j] = mju_dotSparse2(JT, vec, JTnnz, JTind, vecnnz, vecind, flg_vecunc); } } @@ -479,7 +485,9 @@ void mj_mulJacTVec_island(const mjModel* m, const mjData* d, else { int nefc = d->nefc; for (int i=0; i < resnnz; i++) { - res[i] = mju_dotSparse(vec, d->efc_JT + nefc*resind[i], vecnnz, vecind, /*flg_unc1=*/0); + int row = resind[i]; + int j = flg_resunc ? row : i; + res[j] = mju_dotSparse(vec, d->efc_JT + nefc*row, vecnnz, vecind, flg_vecunc); } } } diff --git a/src/engine/engine_core_constraint.h b/src/engine/engine_core_constraint.h index 2dcc42fb..5c6b353a 100644 --- a/src/engine/engine_core_constraint.h +++ b/src/engine/engine_core_constraint.h @@ -39,15 +39,15 @@ MJAPI int mj_isDual(const mjModel* m); 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); +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, 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); +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 ----------------------------------------------------- diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index fee8ba7d..32c5fb0b 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -15,6 +15,7 @@ // Tests for engine/engine_core_constraint.c. #include +#include #include #include @@ -225,6 +226,7 @@ TEST_F(CoreConstraintTest, MulJacVecIsland) { // multiply by Jacobian: vec_nefc = J * vec_nv mjtNum* vec_nefc = (mjtNum*) mju_malloc(sizeof(mjtNum)*data->nefc); mj_mulJacVec(model, data, vec_nefc, vec_nv); + mjtNum* vec_nefc_tmp = (mjtNum*) mju_malloc(sizeof(mjtNum)*data->nefc); // iterate over islands for (int i=0; i < data->nisland; i++) { @@ -234,25 +236,57 @@ TEST_F(CoreConstraintTest, MulJacVecIsland) { int efcnum = data->island_efcnum[i]; mjtNum* vec_nefci = (mjtNum*)mju_malloc(sizeof(mjtNum) * efcnum); - // copy values into vec_nvi + // get indices int* dofind = data->island_dofind + data->island_dofadr[i]; + int* efcind = data->island_efcind + data->island_efcadr[i]; + + // copy values into vec_nvi for (int j=0; j < dofnum; j++) { vec_nvi[j] = vec_nv[dofind[j]]; } - // multiply by Jacobian, for this island - mj_mulJacVec_island(model, data, vec_nefci, vec_nvi, i); + // ===== both compressed + int flg_resunc = 0; + int flg_vecunc = 0; + mju_zero(vec_nefci, efcnum); // clear output + mj_mulJacVec_island(model, data, vec_nefci, vec_nvi, + i, flg_resunc, flg_vecunc); // expect corresponding values to match - int* efcind = data->island_efcind + data->island_efcadr[i]; for (int j=0; j < efcnum; j++) { EXPECT_THAT(vec_nefci[j], DoubleNear(vec_nefc[efcind[j]], 1e-12)); } + // ===== input uncompressed: read from vec_nv + flg_resunc = 0; + flg_vecunc = 1; + mju_zero(vec_nefci, efcnum); // clear output + mj_mulJacVec_island(model, data, vec_nefci, vec_nv, + i, flg_resunc, flg_vecunc); + + // expect corresponding values to match + for (int j=0; j < efcnum; j++) { + EXPECT_THAT(vec_nefci[j], DoubleNear(vec_nefc[efcind[j]], 1e-12)); + } + + // ===== output uncompressed: write to vec_nefc_tmp + flg_resunc = 1; + flg_vecunc = 0; + mju_zero(vec_nefc_tmp, data->nefc); // clear output + mj_mulJacVec_island(model, data, vec_nefc_tmp, vec_nvi, + i, flg_resunc, flg_vecunc); + + // expect corresponding values to match + for (int j=0; j < efcnum; j++) { + EXPECT_THAT(vec_nefc_tmp[efcind[j]], + DoubleNear(vec_nefc[efcind[j]], 1e-12)); + } + mju_free(vec_nvi); mju_free(vec_nefci); } + mju_free(vec_nefc_tmp); mju_free(vec_nefc); } @@ -268,6 +302,7 @@ TEST_F(CoreConstraintTest, MulJacTVecIsland) { // allocate vec_nv mjtNum* vec_nv = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nv); + mjtNum* vec_nv_tmp = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nv); // iterate through dense and sparse for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) { @@ -297,27 +332,59 @@ TEST_F(CoreConstraintTest, MulJacTVecIsland) { int efcnum = data->island_efcnum[i]; mjtNum* vec_nefci = (mjtNum*)mju_malloc(sizeof(mjtNum) * efcnum); - // copy values into vec_nefci + // get indices int* efcind = data->island_efcind + data->island_efcadr[i]; + int* dofind = data->island_dofind + data->island_dofadr[i]; + + // copy values into vec_nefci for (int j=0; j < efcnum; j++) { vec_nefci[j] = vec_nefc[efcind[j]]; } - // multiply by Jacobian, for this island - mj_mulJacTVec_island(model, data, vec_nvi, vec_nefci, i); + // ==== both compressed + int flg_resunc = 0; + int flg_vecunc = 0; + mju_zero(vec_nvi, dofnum); // clear output + mj_mulJacTVec_island(model, data, vec_nvi, vec_nefci, + i, flg_resunc, flg_vecunc); // expect corresponding values to match - int* dofind = data->island_dofind + data->island_dofadr[i]; for (int j=0; j < dofnum; j++) { EXPECT_THAT(vec_nvi[j], DoubleNear(vec_nv[dofind[j]], 1e-12)); } + // ===== input uncompressed: read from vec_nefc + flg_resunc = 0; + flg_vecunc = 1; + mju_zero(vec_nvi, dofnum); // clear output + mj_mulJacTVec_island(model, data, vec_nvi, vec_nefc, + i, flg_resunc, flg_vecunc); + + // expect corresponding values to match + for (int j=0; j < dofnum; j++) { + EXPECT_THAT(vec_nvi[j], DoubleNear(vec_nv[dofind[j]], 1e-12)); + } + + // ===== output uncompressed: write to vec_nv_tmp + flg_resunc = 1; + flg_vecunc = 0; + mju_zero(vec_nv_tmp, model->nv); // clear output + mj_mulJacTVec_island(model, data, vec_nv_tmp, vec_nefci, + i, flg_resunc, flg_vecunc); + + // expect corresponding values to match + for (int j=0; j < dofnum; j++) { + EXPECT_THAT(vec_nv_tmp[dofind[j]], + DoubleNear(vec_nv[dofind[j]], 1e-12)); + } + mju_free(vec_nvi); mju_free(vec_nefci); } mju_free(vec_nefc); } + mju_free(vec_nv_tmp); mju_free(vec_nv); mj_deleteData(data); mj_deleteModel(model); From b7686440d1b7c10d57ff19ac853a3ba3b25a5c6e Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 4 Sep 2023 09:31:48 -0700 Subject: [PATCH 24/38] Indent a couple of #ifdef bodies. PiperOrigin-RevId: 562572883 Change-Id: I44e2d6dc486ed7a181ca4614b1bf3f2f762f52f9 --- src/engine/engine_macro.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/engine/engine_macro.h b/src/engine/engine_macro.h index a817e165..5238000d 100644 --- a/src/engine/engine_macro.h +++ b/src/engine/engine_macro.h @@ -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 ---------------------------------------------------- From 600c12533da72c1c667fdd5e8b0899b5b319b951 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 4 Sep 2023 10:32:45 -0700 Subject: [PATCH 25/38] Add mj_constraintUpdate_island. PiperOrigin-RevId: 562581620 Change-Id: If156a02873168127e2c5f2f377532fd45f7eec1c --- src/engine/engine_core_constraint.c | 65 ++++++++----- src/engine/engine_core_constraint.h | 4 + test/engine/engine_core_constraint_test.cc | 108 +++++++++++++++++++++ 3 files changed, 155 insertions(+), 22 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index c95a3d65..98857258 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -2003,18 +2003,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; } @@ -2022,16 +2029,19 @@ 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]; } // update constraints - for (int i=0; i < nefc; i++) { + for (int c=0; c < nefc; c++) { + int i = efcind ? efcind[c] : c; + // ==== equality if (i < ne) { 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; continue; @@ -2040,9 +2050,9 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar, // ==== friction if (i < ne + nf) { // linear negative - if (jar[i] <= -R[i]*floss[i]) { + if (jar[c] <= -R[i]*floss[i]) { if (cost) { - s += -0.5*R[i]*floss[i]*floss[i] - floss[i]*jar[i]; + s += -0.5*R[i]*floss[i]*floss[i] - floss[i]*jar[c]; } force[i] = floss[i]; @@ -2051,9 +2061,9 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar, } // linear positive - else if (jar[i] >= R[i]*floss[i]) { + else if (jar[c] >= R[i]*floss[i]) { if (cost) { - s += -0.5*R[i]*floss[i]*floss[i] + floss[i]*jar[i]; + s += -0.5*R[i]*floss[i]*floss[i] + floss[i]*jar[c]; } force[i] = -floss[i]; @@ -2064,7 +2074,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; @@ -2077,7 +2087,7 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar, // 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; @@ -2086,7 +2096,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; @@ -2102,9 +2112,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 @@ -2122,7 +2132,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]; } } @@ -2196,15 +2206,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); +} diff --git a/src/engine/engine_core_constraint.h b/src/engine/engine_core_constraint.h index 5c6b353a..ce04b120 100644 --- a/src/engine/engine_core_constraint.h +++ b/src/engine/engine_core_constraint.h @@ -116,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 diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 32c5fb0b..6f62a739 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -390,5 +390,113 @@ TEST_F(CoreConstraintTest, MulJacTVecIsland) { mj_deleteModel(model); } +TEST_F(CoreConstraintTest, ConstraintUpdateIsland) { + const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + mjData* data1 = mj_makeData(model); + mjData* data2 = mj_makeData(model); + + // iterate over sparsity and cone + for (mjtJacobian sparsity : {mjJAC_SPARSE, mjJAC_DENSE}) { + for (mjtCone cone : {mjCONE_PYRAMIDAL, mjCONE_ELLIPTIC}) { + model->opt.jacobian = sparsity; + model->opt.cone = cone; + + // simulate for 0.2 seconds + mj_resetData(model, data1); + mj_resetData(model, data2); + while (data1->time < 0.2) { + mj_step(model, data1); + mj_step(model, data2); + } + mj_forward(model, data1); + mj_forward(model, data2); + + // get sizes + int nefc = data1->nefc; + int nv = model->nv; + int nisland = data1->nisland; + EXPECT_GT(nisland, 0); + + // get jar = J*a - aref + mjtNum* jar = (mjtNum*)mju_malloc(sizeof(mjtNum) * nefc); + mj_mulJacVec(model, data1, jar, data1->qacc); + mju_subFrom(jar, data1->efc_aref, nefc); + + // constraint update for data1 given jar + mjtNum cost1; + mj_constraintUpdate(model, data1, jar, &cost1, /*flg_coneHessian=*/1); + + // iterate over islands, check match + mjtNum cost2 = 0; + for (int island=0; island < nisland; island++) { + // clear outputs from data2 + for (int i=0; i < nefc; i++) data2->efc_state[i] = -1; + mju_zero(data2->efc_force, nefc); + mju_zero(data2->qfrc_constraint, nv); + for (int i=0; i < data2->ncon; i++) mju_zero(data2->contact[i].H, 36); + + // sizes and indices, in this island + int dofnum = data2->island_dofnum[island]; + int efcnum = data2->island_efcnum[island]; + int* dofind = data2->island_dofind + data2->island_dofadr[island]; + int* efcind = data2->island_efcind + data2->island_efcadr[island]; + + // get jar restricted to island + mjtNum* jari = (mjtNum*)mju_malloc(sizeof(mjtNum) * efcnum); + for (int c=0; c < efcnum; c++) { + jari[c] = jar[efcind[c]]; + } + + // update constraints for this island + mjtNum cost2i; + mj_constraintUpdate_island(model, data2, jari, &cost2i, + /*flg_coneHessian=*/1, island); + + // compare nefc vectors + for (int c=0; c < efcnum; c++) { + int i = efcind[c]; + EXPECT_EQ(data2->efc_island[i], island); + EXPECT_EQ(data2->efc_state[i], data1->efc_state[i]); + EXPECT_THAT(data2->efc_force[i], + DoubleNear(data1->efc_force[i], 1e-12)); + } + + // compare qfrc_constraint + for (int c=0; c < dofnum; c++) { + int i = dofind[c]; + EXPECT_THAT(data2->qfrc_constraint[i], + DoubleNear(data1->qfrc_constraint[i], 1e-12)); + } + + // compare cone Hessians + for (int c=0; c < data2->ncon; c++) { + int efcadr = data2->contact[c].efc_address; + if (data2->efc_island[efcadr] == island) { + for (int j=0; j < 36; j++) { + EXPECT_THAT(data2->contact[c].H[j], + DoubleNear(data2->contact[c].H[j], 1e-12)); + } + } + } + + // add island cost to total cost + cost2 += cost2i; + + mju_free(jari); + } + + // expect monolithic total cost + EXPECT_THAT(cost1, DoubleNear(cost2, 1e-12)); + + mju_free(jar); + } + } + + mj_deleteData(data2); + mj_deleteData(data1); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco From 1aa375ef9afadb6096e88777f6d81e53aaaec6cb Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 4 Sep 2023 13:55:56 -0700 Subject: [PATCH 26/38] Allow RHS vector in `mj_mulM_island` to use uncompressed memory. PiperOrigin-RevId: 562605681 Change-Id: If34bceed9eade59957ac9fd9b6a41fb736b58024 --- src/engine/engine_support.c | 18 ++++++++++++++---- src/engine/engine_support.h | 2 +- test/engine/engine_support_test.cc | 17 ++++++++++++++++- 3 files changed, 31 insertions(+), 6 deletions(-) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index c58e5e06..14cd04d5 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -885,7 +885,8 @@ void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) // 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) { +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); @@ -913,7 +914,11 @@ void mj_mulM_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum int adr = Madr[i]; // diagonal - res[k] = M[adr]*vec[k]; + if (flg_vecunc) { + res[k] = M[adr]*vec[i]; + } else { + res[k] = M[adr]*vec[k]; + } // simple dof: continue if (simplenum[i]) { @@ -925,8 +930,13 @@ void mj_mulM_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum while (j >= 0) { adr++; int l = islandind[j]; - res[k] += M[adr]*vec[l]; - res[l] += M[adr]*vec[k]; + 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]; diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index 3b10fbe7..acacbeca 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -110,7 +110,7 @@ MJAPI void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* // 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 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); diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 1d7b8511..10c94734 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -493,8 +493,23 @@ TEST_F(SupportTest, MulMIsland) { vec_i[j] = vec[dofind[j]]; } + // === compressed: use vec_i + // multiply by Jacobian, for this island - mj_mulM_island(model, data, Mvec_i, vec_i, i); + int flg_vecunc = 0; + mj_mulM_island(model, data, Mvec_i, vec_i, i, flg_vecunc); + + // expect corresponding values to match + for (int j=0; j < dofnum; j++) { + EXPECT_THAT(Mvec_i[j], DoubleNear(Mvec[dofind[j]], 1e-12)); + } + + // === uncompressed: use vec + mju_zero(Mvec_i, dofnum); // clear output + + // multiply by Jacobian, for this island + flg_vecunc = 1; + mj_mulM_island(model, data, Mvec_i, vec, i, flg_vecunc); // expect corresponding values to match for (int j=0; j < dofnum; j++) { From 9308e1d383a93bb8c7b3e3528b0af8684a40bbf3 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 4 Sep 2023 14:26:19 -0700 Subject: [PATCH 27/38] Add internal utility for zeroing int vectors. PiperOrigin-RevId: 562608956 Change-Id: I25ff1bbf9abf8fe2a636ed1d728d10ae862e04c9 --- src/engine/engine_core_smooth.c | 2 +- src/engine/engine_derivative_fd.c | 5 +++-- src/engine/engine_io.c | 6 +++--- src/engine/engine_island.c | 11 ++++++----- src/engine/engine_util_misc.c | 9 +++++++++ src/engine/engine_util_misc.h | 3 +++ src/engine/engine_util_sparse.c | 3 ++- 7 files changed, 27 insertions(+), 12 deletions(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 2a5e629e..ebd342a4 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -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); } diff --git a/src/engine/engine_derivative_fd.c b/src/engine/engine_derivative_fd.c index 2cde539b..e095d9de 100644 --- a/src/engine/engine_derivative_fd.c +++ b/src/engine/engine_derivative_fd.c @@ -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" @@ -202,7 +203,7 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { 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); @@ -254,7 +255,7 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { 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++) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index af13d3e0..668891a3 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -836,7 +836,7 @@ static void makeDSparse(const mjModel* m, mjData* 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; @@ -893,7 +893,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]; @@ -927,7 +927,7 @@ static void makeBSparse(const mjModel* m, mjData* d) { // allocate and clear incremental row counts mjMARKSTACK; 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--) { diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 3dca27da..2c0d7a6d 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -27,6 +27,7 @@ #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 @@ -355,7 +356,7 @@ static int findEdges(const mjModel* m, const mjData* d, int* treenedge, int* edg int efc_id = -1; // clear treenedge - memset(treenedge, 0, m->ntree*sizeof(int)); + mju_zeroInt(treenedge, m->ntree); int nedge = 0; for (int i=0; i < nefc; i++) { @@ -456,7 +457,7 @@ 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]]; @@ -477,7 +478,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; @@ -503,7 +504,7 @@ void mj_island(const mjModel* m, mjData* d) { } // 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 tree[2]; treeFirst(m, d, tree, i); @@ -519,7 +520,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++) { diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index dd9a9cbe..a11bbd00 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -1207,6 +1207,15 @@ int mju_isZero(mjtNum* vec, int n) { +// set integer vector to 0 +void mju_zeroInt(int* res, int n) { + if (n > 0) { + memset(res, 0, n*sizeof(int)); + } +} + + + // standard normal random number generator (optional second number) mjtNum mju_standardNormal(mjtNum* num2) { const mjtNum scale = 2.0/((mjtNum)RAND_MAX); diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 6260d0c5..c3150c88 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -119,6 +119,9 @@ 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); + // standard normal random number generator (optional second number) MJAPI mjtNum mju_standardNormal(mjtNum* num2); diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 903d72c8..af1909ae 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -22,6 +22,7 @@ #include #include "engine/engine_io.h" #include "engine/engine_util_blas.h" +#include "engine/engine_util_misc.h" //------------------------------ sparse operations ------------------------------------------------- @@ -393,7 +394,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]; From 94a8705ad05a345c3f03f06ede7f3799fbe28fb6 Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Mon, 4 Sep 2023 17:03:23 -0700 Subject: [PATCH 28/38] Add `mj_markStack` and `mj_freeStack` as public API functions. Also add asan instrumentation to detect stack frame leakages (i.e. `mj_markStack` without a corresponding `mj_freeStack` in the same caller function). PiperOrigin-RevId: 562625645 Change-Id: I4e3ff66ca0b9d08ed0a95cef45393db8e3053e22 --- doc/APIreference/functions.rst | 19 ++++ doc/changelog.rst | 2 + doc/includes/references.h | 3 + include/mujoco/mjdata.h | 1 + include/mujoco/mjmacro.h | 17 +--- include/mujoco/mjxmacro.h | 1 + include/mujoco/mujoco.h | 11 ++- introspect/functions.py | 28 ++++++ introspect/structs.py | 5 ++ src/engine/engine_io.c | 100 ++++++++++++++++++++-- src/engine/engine_io.h | 6 ++ src/engine/engine_print.c | 14 +-- test/engine/engine_io_test.cc | 84 +++++++++++++++++- test/engine/engine_util_container_test.cc | 18 ++-- test/fixture.h | 35 ++++++++ test/xml/xml_native_reader_test.cc | 5 +- unity/Runtime/Bindings/MjBindings.cs | 7 ++ 17 files changed, 311 insertions(+), 45 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index f87ebf2c..3a6eb564 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -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 diff --git a/doc/changelog.rst b/doc/changelog.rst index cebd8ce5..db3b4ab9 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -43,6 +43,8 @@ General functionality of allocating ``mjtNum`` arrays is still available through the new function :ref:`mj_stackAllocNum`. #. Renamed the ``nstack`` field in :ref:`mjModel` and :ref:`mjData` to ``narena``. Changed ``narena``, ``pstack``, and ``maxuse_stack`` to count number of bytes rather than number of :ref:`mjtNum` |-| s. +#. Added new functions ``mj_markStack`` and ``mj_freeStack`` that manages ``mjData`` stack frames in a fully + encapsulated way (i.e. without having to introduce a local variable at the call site). Python bindings ^^^^^^^^^^^^^^^ diff --git a/doc/includes/references.h b/doc/includes/references.h index e6f81619..792c5dcf 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -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 @@ -2205,6 +2206,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); diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 756b7675..51522ca3 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -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 diff --git a/include/mujoco/mjmacro.h b/include/mujoco/mjmacro.h index 713d5c52..e3972015 100644 --- a/include/mujoco/mjmacro.h +++ b/include/mujoco/mjmacro.h @@ -15,8 +15,6 @@ #ifndef MUJOCO_MJMACRO_H_ #define MUJOCO_MJMACRO_H_ -#include - // include asan interface header, or provide stubs for poison/unpoison macros when not using asan #ifdef ADDRESS_SANITIZER #include @@ -33,8 +31,8 @@ #define mjMIN(a, b) (((a) < (b)) ? (a) : (b)) // mjData stack frame management -#define mjMARKSTACK size_t _mark = d->pstack; -#define mjFREESTACK d->pstack = _mark; +#define mjMARKSTACK mj_markStack(d); +#define mjFREESTACK mj_freeStack(d); // return current value of mjOption enable/disable flags #define mjDISABLED(x) (m->opt.disableflags & (x)) @@ -49,15 +47,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_ diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 7d5c5724..bccfcde2 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -625,6 +625,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 ) \ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index d3c1841a..ce813376 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -30,10 +30,6 @@ extern "C" { #include #include -#ifdef ADDRESS_SANITIZER -#include -#endif - // type definitions #include #include @@ -188,6 +184,13 @@ 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); +// 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); diff --git a/introspect/functions.py b/introspect/functions.py index 4ee0d35c..984f4eda 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -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', diff --git a/introspect/structs.py b/introspect/structs.py index 5ac4330a..1573fb0c 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -3502,6 +3502,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'), diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 668891a3..f713c718 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -35,6 +35,11 @@ #include "engine/engine_util_misc.h" #include "engine/engine_vfs.h" +#ifdef ADDRESS_SANITIZER + #include + #include +#endif + #ifdef MEMORY_SANITIZER #include #endif @@ -43,6 +48,13 @@ #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 @@ -54,6 +66,12 @@ static inline size_t fastmod(size_t a, size_t b) { 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 @@ -1226,13 +1244,6 @@ static inline void* stackalloc(mjData* d, size_t size, size_t alignment) { 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; @@ -1284,8 +1295,6 @@ static inline void* stackalloc(mjData* d, size_t size, size_t alignment) { 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); @@ -1294,6 +1303,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); } @@ -1320,6 +1396,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; @@ -1487,6 +1564,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]); diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index 7ad94015..b0595747 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -106,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); diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 8cd95131..d9455c4d 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -730,6 +730,11 @@ 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; @@ -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) { @@ -776,7 +776,9 @@ 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) { \ const char* format = _Generic( \ d->name, \ int : INT_FORMAT, \ diff --git a/test/engine/engine_io_test.cc b/test/engine/engine_io_test.cc index 4ccebcce..c73b28e3 100644 --- a/test/engine/engine_io_test.cc +++ b/test/engine/engine_io_test.cc @@ -17,24 +17,26 @@ #include "src/engine/engine_io.h" #include -#include +#include #include #include #include -#include -#include #include +#include #include #include +#include // IWYU pragma: keep #include #include +#include #include "src/engine/engine_util_errmem.h" #include "test/fixture.h" namespace mujoco { namespace { +using ::testing::ContainsRegex; // NOLINT(misc-unused-using-decls) asan only using ::testing::HasSubstr; using ::testing::IsNull; using ::testing::NotNull; @@ -737,5 +739,81 @@ TEST_F(ValidateReferencesTest, Tuples) { mj_deleteModel(model); } +TEST_F(EngineIoTest, CanMarkAndFreeStack) { + constexpr char xml[] = R"( + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + ASSERT_THAT(data, NotNull()); + + auto pstack_before = data->pstack; + mj_markStack(data); + EXPECT_GT(data->pstack, pstack_before); + mj_freeStack(data); + EXPECT_EQ(data->pstack, pstack_before); + + mj_deleteData(data); + mj_deleteModel(model); +} + +#ifdef ADDRESS_SANITIZER +void MarkFreeStack(mjData* d, bool free) { + mj_markStack(d); + if (free) { + mj_freeStack(d); + } +} + +TEST_F(EngineIoTest, CanDetectStackFrameLeakage) { + static constexpr char xml[] = R"( + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + ASSERT_THAT(data, NotNull()); + + // MarkFreeStack correctly calls mj_freeStack, should not error. + MarkFreeStack(data, /* free= */ true); + + // MarkFreeStack calls mj_markStack without mj_freeStack, the next call to + // mj_freeStack should detect the stack frame leakage. + mj_markStack(data); + MarkFreeStack(data, /* free= */ false); + EXPECT_THAT( + MjuErrorMessageFrom(mj_freeStack)(data), + ContainsRegex( + "mj_markStack in MarkFreeStack at .*engine_io_test\\.cc.* has no " + "corresponding mj_freeStack")); + + // Dangling stack frames should be detected in mj_deleteData. + mj_resetData(model, data); + mj_markStack(data); + EXPECT_THAT( + MjuErrorMessageFrom(mj_deleteData)(data), + ContainsRegex( + "mj_markStack in .+EngineIoTest_CanDetectStackFrameLeakage.+ has no " + "corresponding mj_freeStack")); + + mj_resetData(model, data); + mj_deleteData(data); + mj_deleteModel(model); +} +#endif + } // namespace } // namespace mujoco diff --git a/test/engine/engine_util_container_test.cc b/test/engine/engine_util_container_test.cc index bd9766e2..42408739 100644 --- a/test/engine/engine_util_container_test.cc +++ b/test/engine/engine_util_container_test.cc @@ -27,18 +27,21 @@ namespace { using testing::NotNull; -template +template constexpr int GetExpectedStackUsageBytes() { if constexpr (N <= 0) { - return 0; + return prev_size; } else { constexpr auto RoundUpToAlignment = [](int x, int alignment) { return alignment * (x / alignment + ((x % alignment) ? 1 : 0)); }; - return RoundUpToAlignment(sizeof(mjArrayList), alignof(mjArrayList)) + - RoundUpToAlignment(Capacity * sizeof(T), alignof(std::max_align_t)) + - GetExpectedStackUsageBytes(); + constexpr int size_with_arraylist = RoundUpToAlignment( + prev_size + sizeof(mjArrayList), alignof(mjArrayList)); + constexpr int size_with_buffer = RoundUpToAlignment( + size_with_arraylist + capacity * sizeof(T), alignof(std::max_align_t)); + return GetExpectedStackUsageBytes(); } } @@ -48,6 +51,7 @@ TEST(TestMjArrayList, TestMjArrayListSingleThreaded) { ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error.data(); mjData* d = mj_makeData(m); mjMARKSTACK; + using DataType = int; constexpr int kInitialCapacity = 10; mjArrayList* array_list = @@ -59,8 +63,10 @@ TEST(TestMjArrayList, TestMjArrayListSingleThreaded) { } EXPECT_EQ(mju_arrayListSize(array_list), kNumElements); + constexpr int kFrameMarkerSize = 2 * sizeof(size_t) + sizeof(void*); constexpr int kExpectedMaxUseStack = - GetExpectedStackUsageBytes(); + GetExpectedStackUsageBytes(); EXPECT_EQ(d->maxuse_stack, kExpectedMaxUseStack); for (int i = 0; i < kNumElements; ++i) { diff --git a/test/fixture.h b/test/fixture.h index 83111f1c..cdf097ec 100644 --- a/test/fixture.h +++ b/test/fixture.h @@ -15,12 +15,23 @@ #ifndef MUJOCO_TEST_FIXTURE_H_ #define MUJOCO_TEST_FIXTURE_H_ +#include +#include +#include +#include +#include + #include #include #include #include #include +extern "C" { +MJAPI void _mjPRIVATE__set_tls_error_fn(decltype(mju_user_error)); +MJAPI decltype(mju_user_error) _mjPRIVATE__get_tls_error_fn(); +} + namespace mujoco { // Installs and uninstalls error callbacks on MuJoCo that fail the currently @@ -44,6 +55,30 @@ class MujocoTest : public ::testing::Test { MujocoErrorTestGuard error_guard; }; +template +auto MjuErrorMessageFrom(Return (*func)(Args...)) { + thread_local std::jmp_buf current_jmp_buf; + thread_local char err_msg[1000]; + + auto* old_error_handler = _mjPRIVATE__get_tls_error_fn(); + auto* new_error_handler = +[](const char* msg) -> void { + std::strncpy(err_msg, msg, sizeof(err_msg)); + std::longjmp(current_jmp_buf, 1); + }; + + return [func, old_error_handler, + new_error_handler](Args... args) -> std::string { + if (setjmp(current_jmp_buf) == 0) { + err_msg[0] = '\0'; + _mjPRIVATE__set_tls_error_fn(new_error_handler); + func(args...); + } + + _mjPRIVATE__set_tls_error_fn(old_error_handler); + return err_msg; + }; +} + // Returns a path to a data file, under the mujoco/test directory. const std::string GetTestDataFilePath(absl::string_view path); diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 09d5c519..dad4144e 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -43,12 +44,12 @@ TEST_F(XMLReaderTest, MemorySize) { { static constexpr char xml[] = R"( - + )"; mjModel* model = LoadModelFromString(xml, error.data(), error.size()); ASSERT_THAT(model, NotNull()) << error.data(); - EXPECT_EQ(model->narena, 256); + EXPECT_EQ(model->narena, 512); mj_deleteModel(model); } { diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 7b318b97..e886c3c5 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -574,6 +574,7 @@ public unsafe struct mjData_ { public UIntPtr nbuffer; public int nplugin; public UIntPtr pstack; + public UIntPtr pbase; public UIntPtr parena; public UIntPtr maxuse_stack; public UIntPtr maxuse_arena; @@ -3087,6 +3088,12 @@ public static unsafe extern void mj_resetDataDebug(mjModel_* m, mjData_* d, byte [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_resetDataKeyframe(mjModel_* m, mjData_* d, int key); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mj_markStack(mjData_* d); + +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mj_freeStack(mjData_* d); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void* mj_stackAlloc(mjData_* d, UIntPtr bytes, UIntPtr alignment); From 6225186964b545b0114592a3be4e18ca6e988fd4 Mon Sep 17 00:00:00 2001 From: Matthew Bennice Date: Mon, 4 Sep 2023 20:08:39 -0700 Subject: [PATCH 29/38] Add threading primitives. PiperOrigin-RevId: 562649644 Change-Id: I9c35b270e4b3b50cc7eb5a152f24d71def8e2fcd --- CMakeLists.txt | 2 + doc/APIreference/APIfunctions.rst | 1 + doc/APIreference/APItypes.rst | 19 ++++ doc/APIreference/functions.rst | 40 +++++++ doc/changelog.rst | 1 + doc/includes/references.h | 17 +++ doc/programming/index.rst | 6 ++ include/mujoco/mjdata.h | 3 + include/mujoco/mjthread.h | 47 ++++++++ include/mujoco/mjxmacro.h | 3 +- include/mujoco/mujoco.h | 16 +++ introspect/ast_nodes.py | 1 + introspect/functions.py | 72 +++++++++++++ introspect/structs.py | 35 ++++++ introspect/type_parsing.py | 7 +- src/engine/engine_io.c | 4 + src/engine/engine_print.c | 13 ++- src/thread/CMakeLists.txt | 23 ++++ src/thread/lockless_queue.h | 154 +++++++++++++++++++++++++++ src/thread/task.cc | 24 +++++ src/thread/task.h | 69 ++++++++++++ src/thread/thread_pool.cc | 52 +++++++++ src/thread/thread_pool.h | 103 ++++++++++++++++++ test/thread/lockless_queue_test.cc | 46 ++++++++ test/thread/mjthread_test.cc | 90 ++++++++++++++++ test/thread/task_test.cc | 46 ++++++++ test/thread/thread_pool_test.cc | 134 +++++++++++++++++++++++ unity/Runtime/Bindings/MjBindings.cs | 21 ++++ 28 files changed, 1046 insertions(+), 3 deletions(-) create mode 100644 include/mujoco/mjthread.h create mode 100644 src/thread/CMakeLists.txt create mode 100644 src/thread/lockless_queue.h create mode 100644 src/thread/task.cc create mode 100644 src/thread/task.h create mode 100644 src/thread/thread_pool.cc create mode 100644 src/thread/thread_pool.h create mode 100644 test/thread/lockless_queue_test.cc create mode 100644 test/thread/mjthread_test.cc create mode 100644 test/thread/task_test.cc create mode 100644 test/thread/thread_pool_test.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 1caaa05c..bc6c5126 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/doc/APIreference/APIfunctions.rst b/doc/APIreference/APIfunctions.rst index fca904c7..ce8309ea 100644 --- a/doc/APIreference/APIfunctions.rst +++ b/doc/APIreference/APIfunctions.rst @@ -32,6 +32,7 @@ API function can be classified as: - :ref:`Derivatives`. - :ref:`Plugin` related functions. - :ref:`Macros`. +- :ref:`Thread` related functions. .. TODO(b/273075045): Better category-label namespacing. diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index 81a5ff17..d8ff40ad 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -712,7 +712,26 @@ Options for configuring the automatic :ref:`actuator length-range computation`__ Defines data structures required by :ref:`engine plugins`. +`mjthread.h `__ + Defines data structures and functions required by :ref:`thread`. .. _inVersion: diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 51522ca3..408e62f8 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -402,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; diff --git a/include/mujoco/mjthread.h b/include/mujoco/mjthread.h new file mode 100644 index 00000000..1538e954 --- /dev/null +++ b/include/mujoco/mjthread.h @@ -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 + +#include + +// 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[48]; +}; +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_ diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index bccfcde2..2ef1c7ee 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -644,7 +644,8 @@ X( int, nnzJ ) \ X( int, ncon ) \ X( int, nisland ) \ - X( mjtNum, time ) + X( mjtNum, time ) \ + X( uintptr_t, threadpool ) // vector fields of mjData diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index ce813376..c1c6eb41 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -36,6 +36,7 @@ extern "C" { #include #include #include +#include #include #include #include @@ -1304,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) } diff --git a/introspect/ast_nodes.py b/introspect/ast_nodes.py index 926cff42..9aa58c00 100644 --- a/introspect/ast_nodes.py +++ b/introspect/ast_nodes.py @@ -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: diff --git a/introspect/functions.py b/introspect/functions.py index 984f4eda..cf2417d2 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -8257,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.', + )), ]) diff --git a/introspect/structs.py b/introspect/structs.py index 1573fb0c..4bdc211c 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -4475,6 +4475,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', @@ -6978,6 +6983,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=(48,), + ), + 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', diff --git a/introspect/type_parsing.py b/introspect/type_parsing.py index 80bf9668..dfc2ab53 100644 --- a/introspect/type_parsing.py +++ b/introspect/type_parsing.py @@ -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('(')]) diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index f713c718..5df0f59e 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1114,6 +1114,8 @@ static mjData* _makeData(const mjModel* m) { } } + d->threadpool = 0; + return d; } @@ -1202,6 +1204,8 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) { } } + dest->threadpool = src->threadpool; + return dest; } diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index d9455c4d..6e215a9c 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -778,7 +778,8 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, #define X(type, name) \ if (strcmp(#name, "pstack") != 0 && \ strcmp(#name, "pbase") != 0 && \ - strcmp(#name, "parena") != 0) { \ + strcmp(#name, "parena") != 0 && \ + strcmp(#name, "threadpool") != 0) { \ const char* format = _Generic( \ d->name, \ int : INT_FORMAT, \ @@ -794,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 diff --git a/src/thread/CMakeLists.txt b/src/thread/CMakeLists.txt new file mode 100644 index 00000000..948cd2e7 --- /dev/null +++ b/src/thread/CMakeLists.txt @@ -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}) diff --git a/src/thread/lockless_queue.h b/src/thread/lockless_queue.h new file mode 100644 index 00000000..fbe21b96 --- /dev/null +++ b/src/thread/lockless_queue.h @@ -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 +#include +#include +#include + +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 +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 read_cursor_ = 0; + std::atomic write_cursor_ = 0; + std::atomic maximum_read_cursor_ = 0; + + std::atomic buffer_[(buffer_capacity + 1)]; +}; + +} // namespace mujoco + +#endif // MUJOCO_SRC_THREAD_LOCKLESS_QUEUE_H_ diff --git a/src/thread/task.cc b/src/thread/task.cc new file mode 100644 index 00000000..acb1858b --- /dev/null +++ b/src/thread/task.cc @@ -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 +#include + +// waits for a task to complete +void mju_taskJoin(mjTask* task) { + mujoco::Task* task_ptr = static_cast(static_cast(task)); + task_ptr->Join(); +} diff --git a/src/thread/task.h b/src/thread/task.h new file mode 100644 index 00000000..390a4571 --- /dev/null +++ b/src/thread/task.h @@ -0,0 +1,69 @@ +// 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 +#include +#include + +namespace mujoco { + +class Task { + public: + enum Status { + QUEUED, + COMPLETE, + }; + + static void Initialize( + Task* task, + std::function 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: + std::function start_routine_; + + void* args_; + + std::atomic status_ = Status::QUEUED; +}; + +} // namespace mujoco + +#endif // __cplusplus + +#endif // MUJOCO_SRC_THREAD_TASK_H_ diff --git a/src/thread/thread_pool.cc b/src/thread/thread_pool.cc new file mode 100644 index 00000000..dd060737 --- /dev/null +++ b/src/thread/thread_pool.cc @@ -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 + +#include +#include +#include "thread/task.h" + +static constexpr size_t kMaxThreads = 128; + +// create a thread pool +mjThreadPool* mju_threadPoolCreate(size_t number_of_threads) { + mujoco::ThreadPool* thread_pool = + new mujoco::ThreadPool(number_of_threads); + return static_cast(static_cast(thread_pool)); +} + +// start a task in the threadpool +void mju_threadPoolEnqueue( + mjThreadPool* thread_pool, mjTask* task, mjStartRoutine start_routine, + void* args) { + mujoco::ThreadPool* thread_pool_ptr = + static_cast*>( + static_cast(thread_pool)); + thread_pool_ptr->Enqueue( + static_cast(static_cast(task)), start_routine, + args); +} + +// shutdown the threadpool and free the memory +void mju_threadPoolDestroy(mjThreadPool* thread_pool) { + mujoco::ThreadPool* thread_pool_ptr = + static_cast*>( + static_cast(thread_pool)); + thread_pool_ptr->Shutdown(); + delete thread_pool_ptr; +} + diff --git a/src/thread/thread_pool.h b/src/thread/thread_pool.h new file mode 100644 index 00000000..d8526d54 --- /dev/null +++ b/src/thread/thread_pool.h @@ -0,0 +1,103 @@ +// 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 +#include +#include +#include + +#include "thread/lockless_queue.h" +#include "thread/task.h" + +namespace mujoco { + +static constexpr size_t kThreadPoolQueueSize = 640; + +template +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(this)); + } + } + + // start a task in the threadpool + void Enqueue( + Task* task, std::function start_routine, void* args) { + Task::Initialize(task, start_routine, args); + lockless_queue_.push(static_cast(task)); + } + + // shutdown the threadpool + void Shutdown() { + if (shutdown_) { + return; + } + + shutdown_ = true; + Task shutdown_tasks[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* thread_pool = + static_cast*>(arg); + while (!thread_pool->shutdown_) { + Task* task = static_cast(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 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 lockless_queue_; +}; + +} // namespace mujoco + +#endif // __cplusplus + +#endif // MUJOCO_SRC_THREAD_THREAD_POOL_H_ diff --git a/test/thread/lockless_queue_test.cc b/test/thread/lockless_queue_test.cc new file mode 100644 index 00000000..774f0ac4 --- /dev/null +++ b/test/thread/lockless_queue_test.cc @@ -0,0 +1,46 @@ +// 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 "src/thread/lockless_queue.h" + +#include + +#include + +namespace mujoco { +namespace { + +constexpr size_t kBufferCapacity = 640; + +TEST(TestMujocoLocklessQueue, TestMujocoLocklessQueue) { + LocklessQueue queue; + EXPECT_TRUE(queue.empty()); + int test_integers[kBufferCapacity]; + for (int h = 0; h < 10; ++h) { + for (int i = 0; i < kBufferCapacity; ++i) { + test_integers[i] = i; + queue.push(&test_integers[i]); + } + EXPECT_TRUE(queue.full()); + + for (int i = 0; i < kBufferCapacity; ++i) { + void* output_ptr = queue.pop(); + ASSERT_EQ(output_ptr, &test_integers[i]); + } + EXPECT_TRUE(queue.empty()); + } +} + +} // namespace +} // namespace mujoco diff --git a/test/thread/mjthread_test.cc b/test/thread/mjthread_test.cc new file mode 100644 index 00000000..ba3ee7e9 --- /dev/null +++ b/test/thread/mjthread_test.cc @@ -0,0 +1,90 @@ +// 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 + +#include + +#include +#include +#include "src/thread/task.h" +#include "src/thread/thread_pool.h" + +namespace { + +struct TestFunctionArgs_ { + int input; + // make this atomic to avoid red-herring tsan failures. + std::atomic output; +}; +typedef struct TestFunctionArgs_ TestFunctionArgs; + +void* test_function(void* args) { + TestFunctionArgs* test_function_args = static_cast(args); + if (!test_function_args) { + return nullptr; + } + test_function_args->output = test_function_args->input; + return nullptr; +} + +TEST(TestMjThreadPool, EnsureStructClassSizeMatch) { + EXPECT_EQ(sizeof(mjTask), sizeof(mujoco::Task)); + EXPECT_EQ(sizeof(mjThreadPool), sizeof(mujoco::ThreadPool<128>)); +} + +TEST(TestMjThreadPool, TestMjThreadPool10Threads) { + mjThreadPool* thread_pool = mju_threadPoolCreate(10); + + TestFunctionArgs test_function_args[1000]; + mjTask tasks[1000]; + for (int i = 0; i < 1000; ++i) { + test_function_args[i].input = i; + mju_threadPoolEnqueue(thread_pool, &tasks[i], test_function, + (void*)&test_function_args[i]); + } + + for (int i = 0; i < 1000; ++i) { + mju_taskJoin(&tasks[i]); + } + + for (int i = 0; i < 1000; ++i) { + EXPECT_EQ(test_function_args[i].input, test_function_args[i].output); + } + mju_threadPoolDestroy(thread_pool); +} + +TEST(TestMjThreadPool, TestMjThreadPool100Threads) { + mjThreadPool* thread_pool = mju_threadPoolCreate(100); + + TestFunctionArgs test_function_args[1000]; + mjTask tasks[1000]; + for (int i = 0; i < 1000; ++i) { + test_function_args[i].input = i; + mju_threadPoolEnqueue(thread_pool, &tasks[i], test_function, + (void*)&test_function_args[i]); + } + + for (int i = 0; i < 1000; ++i) { + mju_taskJoin(&tasks[i]); + } + + for (int i = 0; i < 1000; ++i) { + EXPECT_EQ(test_function_args[i].input, test_function_args[i].output); + } + + mju_threadPoolDestroy(thread_pool); +} + +} // namespace diff --git a/test/thread/task_test.cc b/test/thread/task_test.cc new file mode 100644 index 00000000..7bd53274 --- /dev/null +++ b/test/thread/task_test.cc @@ -0,0 +1,46 @@ +// 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 "src/thread/task.h" + +#include + +namespace mujoco { +namespace { + +struct TestFunctionArgs { + int input; + int output; +}; + +void* test_function(void* args) { + TestFunctionArgs* test_function_args = (TestFunctionArgs*)args; + test_function_args->output = test_function_args->input; + return nullptr; +} + +TEST(TestMjThread, TestMjThread) { + TestFunctionArgs test_function_args; + test_function_args.input = 1; + test_function_args.output = 2; + Task task; + Task::Initialize( + &task, test_function, static_cast(&test_function_args)); + task.Execute(); + task.Join(); + EXPECT_EQ(test_function_args.input, test_function_args.output); +} + +} // namespace +} // namespace mujoco diff --git a/test/thread/thread_pool_test.cc b/test/thread/thread_pool_test.cc new file mode 100644 index 00000000..a488e21f --- /dev/null +++ b/test/thread/thread_pool_test.cc @@ -0,0 +1,134 @@ +// 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 "src/thread/thread_pool.h" + +#include +#include +#include +#include +#include + +#include +#include "src/thread/task.h" + +namespace mujoco { +namespace { + +struct TestFunctionArgs { + int input; + // make this atomic to avoid red-herring tsan failures. + std::atomic output; +}; + +void* test_function(void* args) { + TestFunctionArgs* test_function_args = static_cast(args); + test_function_args->output = test_function_args->input; + return nullptr; +} + +TEST(TestMjThreadPool, TestMjThreadPool10Threads) { + ThreadPool<10> thread_pool(10); + + constexpr int kTasks = 1000; + TestFunctionArgs test_function_args[kTasks]; + Task tasks[kTasks]; + for (int i = 0; i < kTasks; ++i) { + test_function_args[i].input = i; + thread_pool.Enqueue( + &tasks[i], test_function, static_cast(&test_function_args[i])); + } + + for (int i = 0; i < kTasks; ++i) { + tasks[i].Join(); + } + + for (int i = 0; i < kTasks; ++i) { + EXPECT_EQ(test_function_args[i].input, test_function_args[i].output); + } + + thread_pool.Shutdown(); +} + +TEST(TestMjThreadPool, TestMjThreadPool100Threads) { + ThreadPool<100> thread_pool(100); + + constexpr int kTasks = 1000; + TestFunctionArgs test_function_args[kTasks]; + Task tasks[kTasks]; + for (int i = 0; i < kTasks; ++i) { + test_function_args[i].input = i; + thread_pool.Enqueue( + &tasks[i], test_function, static_cast(&test_function_args[i])); + } + + for (int i = 0; i < kTasks; ++i) { + tasks[i].Join(); + } + + for (int i = 0; i < kTasks; ++i) { + EXPECT_EQ(test_function_args[i].input, test_function_args[i].output); + } + + thread_pool.Shutdown(); +} + +TEST(TestMjThreadPool, TestMjThreadPoolManyWriters) { + ThreadPool<10> thread_pool(10); + + constexpr int kTasks = 20; + TestFunctionArgs test_function_args[kTasks]; + Task tasks[kTasks]; + std::unique_ptr enqueue_threads[kTasks]; + + // add tasks to the thread pool from many threads + std::condition_variable start_cv; + std::mutex start_mutex; + bool start = false; + for (int i = 0; i < kTasks; ++i) { + test_function_args[i].input = i; + enqueue_threads[i] = std::make_unique([&, i] { + // synchronize all threads adding to the thread_pool at the same time + { + std::unique_lock lock(start_mutex); + start_cv.wait(lock, [&] { return start; }); + } + // enqueue outside the lock, to get some concurrency + thread_pool.Enqueue( + &tasks[i], test_function, static_cast(&test_function_args[i])); + }); + } + { + std::unique_lock lock(start_mutex); + start = true; + } + start_cv.notify_all(); + + for (int i = 0; i < kTasks; ++i) { + enqueue_threads[i]->join(); + } + + for (int i = 0; i < kTasks; ++i) { + tasks[i].Join(); + } + + for (int i = 0; i < kTasks; ++i) { + EXPECT_EQ(test_function_args[i].input, test_function_args[i].output); + } + + thread_pool.Shutdown(); +} + +} // namespace +} // namespace mujoco diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index e886c3c5..37a1278a 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -56,6 +56,7 @@ public const bool mjEXTERNC = true; public const bool THIRD_PARTY_MUJOCO_MJRENDER_H_ = true; public const int mjNAUX = 10; public const int mjMAXTEXTURE = 1000; +public const bool THIRD_PARTY_MUJOCO_INCLUDE_MJTHREAD_H_ = true; public const bool THIRD_PARTY_MUJOCO_INCLUDE_MJTNUM_H_ = true; public const bool mjUSEDOUBLE = true; public const double mjMINVAL = 1e-15; @@ -1736,6 +1737,7 @@ public unsafe struct mjData_ { public double* efc_b; public double* efc_force; public int* efc_state; + public UIntPtr threadpool; } [StructLayout(LayoutKind.Sequential)] @@ -2354,6 +2356,16 @@ public unsafe struct mjrContext_ { public int readPixelFormat; } +[StructLayout(LayoutKind.Sequential)] +public unsafe struct mjTask_ { + public fixed sbyte buffer[48]; +} + +[StructLayout(LayoutKind.Sequential)] +public unsafe struct mjThreadPool_ { + public fixed sbyte buffer[6208]; +} + [StructLayout(LayoutKind.Sequential)] public unsafe struct mjuiState_ { public int nrect; @@ -3930,5 +3942,14 @@ public static unsafe extern void mjd_subQuat(double* qa, double* qb, double* Da, [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mjd_quatIntegrate(double* vel, double scale, double* Dquat, double* Dvel, double* Dscale); + +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern mjThreadPool_* mju_threadPoolCreate(UIntPtr number_of_threads); + +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mju_taskJoin(mjTask_* task); + +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mju_threadPoolDestroy(mjThreadPool_* thread_pool); } } From edbe9ce86fbf74073ef5ae7fb612d7c36a7472fd Mon Sep 17 00:00:00 2001 From: Matthew Bennice Date: Tue, 5 Sep 2023 05:05:05 -0700 Subject: [PATCH 30/38] Correct use of number_of_threads_ in an array length definition. It should be max_number_of_threads to allow Windows to compile it, GCC tolerates the variable length array. PiperOrigin-RevId: 562743538 Change-Id: I5dc6dfb54002a996fe129b91e599cccfc9420e9c --- src/thread/thread_pool.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/thread/thread_pool.h b/src/thread/thread_pool.h index d8526d54..e987318e 100644 --- a/src/thread/thread_pool.h +++ b/src/thread/thread_pool.h @@ -55,7 +55,7 @@ class ThreadPool { } shutdown_ = true; - Task shutdown_tasks[number_of_threads_]; + Task shutdown_tasks[max_number_of_threads]; for (int i = 0; i < number_of_threads_; ++i) { Enqueue(&shutdown_tasks[i], ShutdownFunction, nullptr); } From f976d6d228612c6dd863ee4e094ae9d7452fcfa9 Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Tue, 5 Sep 2023 06:28:58 -0700 Subject: [PATCH 31/38] Use a raw function pointer instead of `std::function` objects in thread pool. std::function can have an allocation cost when it's created, and also adds overhead to function calls. Since this API must be used from C, using a raw function pointer is good enough. PiperOrigin-RevId: 562760324 Change-Id: I2b714a13e07a2b71566d013f21f76c506e817a52 --- doc/includes/references.h | 2 +- include/mujoco/mjthread.h | 2 +- introspect/structs.py | 2 +- src/thread/task.h | 6 +++--- src/thread/thread_pool.h | 3 +-- unity/Runtime/Bindings/MjBindings.cs | 2 +- 6 files changed, 8 insertions(+), 9 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 34d9f187..d91fa3f2 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1440,7 +1440,7 @@ struct mjrContext_ { // custom OpenGL context }; typedef struct mjrContext_ mjrContext; struct mjTask_ { - char buffer[48]; + char buffer[24]; }; typedef struct mjTask_ mjTask; struct mjThreadPool_ { diff --git a/include/mujoco/mjthread.h b/include/mujoco/mjthread.h index 1538e954..66290370 100644 --- a/include/mujoco/mjthread.h +++ b/include/mujoco/mjthread.h @@ -27,7 +27,7 @@ extern "C" { // 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[48]; + char buffer[24]; }; typedef struct mjTask_ mjTask; diff --git a/introspect/structs.py b/introspect/structs.py index 4bdc211c..d399c6bc 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -6992,7 +6992,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ name='buffer', type=ArrayType( inner_type=ValueType(name='char'), - extents=(48,), + extents=(24,), ), doc='', ), diff --git a/src/thread/task.h b/src/thread/task.h index 390a4571..296592a5 100644 --- a/src/thread/task.h +++ b/src/thread/task.h @@ -20,13 +20,13 @@ #ifdef __cplusplus #include -#include #include namespace mujoco { class Task { public: + using FunctionPtr = void* (*)(void*); enum Status { QUEUED, COMPLETE, @@ -34,7 +34,7 @@ class Task { static void Initialize( Task* task, - std::function start_routine, + FunctionPtr start_routine, void* args) { // instantiate a task at the pointer passed in new(task) Task(); @@ -55,7 +55,7 @@ class Task { } private: - std::function start_routine_; + FunctionPtr start_routine_; void* args_; diff --git a/src/thread/thread_pool.h b/src/thread/thread_pool.h index e987318e..11b51145 100644 --- a/src/thread/thread_pool.h +++ b/src/thread/thread_pool.h @@ -21,7 +21,6 @@ #include #include -#include #include #include "thread/lockless_queue.h" @@ -43,7 +42,7 @@ class ThreadPool { // start a task in the threadpool void Enqueue( - Task* task, std::function start_routine, void* args) { + Task* task, Task::FunctionPtr start_routine, void* args) { Task::Initialize(task, start_routine, args); lockless_queue_.push(static_cast(task)); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 37a1278a..0a646168 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -2358,7 +2358,7 @@ public unsafe struct mjrContext_ { [StructLayout(LayoutKind.Sequential)] public unsafe struct mjTask_ { - public fixed sbyte buffer[48]; + public fixed sbyte buffer[24]; } [StructLayout(LayoutKind.Sequential)] From d5292976de7aed080672b0a23b5617271d040ff6 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 5 Sep 2023 07:13:37 -0700 Subject: [PATCH 32/38] Add internal utility for zeroing int vectors. Remove unnecessary branches in related functions. PiperOrigin-RevId: 562768873 Change-Id: I3964a7a953d95bcd4e29137a7b590be5ffa6e311 --- src/engine/engine_core_constraint.c | 11 +++++------ src/engine/engine_io.c | 2 +- src/engine/engine_island.c | 3 +-- src/engine/engine_solver.c | 10 +++++----- src/engine/engine_support.c | 4 +--- src/engine/engine_util_blas.c | 8 ++------ src/engine/engine_util_misc.c | 11 ++++++++--- src/engine/engine_util_misc.h | 3 +++ src/engine/engine_util_solve.c | 3 +-- src/engine/engine_util_sparse.c | 6 +++--- 10 files changed, 30 insertions(+), 31 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 98857258..b44ae2b3 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -16,7 +16,6 @@ #include #include -#include #include #include @@ -252,7 +251,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); } } @@ -644,11 +643,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 { @@ -1530,10 +1529,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); } } } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 5df0f59e..e78b4a37 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -874,7 +874,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]--; diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 2c0d7a6d..0c6771a8 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -16,7 +16,6 @@ #include #include -#include #include #include @@ -76,7 +75,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]; } diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index dd058a04..8e4ab123 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -464,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++) { @@ -680,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++) { @@ -1314,7 +1314,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, @@ -1476,7 +1476,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, @@ -1563,7 +1563,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 diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 14cd04d5..44a1a20c 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -14,8 +14,6 @@ #include "engine/engine_support.h" -#include - #include #include #include @@ -1161,7 +1159,7 @@ void mj_copyM2DSparse(const mjModel* m, mjData* d, mjtNum* dst, const mjtNum* sr // 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--) { diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index f0b19daa..31a77a58 100644 --- a/src/engine/engine_util_blas.c +++ b/src/engine/engine_util_blas.c @@ -238,9 +238,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 +254,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)); } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index a11bbd00..924d0bfc 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -1209,9 +1209,14 @@ int mju_isZero(mjtNum* vec, int n) { // set integer vector to 0 void mju_zeroInt(int* res, int n) { - if (n > 0) { - memset(res, 0, n*sizeof(int)); - } + 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)); } diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index c3150c88..08ca64fa 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -122,6 +122,9 @@ 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); diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 933b2d90..d5dafbb5 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -16,7 +16,6 @@ #include #include -#include #include #include @@ -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--) { diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index af1909ae..cde53635 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -273,8 +273,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 @@ -599,7 +599,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' From 8ffdee355a08df22340dfe8de498486d92dc6fb9 Mon Sep 17 00:00:00 2001 From: Matthew Bennice Date: Tue, 5 Sep 2023 09:20:01 -0700 Subject: [PATCH 33/38] Add new header to correct warning for placement new PiperOrigin-RevId: 562800238 Change-Id: I94c883488391f7e95347eb2fe8dc291125233fff --- src/thread/task.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/thread/task.h b/src/thread/task.h index 296592a5..1a4f8c28 100644 --- a/src/thread/task.h +++ b/src/thread/task.h @@ -20,6 +20,7 @@ #ifdef __cplusplus #include +#include #include namespace mujoco { From 329ed193acb7e22ec51a4883ef87b03d484292ad Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Tue, 5 Sep 2023 11:04:12 -0700 Subject: [PATCH 34/38] Disallow variable-length arrays and turn on `-Wpedantic` in Clang and GCC. PiperOrigin-RevId: 562832747 Change-Id: I0532624cee31e089a47426305d885687eb0fc5c5 --- cmake/MujocoOptions.cmake | 13 +++---- doc/includes/references.h | 6 +-- include/mujoco/mujoco.h | 6 +-- sample/cmake/SampleOptions.cmake | 13 +++---- simulate/cmake/SimulateOptions.cmake | 13 +++---- src/engine/engine_crossplatform.c | 4 +- src/engine/engine_plugin.h | 4 +- src/engine/engine_support.c | 9 +++-- src/engine/engine_support.h | 5 ++- src/engine/engine_util_errmem.c | 29 +++++++++------ src/engine/engine_util_errmem.h | 51 +++++++------------------- src/render/glad/glad.c | 17 +++++++-- src/render/render_context.c | 4 +- src/user/user_mesh.cc | 21 ++++++++++- src/user/user_objects.cc | 11 ++---- test/engine/engine_util_errmem_test.cc | 10 ++++- unity/Runtime/Bindings/MjBindings.cs | 4 ++ 17 files changed, 121 insertions(+), 99 deletions(-) diff --git a/cmake/MujocoOptions.cmake b/cmake/MujocoOptions.cmake index 998b966f..3e3f080f 100644 --- a/cmake/MujocoOptions.cmake +++ b/cmake/MujocoOptions.cmake @@ -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() diff --git a/doc/includes/references.h b/doc/includes/references.h index d91fa3f2..da9f1560 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2318,7 +2318,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); @@ -2575,12 +2575,12 @@ 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); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index c1c6eb41..df70de12 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -488,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 ------------------------------------------------ @@ -1278,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); @@ -1295,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. diff --git a/sample/cmake/SampleOptions.cmake b/sample/cmake/SampleOptions.cmake index 998b966f..3e3f080f 100644 --- a/sample/cmake/SampleOptions.cmake +++ b/sample/cmake/SampleOptions.cmake @@ -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() diff --git a/simulate/cmake/SimulateOptions.cmake b/simulate/cmake/SimulateOptions.cmake index 998b966f..3e3f080f 100644 --- a/simulate/cmake/SimulateOptions.cmake +++ b/simulate/cmake/SimulateOptions.cmake @@ -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() diff --git a/src/engine/engine_crossplatform.c b/src/engine/engine_crossplatform.c index c7f5f6b7..e031f30a 100644 --- a/src/engine/engine_crossplatform.c +++ b/src/engine/engine_crossplatform.c @@ -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 @@ -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); diff --git a/src/engine/engine_plugin.h b/src/engine/engine_plugin.h index 13068a4a..d65b2cf9 100644 --- a/src/engine/engine_plugin.h +++ b/src/engine/engine_plugin.h @@ -32,10 +32,10 @@ MJAPI int mjp_registerPlugin(const mjpPlugin* plugin); MJAPI int mjp_registerResourceProvider(const mjpResourceProvider* provider); // 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); diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 44a1a20c..cf7456c3 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -14,10 +14,13 @@ #include "engine/engine_support.h" +#include +#include +#include + #include #include #include -#include "engine/engine_array_safety.h" #include "engine/engine_core_constraint.h" #include "engine/engine_crossplatform.h" #include "engine/engine_io.h" @@ -1056,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; @@ -1584,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; } diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index acacbeca..6424115a 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -15,9 +15,12 @@ #ifndef MUJOCO_SRC_ENGINE_ENGINE_SUPPORT_H_ #define MUJOCO_SRC_ENGINE_ENGINE_SUPPORT_H_ +#include + #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -197,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 diff --git a/src/engine/engine_util_errmem.c b/src/engine/engine_util_errmem.c index 9ed469fb..7c63c1ce 100644 --- a/src/engine/engine_util_errmem.c +++ b/src/engine/engine_util_errmem.c @@ -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; diff --git a/src/engine/engine_util_errmem.h b/src/engine/engine_util_errmem.h index 44f1aec5..60d7b7e1 100644 --- a/src/engine/engine_util_errmem.h +++ b/src/engine/engine_util_errmem.h @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -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 --------------------------------------------------- diff --git a/src/render/glad/glad.c b/src/render/glad/glad.c index 5987fc3a..9b5e4c6d 100644 --- a/src/render/glad/glad.c +++ b/src/render/glad/glad.c @@ -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 diff --git a/src/render/render_context.c b/src/render/render_context.c index a1b1fd07..18c71805 100644 --- a/src/render/render_context.c +++ b/src/render/render_context.c @@ -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; } diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 548819ab..57ba191f 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -13,12 +13,14 @@ // limitations under the License. #include +#include #include #include #include #include #include #include +#include #include #include #include @@ -27,11 +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 +#if defined(__clang__) +#pragma clang diagnostic pop +#elif defined(__GNUC__) +#pragma GCC diagnostic pop +#endif + +#include #include +#include #include -#include "cc/array_safety.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" @@ -1838,7 +1856,6 @@ mjCSkin::~mjCSkin() { // compiler void mjCSkin::Compile(const mjVFS* vfs) { - // load file if (!file.empty()) { // make sure data is not present diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 75eca814..df06cafe 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -14,22 +14,22 @@ #include "user/user_objects.h" +#include #include #include #include #include -#include #include #include +#include #include #include "lodepng.h" #include #include #include +#include #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" @@ -131,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 @@ -1514,7 +1514,6 @@ void mjCGeom::SetFluidCoefs(void) { // get semiaxes switch (type) { - case mjGEOM_SPHERE: dx = size[0]; dy = size[0]; @@ -4497,7 +4496,6 @@ mjCKey::~mjCKey() { // compiler void mjCKey::Compile(const mjModel* m) { - // qpos: allocate or check size if (qpos.empty()) { qpos.resize(m->nq); @@ -4572,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); } - } diff --git a/test/engine/engine_util_errmem_test.cc b/test/engine/engine_util_errmem_test.cc index 4e9dd62f..1b398f5d 100644 --- a/test/engine/engine_util_errmem_test.cc +++ b/test/engine/engine_util_errmem_test.cc @@ -23,7 +23,7 @@ namespace mujoco { namespace { -constexpr int kBufferSize = 1000; +constexpr int kBufferSize = 1024; char* ErrorMessageBuffer() { static char error_message[kBufferSize] = ""; @@ -132,5 +132,13 @@ TEST_F(MujocoErrorAndWarningTest, MjuWarningS) { EXPECT_EQ(std::string(WarningMessageBuffer()), expected_message); } +TEST_F(MujocoErrorAndWarningTest, MjuErrorInternal) { + ClearErrorMessage(); + mjERROR("foobar %d", 123); + std::string funcname(__func__); + ASSERT_TRUE(funcname.length()); + EXPECT_EQ(std::string(ErrorMessageBuffer()), funcname + ": foobar 123"); +} + } // namespace } // namespace mujoco diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 0a646168..4891f9f8 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -3369,6 +3369,10 @@ public static unsafe extern void mj_loadPluginLibrary([MarshalAs(UnmanagedType.L [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern int mj_version(); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +[return: MarshalAs(UnmanagedType.LPStr)] +public static unsafe extern string mj_versionString(); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_multiRay(mjModel_* m, mjData_* d, double* pnt, double* vec, byte* geomgroup, byte flg_static, int bodyexclude, int* geomid, double* dist, int nray, double cutoff); From 49290772117f9c7b835519b075145e05c5cc117c Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Tue, 5 Sep 2023 11:41:01 -0700 Subject: [PATCH 35/38] Remove `mjMARKSTACK` and `mjFREESTACK` macros. PiperOrigin-RevId: 562844184 Change-Id: Id2f57f5d132c47094dad75c6dab7297d7aa73458 --- doc/APIreference/APIfunctions.rst | 33 -------- doc/changelog.rst | 78 ++++++++++++------- doc/includes/references.h | 6 +- doc/programming/simulation.rst | 12 ++- include/mujoco/mjmacro.h | 4 - include/mujoco/mujoco.h | 6 +- introspect/functions.py | 6 +- plugin/sensor/touch_grid.cc | 18 ++--- sample/derivative.cc | 8 +- src/engine/engine_collision_driver.c | 12 +-- src/engine/engine_collision_sdf.c | 7 +- src/engine/engine_core_constraint.c | 30 ++++--- src/engine/engine_core_smooth.c | 20 ++--- src/engine/engine_derivative.c | 24 +++--- src/engine/engine_derivative_fd.c | 20 ++--- src/engine/engine_forward.c | 24 +++--- src/engine/engine_inverse.c | 18 ++--- src/engine/engine_io.c | 8 +- src/engine/engine_island.c | 8 +- src/engine/engine_island.h | 2 +- src/engine/engine_print.c | 6 +- src/engine/engine_ray.c | 5 +- src/engine/engine_sensor.c | 4 +- src/engine/engine_setconst.c | 8 +- src/engine/engine_solver.c | 26 +++---- src/engine/engine_support.c | 24 +++--- src/engine/engine_util_blas.c | 1 - src/engine/engine_util_solve.c | 8 +- src/engine/engine_util_sparse.c | 12 ++- src/engine/engine_vis_interact.c | 7 +- .../engine_core_smooth_benchmark_test.cc | 8 +- .../engine_util_sparse_benchmark_test.cc | 23 +++--- test/engine/engine_derivative_test.cc | 7 +- test/engine/engine_util_container_test.cc | 8 +- unity/Runtime/Bindings/MjBindings.cs | 6 +- 35 files changed, 239 insertions(+), 258 deletions(-) diff --git a/doc/APIreference/APIfunctions.rst b/doc/APIreference/APIfunctions.rst index ce8309ea..48464ae2 100644 --- a/doc/APIreference/APIfunctions.rst +++ b/doc/APIreference/APIfunctions.rst @@ -43,39 +43,6 @@ API function can be classified as: Macros ^^^^^^ - -.. _mjMARKSTACK: - -mjMARKSTACK -~~~~~~~~~~~ - -.. code-block:: C - - #define mjMARKSTACK int _mark = d->pstack; - -This macro is helpful when using the MuJoCo stack in custom computations. It works together with the next macro and the -:ref:`mj_stackAlloc` family of functions, and assumes that mjData\* d is defined. The use pattern is this: - -.. code-block:: C - - mjMARKSTACK; - mjtNum* temp = mj_stackAllocNum(d, 100); - // ... use temp as needed - mjFREESTACK; - - -.. _mjFREESTACK: - -mjFREESTACK -~~~~~~~~~~~ - -.. code-block:: C - - #define mjFREESTACK d->pstack = _mark; - -Reset the MuJoCo stack pointer to the variable \_mark, normally saved by mjMARKSTACK. - - .. _mjDISABLED: mjDISABLED diff --git a/doc/changelog.rst b/doc/changelog.rst index 2a98a3be..068db417 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,58 +5,80 @@ Changelog Upcoming version (not yet released) ----------------------------------- -General -^^^^^^^ +New features +^^^^^^^^^^^^ + +.. youtube:: Vc1tq0fFvQA + :align: right + :width: 240px + +1. Added constraint island discovery in :ref:`mj_island`. Constraint islands are disjoint sets of constraints + and degrees-of-freedom that do not interact. In a future release the constraint solver will be refactored to + exploit the disjoint structure. Island discovery can be activated using a new :ref:`enable flag` + which will be removed after the refactor. If island discovery is enabled, geoms, contacts and + tendons will be colored according to the corresponding island, see video. .. youtube:: QewlEqIZi1o :align: right :width: 240px -1. Added new signed distance field (SDF) collision primitive. SDFs can take any shape and are not constrained to be +2. Added new signed distance field (SDF) collision primitive. SDFs can take any shape and are not constrained to be convex. Collision points are found by minimizing the maximum of the two colliding SDFs via gradient descent. - Added new SDF plugin for defining implicit geometries. The plugin must define methods computing an SDF and its gradient at query points See the :ref:`documentation` for more details. - .. youtube:: Vc1tq0fFvQA - :align: right - :width: 240px +3. Added :ref:`mjThreadPool` and :ref:`mjTask` which allow for multi-threaded operations within MuJoCo engine pipeline. -#. Added constraint island discovery in :ref:`mj_island`. Constraint islands are disjoint sets of constraints - and degrees-of-freedom that do not interact. In a future release the constraint solver will be refactored to - exploit the disjoint structure. Island discovery can be activated using a new :ref:`enable flag` - which will be removed after the refactor. If island discovery is enabled, geoms, contacts and - tendons will be colored according to the corresponding island, see video. -#. Added a new :ref:`dyntype`, ``filterexact``, which updates first-order filter states with +General +^^^^^^^ + +.. admonition:: Breaking API changes + :class: attention + + 4. Removed macros ``mjMARKSTACK`` and ``mjFREESTACK``. + + .. admonition:: Migration note + :class: note + + These macros have been replaced by new functions :ref:`mj_markStack` and :ref:`mj_freeStack`. These functions + manages ``mjData`` stack frames in a fully encapsulated way (i.e. without having to introduce a local variable + at the call site). + + 5. Changed the function :ref:`mj_stackAlloc` to allocate an arbitrary number of bytes, rather than in multiples of + ``sizeof(mjtNum)``, and add an additional argument for specifying the alignment of the returned pointer. + + .. admonition:: Migration note + :class: note + + The old functionality for allocating ``mjtNum`` arrays is still available through a new function + :ref:`mj_stackAllocNum`. + + 6. Renamed the ``nstack`` field in :ref:`mjModel` and :ref:`mjData` to ``narena``. Changed ``narena``, ``pstack``, + and ``maxuse_stack`` to count number of bytes rather than number of :ref:`mjtNum` |-| s. + +7. Added a new :ref:`dyntype`, ``filterexact``, which updates first-order filter states with the exact formula rather than with Euler integration. -#. Added an actuator attribute, :ref:`actearly`, which uses semi-implicit integration for +8. Added an actuator attribute, :ref:`actearly`, which uses semi-implicit integration for actuator forces: using the next step's actuator state to compute the current actuator forces at the current timestep. -#. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to +9. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to :ref:`actuatorfrcrange` and :ref:`actuatorfrclimited`, respectively. -#. Added the flag :ref:`eulerdamp`, which disables implicit integration of joint damping in the - Euler integrator. See the :ref:`Numerical Integration` section for more details. -#. Added the flag :ref:`invdiscrete`, which enables discrete-time inverse dynamics for all - :ref:`integrators` other than ``RK4``. See the flag documentation for more details. -#. Changed the function :ref:`mj_stackAlloc` to allocate an arbitrary number of bytes, rather than in multiples of - ``sizeof(mjtNum)``, and add an additional argument for specifying the alignment of the returned pointer. The existing - functionality of allocating ``mjtNum`` arrays is still available through the new function :ref:`mj_stackAllocNum`. -#. Renamed the ``nstack`` field in :ref:`mjModel` and :ref:`mjData` to ``narena``. Changed ``narena``, ``pstack``, and - ``maxuse_stack`` to count number of bytes rather than number of :ref:`mjtNum` |-| s. -#. Added new functions ``mj_markStack`` and ``mj_freeStack`` that manages ``mjData`` stack frames in a fully - encapsulated way (i.e. without having to introduce a local variable at the call site). -#. Added :ref:`mjThreadPool` and :ref:`mjTask` which allow for multi-threaded operations within MuJoCo engine pipeline. +10. Added the flag :ref:`eulerdamp`, which disables implicit integration of joint damping in the + Euler integrator. See the :ref:`Numerical Integration` section for more details. +11. Added the flag :ref:`invdiscrete`, which enables discrete-time inverse dynamics for all + :ref:`integrators` other than ``RK4``. See the flag documentation for more details. Python bindings ^^^^^^^^^^^^^^^ -10. Fixed `#870 `__ where calling ``update_scene`` with an invalid +12. Fixed `#870 `__ where calling ``update_scene`` with an invalid camera name used the default camera. Bug fixes ^^^^^^^^^ -11. Fixed a bug that was causing the geom margins to be ignored during the midphase. +13. Fixed a bug that was causing the geom margins to be ignored during the midphase. Version 2.3.7 (July 20, 2023) diff --git a/doc/includes/references.h b/doc/includes/references.h index da9f1560..bf956b54 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2456,7 +2456,7 @@ void mju_addTo3(mjtNum res[3], const mjtNum vec[3]); void mju_subFrom3(mjtNum res[3], const mjtNum vec[3]); void mju_addToScl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl); void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtNum scl); -mjtNum mju_normalize3(mjtNum res[3]); +mjtNum mju_normalize3(mjtNum vec[3]); mjtNum mju_norm3(const mjtNum vec[3]); mjtNum mju_dot3(const mjtNum vec1[3], const mjtNum vec2[3]); mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]); @@ -2466,10 +2466,10 @@ void mju_cross(mjtNum res[3], const mjtNum a[3], const mjtNum b[3]); void mju_zero4(mjtNum res[4]); void mju_unit4(mjtNum res[4]); void mju_copy4(mjtNum res[4], const mjtNum data[4]); -mjtNum mju_normalize4(mjtNum res[4]); +mjtNum mju_normalize4(mjtNum vec[4]); void mju_zero(mjtNum* res, int n); void mju_fill(mjtNum* res, mjtNum val, int n); -void mju_copy(mjtNum* res, const mjtNum* data, int n); +void mju_copy(mjtNum* res, const mjtNum* vec, int n); mjtNum mju_sum(const mjtNum* vec, int n); mjtNum mju_L1(const mjtNum* vec, int n); void mju_scl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n); diff --git a/doc/programming/simulation.rst b/doc/programming/simulation.rst index e3a58e8b..b63ab55b 100644 --- a/doc/programming/simulation.rst +++ b/doc/programming/simulation.rst @@ -703,21 +703,19 @@ internally when an instability is detected in :ref:`mj_step`, :ref:`mj_step1` an take advantage of the custom stack, this needs to be done in-between MuJoCo calls that have the potential to reset the simulation. -Below is the general template for using the custom stack in user code. This assumes that ``mjData\* d`` is defined in -the scope. If not, saving and restoring the stack pointer should be done manually instead of using the -:ref:`mjMARKSTACK` and :ref:`mjFREESTACK` macros. +Below is the general template for using the custom stack in user code. .. code-block:: C - // save stack pointer in the "hidden" variable _mark - mjMARKSTACK; + // mark an mjData stack frame + mj_markStack(d); // allocate space mjtNum* myqpos = mj_stackAllocNum(d, m->nq); mjtNum* myqvel = mj_stackAllocNum(d, m->nv); - // restore stack from _mark - mjFREESTACK; + // restore the mjData stack frame + mj_freeStack(d); The function :ref:`mj_stackAllocNum` checks if there is enough space, and if so it advances the stack pointer, otherwise it triggers an error. It also keeps track of the maximum stack allocation; diff --git a/include/mujoco/mjmacro.h b/include/mujoco/mjmacro.h index e3972015..9ccf3042 100644 --- a/include/mujoco/mjmacro.h +++ b/include/mujoco/mjmacro.h @@ -30,10 +30,6 @@ #define mjMAX(a, b) (((a) > (b)) ? (a) : (b)) #define mjMIN(a, b) (((a) < (b)) ? (a) : (b)) -// mjData stack frame management -#define mjMARKSTACK mj_markStack(d); -#define mjFREESTACK mj_freeStack(d); - // return current value of mjOption enable/disable flags #define mjDISABLED(x) (m->opt.disableflags & (x)) #define mjENABLED(x) (m->opt.enableflags & (x)) diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index df70de12..8abb1e1f 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -895,7 +895,7 @@ MJAPI void mju_addToScl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl); MJAPI void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtNum scl); // Normalize vector, return length before normalization. -MJAPI mjtNum mju_normalize3(mjtNum res[3]); +MJAPI mjtNum mju_normalize3(mjtNum vec[3]); // Return vector length (without normalizing the vector). MJAPI mjtNum mju_norm3(const mjtNum vec[3]); @@ -925,7 +925,7 @@ MJAPI void mju_unit4(mjtNum res[4]); MJAPI void mju_copy4(mjtNum res[4], const mjtNum data[4]); // Normalize vector, return length before normalization. -MJAPI mjtNum mju_normalize4(mjtNum res[4]); +MJAPI mjtNum mju_normalize4(mjtNum vec[4]); // Set res = 0. MJAPI void mju_zero(mjtNum* res, int n); @@ -934,7 +934,7 @@ MJAPI void mju_zero(mjtNum* res, int n); MJAPI void mju_fill(mjtNum* res, mjtNum val, int n); // Set res = vec. -MJAPI void mju_copy(mjtNum* res, const mjtNum* data, int n); +MJAPI void mju_copy(mjtNum* res, const mjtNum* vec, int n); // Return sum(vec). MJAPI mjtNum mju_sum(const mjtNum* vec, int n); diff --git a/introspect/functions.py b/introspect/functions.py index cf2417d2..e5323e4c 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -5558,7 +5558,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ return_type=ValueType(name='mjtNum'), parameters=( FunctionParameterDecl( - name='res', + name='vec', type=ArrayType( inner_type=ValueType(name='mjtNum'), extents=(3,), @@ -5771,7 +5771,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ return_type=ValueType(name='mjtNum'), parameters=( FunctionParameterDecl( - name='res', + name='vec', type=ArrayType( inner_type=ValueType(name='mjtNum'), extents=(4,), @@ -5832,7 +5832,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), FunctionParameterDecl( - name='data', + name='vec', type=PointerType( inner_type=ValueType(name='mjtNum', is_const=True), ), diff --git a/plugin/sensor/touch_grid.cc b/plugin/sensor/touch_grid.cc index da318f81..4e58fee3 100644 --- a/plugin/sensor/touch_grid.cc +++ b/plugin/sensor/touch_grid.cc @@ -19,10 +19,8 @@ #include #include #include -#include -#include #include -#include +#include #include #include @@ -249,7 +247,7 @@ TouchGrid::TouchGrid(const mjModel* m, mjData* d, int instance, int nchannel, void TouchGrid::Reset(const mjModel* m, int instance) {} void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) { - mjMARKSTACK; + mj_markStack(d); // Get sensor id. int id; @@ -283,7 +281,7 @@ void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) { // No contacts, return. if (!ncon) { - mjFREESTACK; + mj_freeStack(d); return; } @@ -372,7 +370,7 @@ void TouchGrid::Compute(const mjModel* m, mjData* d, int instance) { } } - mjFREESTACK; + mj_freeStack(d); } // Thickness of taxel-visualization boxes relative to contact distance. @@ -380,7 +378,7 @@ static const mjtNum kRelativeThickness = 0.02; void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, mjvScene* scn, int instance) { - mjMARKSTACK; + mj_markStack(d); // Get sensor id. int id; @@ -403,7 +401,7 @@ void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, // If no normal force readings, quick return. if (!maxval) { - mjFREESTACK; + mj_freeStack(d); return; } @@ -430,7 +428,7 @@ void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, } if (scn->ngeom >= scn->maxgeom) { mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom); - mjFREESTACK; + mj_freeStack(d); return; } else { // size @@ -478,7 +476,7 @@ void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, } } - mjFREESTACK; + mj_freeStack(d); } diff --git a/sample/derivative.cc b/sample/derivative.cc index f15556da..e3fcd94f 100644 --- a/sample/derivative.cc +++ b/sample/derivative.cc @@ -61,7 +61,7 @@ void worker(const mjModel* m, const mjData* dmain, mjData* d, int id) { int nv = m->nv; // allocate stack space for result at center - mjMARKSTACK; + mj_markStack(d); mjtNum* center = mj_stackAllocNum(d, nv); mjtNum* warmstart = mj_stackAllocNum(d, nv); @@ -188,7 +188,7 @@ void worker(const mjModel* m, const mjData* dmain, mjData* d, int id) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -222,7 +222,7 @@ void checkderiv(const mjModel* m, mjData* d, mjtNum error[7]) { int nv = m->nv; // allocate space - mjMARKSTACK; + mj_markStack(d); mjtNum* mat = mj_stackAllocNum(d, nv*nv); // get pointers to derivative matrices @@ -275,7 +275,7 @@ void checkderiv(const mjModel* m, mjData* d, mjtNum error[7]) { mju_addTo(mat, F0, nv*nv); error[7] = relnorm(mat, F0, nv*nv); - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index ede3a6bf..3a887c7f 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -240,7 +240,7 @@ static void collideTree(const mjModel* m, mjData* d, int b1, int b2, mjtNum offset[12]; // 2 bb x 2 bb x 3 axes (world) mjtByte initialize = 1; - mjMARKSTACK; + mj_markStack(d); // TODO(b/273737633): Store bvh max depths to make this bound tighter. const int max_stack = m->body_bvhnum[b1] + m->body_bvhnum[b2]; mjCollisionTree* stack = mj_stackAllocTree(d, max_stack); @@ -345,7 +345,7 @@ static void collideTree(const mjModel* m, mjData* d, int b1, int b2, } } } - mjFREESTACK; + mj_freeStack(d); } @@ -408,7 +408,7 @@ void mj_collision(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // predefined only; ignore exclude if (m->opt.collision == mjCOL_PAIR) { @@ -503,7 +503,7 @@ void mj_collision(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -750,7 +750,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) { mju_eig3(eigval, frame, quat, cov); // allocate AABB; clear world entry (not used) - mjMARKSTACK; + mj_markStack(d); aabb = mj_stackAllocNum(d, 6*nbody); mju_zero(aabb, 6); @@ -859,7 +859,7 @@ endbroad: mjQUICKSORT(pair, npair, sizeof(int), paircompare, 0); } - mjFREESTACK; + mj_freeStack(d); return npair; } diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index a97af9f6..65006785 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -14,6 +14,7 @@ #include "engine/engine_collision_sdf.h" +#include #include #include @@ -449,7 +450,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g, const int* child = m->bvh_child + 2*bvhadr; mjtByte* visited = d->bvh_active + bvhadr; - mjMARKSTACK; + mj_markStack(d); // TODO(quaglino): Store bvh max depths to make this bound tighter. int max_stack = m->mesh_bvhnum[m->geom_dataid[g]]; struct CollideTreeArgs_ { @@ -479,7 +480,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g, faces[*npoints] = faceid[node]; if (++(*npoints)==MAXSDFFACE) { mju_warning("mjc_MeshSDF: too many bounding volumes, some contacts may be missed"); - mjFREESTACK; + mj_freeStack(d); return; } visited[node] = 1; @@ -504,7 +505,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g, } } - mjFREESTACK; + mj_freeStack(d); } //------------------------------ collision functions ----------------------------------------------- diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index b44ae2b3..1ffe5b99 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -21,8 +21,6 @@ #include #include #include -#include "engine/engine_array_safety.h" -#include "engine/engine_crossplatform.h" #include "engine/engine_core_smooth.h" #include "engine/engine_io.h" #include "engine/engine_support.h" @@ -508,7 +506,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // allocate space jac[0] = mj_stackAllocNum(d, 6*nv); @@ -702,7 +700,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -717,7 +715,7 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // allocate Jacobian jac = mj_stackAllocNum(d, nv); @@ -763,7 +761,7 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -779,7 +777,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // allocate Jacobian jac = mj_stackAllocNum(d, nv); @@ -909,7 +907,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -925,7 +923,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // allocate Jacobian jac = mj_stackAllocNum(d, 6*NV); @@ -1019,7 +1017,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -1474,7 +1472,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) { return 0; } - mjMARKSTACK; + mj_markStack(d); if (nnz) { chain = mj_stackAllocInt(d, nv); @@ -1555,7 +1553,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) { *nnz += nnze; } - mjFREESTACK; + mj_freeStack(d); return ne; } @@ -1670,7 +1668,7 @@ static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) { return 0; } - mjMARKSTACK; + mj_markStack(d); int *chain = mj_stackAllocInt(d, m->nv); for (int i=0; i < ncon; i++) { @@ -1703,7 +1701,7 @@ static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) { *nnz += nnzc; } - mjFREESTACK; + mj_freeStack(d); return nc; } @@ -1838,7 +1836,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // space for backsubM2(J')' and its traspose mjtNum* JM2 = mj_stackAllocNum(d, nefc*nv); @@ -1978,7 +1976,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index ebd342a4..a56665c6 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -178,7 +178,7 @@ void mj_kinematics(const mjModel* m, mjData* d) { // map inertias and motion dofs to global frame centered at subtree-CoM void mj_comPos(const mjModel* m, mjData* d) { mjtNum offset[3], axis[3]; - mjMARKSTACK; + mj_markStack(d); mjtNum* mass_subtree = mj_stackAllocNum(d, m->nbody); // clear subtree @@ -261,7 +261,7 @@ void mj_comPos(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -400,7 +400,7 @@ void mj_tendon(const mjModel* m, mjData* d) { } // allocate space - mjMARKSTACK; + mj_markStack(d); jac1 = mj_stackAllocNum(d, 3*nv); jac2 = mj_stackAllocNum(d, 3*nv); jacdif = mj_stackAllocNum(d, 3*nv); @@ -609,7 +609,7 @@ void mj_tendon(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -628,7 +628,7 @@ void mj_transmission(const mjModel* m, mjData* d) { } // allocate space, clear moments - mjMARKSTACK; + mj_markStack(d); jac = mj_stackAllocNum(d, 3*nv); jacA = mj_stackAllocNum(d, 3*nv); jacS = mj_stackAllocNum(d, 3*nv); @@ -954,7 +954,7 @@ void mj_transmission(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -1383,7 +1383,7 @@ void mj_comVel(const mjModel* m, mjData* d) { // subtree linear velocity and angular momentum void mj_subtreeVel(const mjModel* m, mjData* d) { mjtNum dx[3], dv[3], dp[3], dL[3]; - mjMARKSTACK; + mj_markStack(d); mjtNum* body_vel = mj_stackAllocNum(d, 6*m->nbody); // bodywise quantities @@ -1440,7 +1440,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) { mju_addTo3(d->subtree_angmom+3*parent, dL); } - mjFREESTACK; + mj_freeStack(d); } @@ -1449,7 +1449,7 @@ void mj_subtreeVel(const mjModel* m, mjData* d) { // RNE: compute M(qpos)*qacc + C(qpos,qvel); flg_acc=0 removes inertial term void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) { mjtNum tmp[6], tmp1[6]; - mjMARKSTACK; + mj_markStack(d); mjtNum* loc_cacc = mj_stackAllocNum(d, m->nbody*6); mjtNum* loc_cfrc_body = mj_stackAllocNum(d, m->nbody*6); @@ -1495,7 +1495,7 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) { result[i] = mju_dot(d->cdof+6*i, loc_cfrc_body+6*m->dof_bodyid[i], 6); } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 932fb8a5..76a56439 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -399,7 +399,7 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) { int nv = m->nv, nbody = m->nbody; mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6]; - mjMARKSTACK; + mj_markStack(d); mjtNum* Dcvel = mj_stackAllocNum(d, nbody*6*nv); mjtNum* Dcdofdot = mj_stackAllocNum(d, nv*6*nv); mjtNum* Dcacc = mj_stackAllocNum(d, nbody*6*nv); @@ -470,7 +470,7 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -614,7 +614,7 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) { mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6]; - mjMARKSTACK; + mj_markStack(d); mjtNum* Dcdofdot = mj_stackAllocNum(d, 6*m->nD); mjtNum* Dcvel = mj_stackAllocNum(d, 6*m->nB); mjtNum* Dcacc = mj_stackAllocNum(d, 6*m->nB); @@ -687,7 +687,7 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) { mju_subFrom(d->qDeriv + Dadr[j], row, Bnnz[i]); } - mjFREESTACK; + mj_freeStack(d); } @@ -734,7 +734,7 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum* int nv = m->nv; // allocate dense row - mjMARKSTACK; + mj_markStack(d); mjtNum* row = mj_stackAllocNum(d, nv); // process non-zero elements of B @@ -760,7 +760,7 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum* } } - mjFREESTACK; + mj_freeStack(d); } @@ -773,7 +773,7 @@ static void addJTBJSparse( int nv = m->nv; // allocate row - mjMARKSTACK; + mj_markStack(d); mjtNum* row = mj_stackAllocNum(d, nv); // compute qDeriv(k,p) += sum_{i,j} ( J(i,k)*B(i,j)*J(j,p) ) @@ -808,7 +808,7 @@ static void addJTBJSparse( } // free space - mjFREESTACK; + mj_freeStack(d); } @@ -1226,7 +1226,7 @@ static inline void mjd_magnus_force( // fluid forces based on ellipsoid approximation void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) { - mjMARKSTACK; + mj_markStack(d); int nv = m->nv; int nnz = nv; @@ -1331,14 +1331,14 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) { } } - mjFREESTACK; + mj_freeStack(d); } // fluid forces based on inertia-box approximation void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) { - mjMARKSTACK; + mj_markStack(d); int nv = m->nv; int rownnz[6], rowadr[6]; @@ -1488,7 +1488,7 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i) } } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_derivative_fd.c b/src/engine/engine_derivative_fd.c index e095d9de..9281955f 100644 --- a/src/engine/engine_derivative_fd.c +++ b/src/engine/engine_derivative_fd.c @@ -197,7 +197,7 @@ static void inverseSkip(const mjModel* m, mjData* d, mjtStage stage, int skipsen void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { int nv = m->nv; - mjMARKSTACK; + mj_markStack(d); mjtNum* qfrc_passive = mj_stackAllocNum(d, nv); mjtNum* fd = mj_stackAllocNum(d, nv); int* cnt = mj_stackAllocInt(d, nv); @@ -237,7 +237,7 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { // restore mj_fwdVelocity(m, d); - mjFREESTACK; + mj_freeStack(d); } @@ -248,7 +248,7 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { int nv = m->nv; - mjMARKSTACK; + mj_markStack(d); mjtNum* plus = mj_stackAllocNum(d, nv); mjtNum* minus = mj_stackAllocNum(d, nv); mjtNum* fd = mj_stackAllocNum(d, nv); @@ -303,7 +303,7 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { mj_fwdVelocity(m, d); mj_fwdActuation(m, d); - mjFREESTACK; + mj_freeStack(d); } @@ -330,7 +330,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, mjtNum* DsDq, mjtNum* DsDv, mjtNum* DsDa, mjtNum* DsDu) { int nq = m->nq, nv = m->nv, na = m->na, nu = m->nu, ns = m->nsensordata; int ndx = 2*nv+na; // row length of Dy Jacobians - mjMARKSTACK; + mj_markStack(d); // states mjtNum *state = mj_stackAllocNum(d, nq+nv+na); // current state @@ -558,7 +558,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, } } - mjFREESTACK; + mj_freeStack(d); } @@ -580,7 +580,7 @@ void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_cente mjtNum *DyDq, *DyDv, *DyDa, *DsDq, *DsDv, *DsDa; DyDq = DyDv = DyDa = DsDq = DsDv = DsDa = NULL; - mjMARKSTACK; + mj_markStack(d); // allocate transposed matrices mjtNum *AT = A ? mj_stackAllocNum(d, ndx*ndx) : NULL; // state-transition matrix (transposed) @@ -611,7 +611,7 @@ void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_cente if (C) mju_transpose(C, CT, ndx, ns); if (D) mju_transpose(D, DT, nu, ns); - mjFREESTACK; + mj_freeStack(d); } // finite differenced Jacobians of (force, sensors) = mj_inverse(state, acceleration) @@ -648,7 +648,7 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio int skipsensor = !DsDq && !DsDv && !DsDa; // local vectors - mjMARKSTACK; + mj_markStack(d); mjtNum *pos = mj_stackAllocNum(d, nq); // position mjtNum *force = mj_stackAllocNum(d, nv); // force mjtNum *force_plus = mj_stackAllocNum(d, nv); // nudged force @@ -731,5 +731,5 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio } } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 0ebe6a72..5a38c92e 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -199,7 +199,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // local, clamped copy of ctrl - mjMARKSTACK; + mj_markStack(d); mjtNum *ctrl = mj_stackAllocNum(d, nu); if (mjDISABLED(mjDSBL_CLAMPCTRL)) { mju_copy(ctrl, d->ctrl, nu); @@ -392,7 +392,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); TM_END(mjTIMER_ACTUATION); } @@ -401,7 +401,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // add up all non-constraint forces, compute qacc_smooth void mj_fwdAcceleration(const mjModel* m, mjData* d) { TM_START; - mjMARKSTACK; + mj_markStack(d); int nv = m->nv; // qforce = sum of all non-constraint forces @@ -413,7 +413,7 @@ void mj_fwdAcceleration(const mjModel* m, mjData* d) { // qacc_smooth = M \ qfr_smooth mj_solveM(m, d, d->qacc_smooth, d->qfrc_smooth, 1); - mjFREESTACK; + mj_freeStack(d); TM_END(mjTIMER_ACCELERATION); } @@ -425,7 +425,7 @@ static void warmstart(const mjModel* m, mjData* d) { // warmstart with best of (qacc_warmstart, qacc_smooth) if (!mjDISABLED(mjDSBL_WARMSTART)) { - mjMARKSTACK; + mj_markStack(d); mjtNum* jar = mj_stackAllocNum(d, nefc); // start with qacc = qacc_warmstart @@ -479,7 +479,7 @@ static void warmstart(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } // coldstart with qacc = qacc_smooth, efc_force = 0 @@ -589,7 +589,7 @@ static void mj_advance(const mjModel* m, mjData* d, // Euler integrator, semi-implicit in velocity, possibly skipping factorisation void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { int nv = m->nv, nM = m->nM; - mjMARKSTACK; + mj_markStack(d); mjtNum* qfrc = mj_stackAllocNum(d, nv); mjtNum* qacc = mj_stackAllocNum(d, nv); @@ -633,7 +633,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) { // advance state and time mj_advance(m, d, d->act_dot, qacc, NULL); - mjFREESTACK; + mj_freeStack(d); } @@ -672,7 +672,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { } // allocate space for intermediate solutions - mjMARKSTACK; + mj_markStack(d); dX = mj_stackAllocNum(d, 2*nv+na); for (int i=0; i < N; i++) { X[i] = mj_stackAllocNum(d, nq+nv+na); @@ -746,7 +746,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { // advance state and time mj_advance(m, d, dX+2*nv, dX+nv, dX); - mjFREESTACK; + mj_freeStack(d); } @@ -755,7 +755,7 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { int nv = m->nv; - mjMARKSTACK; + mj_markStack(d); mjtNum* qfrc = mj_stackAllocNum(d, nv); mjtNum* qacc = mj_stackAllocNum(d, nv); @@ -810,7 +810,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { // advance state and time mj_advance(m, d, d->act_dot, qacc, NULL); - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index 1e1fa50c..d81b8746 100644 --- a/src/engine/engine_inverse.c +++ b/src/engine/engine_inverse.c @@ -95,7 +95,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { int nv = m->nv, dof_damping; mjtNum *qacc = d->qacc; - mjMARKSTACK; + mj_markStack(d); mjtNum* qfrc = mj_stackAllocNum(d, nv); // use selected integrator @@ -119,7 +119,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { // if disabled or no dof damping, nothing to do if (!dof_damping) { - mjFREESTACK; + mj_freeStack(d); return; } @@ -169,7 +169,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) { // solve for qacc: qfrc = M * qacc mj_solveM(m, d, qacc, qfrc, 1); - mjFREESTACK; + mj_freeStack(d); } @@ -186,7 +186,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); mjtNum* jar = mj_stackAllocNum(d, nefc); // compute jar = Jac*qacc - aref @@ -196,7 +196,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) { // call update function mj_constraintUpdate(m, d, jar, NULL, 0); - mjFREESTACK; + mj_freeStack(d); TM_END(mjTIMER_CONSTRAINT); } @@ -206,7 +206,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) { void mj_inverseSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) { TM_START; - mjMARKSTACK; + mj_markStack(d); mjtNum* qacc; int nv = m->nv; @@ -259,7 +259,7 @@ void mj_inverseSkip(const mjModel* m, mjData* d, mju_copy(d->qacc, qacc, nv); } - mjFREESTACK; + mj_freeStack(d); TM_END(mjTIMER_INVERSE); } @@ -286,7 +286,7 @@ void mj_compareFwdInv(const mjModel* m, mjData* d) { } // allocate - mjMARKSTACK; + mj_markStack(d); qforce = mj_stackAllocNum(d, nv); dif = mj_stackAllocNum(d, nv); save_qfrc_constraint = mj_stackAllocNum(d, nv); @@ -314,5 +314,5 @@ void mj_compareFwdInv(const mjModel* m, mjData* d) { mju_copy(d->qfrc_constraint, save_qfrc_constraint, nv); mju_copy(d->efc_force, save_efc_force, nefc); - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index e78b4a37..21324fde 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -850,7 +850,7 @@ static void makeDSparse(const mjModel* m, mjData* d) { int* rowadr = d->D_rowadr; int* colind = d->D_colind; - mjMARKSTACK; + mj_markStack(d); int* remaining = mj_stackAllocInt(d, nv); // compute rownnz @@ -898,7 +898,7 @@ static void makeDSparse(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -943,7 +943,7 @@ static void makeBSparse(const mjModel* m, mjData* d) { } // allocate and clear incremental row counts - mjMARKSTACK; + mj_markStack(d); int* cnt = mj_stackAllocInt(d, nbody); mju_zeroInt(cnt, nbody); @@ -991,7 +991,7 @@ static void makeBSparse(const mjModel* m, mjData* d) { } } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 0c6771a8..c7bba3b9 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -18,11 +18,9 @@ #include #include -#include #include #include #include "engine/engine_core_constraint.h" -#include "engine/engine_crossplatform.h" #include "engine/engine_io.h" #include "engine/engine_support.h" #include "engine/engine_util_errmem.h" @@ -414,7 +412,7 @@ void mj_island(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); // allocate edge array int nedge_max = countMaxEdge(m, d); @@ -448,7 +446,7 @@ void mj_island(const mjModel* m, mjData* d) { // allocate island arrays on arena if (!arenaAllocIsland(m, d)) { - mjFREESTACK; + mj_freeStack(d); return; } @@ -527,5 +525,5 @@ void mj_island(const mjModel* m, mjData* d) { d->island_efcind[d->island_efcadr[island] + (d->island_efcnum[island]++)] = i; } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_island.h b/src/engine/engine_island.h index a724ac33..140bcdf6 100644 --- a/src/engine/engine_island.h +++ b/src/engine/engine_island.h @@ -26,7 +26,7 @@ extern "C" { // find disjoint subgraphs ("islands") given sparse symmetric adjacency matrix MJAPI int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, const int* colind, - int* scratch); + int* stack); //-------------------------- top-level API for island construction --------------------------------- diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 6e215a9c..7504f3f8 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -736,7 +736,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, } mjtNum *M; - mjMARKSTACK; + mj_markStack(d); // check format string if (!validateFloatFormat(float_format)) { @@ -755,7 +755,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, // check for nullptr if (!fp) { mju_warning("Could not open file '%s' for writing mjModel", filename); - mjFREESTACK; + mj_freeStack(d); return; } @@ -1145,7 +1145,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename, fclose(fp); } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index 462ba914..9904ee7a 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -15,6 +15,7 @@ #include "engine/engine_ray.h" +#include #include #include @@ -1158,7 +1159,7 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, int* geomid, mjtNum* dist, int nray, mjtNum cutoff) { - mjMARKSTACK; + mj_markStack(d); // allocate source mjtNum* geom_ba = mj_stackAllocNum(d, 4*m->ngeom); @@ -1173,5 +1174,5 @@ void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* dist[i] = mju_singleRay(m, d, pnt, vec+3*i, geom_eliminate, geom_ba, geomid+i); } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 45400bbf..6be75fc9 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -860,12 +860,12 @@ void mj_energyVel(const mjModel* m, mjData* d) { return; } - mjMARKSTACK; + mj_markStack(d); vec = mj_stackAllocNum(d, m->nv); // kinetic energy: 0.5 * qvel' * M * qvel mj_mulM(m, d, vec, d->qvel); d->energy[1] = 0.5*mju_dot(vec, d->qvel, m->nv); - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 7313d029..397ce15f 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -61,7 +61,7 @@ static void mj_setM0(mjModel* m, mjData* d) { static void set0(mjModel* m, mjData* d) { int id, id1, id2, dnum, nv = m->nv; mjtNum A[36] = {0}, pos[3], quat[4]; - mjMARKSTACK; + mj_markStack(d); mjtNum* jac = mj_stackAllocNum(d, 6*nv); mjtNum* tmp = mj_stackAllocNum(d, 6*nv); int* cammode = 0; @@ -264,7 +264,7 @@ static void set0(mjModel* m, mjData* d) { mju_copy3(m->light_dir0+3*i, d->light_xdir+3*i); } - mjFREESTACK; + mj_freeStack(d); } @@ -283,7 +283,7 @@ static void setStat(mjModel* m, mjData* d) { mjtNum xmin[3] = {1E+10, 1E+10, 1E+10}; mjtNum xmax[3] = {-1E+10, -1E+10, -1E+10}; mjtNum rbound; - mjMARKSTACK; + mj_markStack(d); mjtNum* body = mj_stackAllocNum(d, m->nbody); // compute bounding box of bodies, joint centers, geoms and sites @@ -383,7 +383,7 @@ static void setStat(mjModel* m, mjData* d) { m->stat.meaninertia /= m->nv; } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 8e4ab123..c0f1e313 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -309,7 +309,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { mjtNum *mu, x, denom, improvement; mjtNum v[6], v1[6], Athis[36], Ac[25], bc[5], res[6], oldforce[6]; mjContact* con; - mjMARKSTACK; + mj_markStack(d); mjtNum* ARinv = mj_stackAllocNum(d, nefc); int* oldstate = mj_stackAllocInt(d, nefc); @@ -497,7 +497,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { // map to joint space dualFinish(m, d); - mjFREESTACK; + mj_freeStack(d); } @@ -511,7 +511,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { mjtNum *mu, improvement; mjtNum v[5], Ac[25], bc[5], res[5], oldforce[5], delta[5], mid, y, K0, K1; mjContact* con; - mjMARKSTACK; + mj_markStack(d); mjtNum* ARinv = mj_stackAllocNum(d, nefc); int* oldstate = mj_stackAllocInt(d, nefc); @@ -703,7 +703,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { // map to joint space dualFinish(m, d); - mjFREESTACK; + mj_freeStack(d); } @@ -1277,7 +1277,7 @@ static mjtNum CGsearch(const mjModel* m, mjData* d, mjCGContext* ctx) { static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) { int nv = m->nv, nefc = d->nefc; mjtNum local[36]; - mjMARKSTACK; + mj_markStack(d); // storage for L'*J mjtNum* LTJ = mj_stackAllocNum(d, 6*nv); @@ -1347,7 +1347,7 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) { } } - mjFREESTACK; + mj_freeStack(d); } @@ -1355,7 +1355,7 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) { // compute and factorize Hessian: direct method static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) { int nv = m->nv, nefc = d->nefc; - mjMARKSTACK; + mj_markStack(d); // compute D corresponding to quad states mjtNum* D = mj_stackAllocNum(d, nefc); @@ -1427,7 +1427,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) { ctx->nnz = nv*nv; } - mjFREESTACK; + mj_freeStack(d); // add cones if present if (ctx->ncone) { @@ -1444,7 +1444,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) { static void HessianIncremental(const mjModel* m, mjData* d, mjCGContext* ctx, const int* oldstate) { int rank, nv = m->nv, nefc = d->nefc; - mjMARKSTACK; + mj_markStack(d); // local space mjtNum* vec = mj_stackAllocNum(d, nv); @@ -1490,7 +1490,7 @@ static void HessianIncremental(const mjModel* m, mjData* d, // recompute H directly if accuracy lost if (rank < nv) { - mjFREESTACK; + mj_freeStack(d); HessianDirect(m, d, ctx); // nothing else to do @@ -1504,7 +1504,7 @@ static void HessianIncremental(const mjModel* m, mjData* d, HessianCone(m, d, ctx); } - mjFREESTACK; + mj_freeStack(d); } @@ -1515,7 +1515,7 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New mjtNum alpha, beta; mjtNum *gradold = NULL, *Mgradold = NULL, *Mgraddif = NULL; mjCGContext ctx; - mjMARKSTACK; + mj_markStack(d); // allocate context CGallocate(m, d, &ctx, flg_Newton); @@ -1625,7 +1625,7 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New d->solver_nnz = 0; } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index cf7456c3..d502a614 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -279,7 +279,7 @@ void mj_jacBodyCom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr // compute subtree-com Jacobian void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) { int nv = m->nv; - mjMARKSTACK; + mj_markStack(d); mjtNum* jacp_b = mj_stackAllocNum(d, 3*nv); // clear output @@ -300,7 +300,7 @@ void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) { // normalize by subtree mass mju_scl(jacp, jacp, 1/m->body_subtreemass[body], 3*nv); - mjFREESTACK; + mj_freeStack(d); } @@ -325,7 +325,7 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA int nv = m->nv; // get full Jacobian of point - mjMARKSTACK; + mj_markStack(d); mjtNum* jacp = (jacPoint ? jacPoint : mj_stackAllocNum(d, 3*nv)); mjtNum* jacr = mj_stackAllocNum(d, 3*nv); mj_jac(m, d, jacp, jacr, point, body); @@ -339,7 +339,7 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA } } - mjFREESTACK; + mj_freeStack(d); } @@ -1014,7 +1014,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, // sparse if (rownnz && rowadr && colind) { int nv = m->nv; - mjMARKSTACK; + mj_markStack(d); // create sparse inertia matrix M int nnz = m->nD; // use sparse dof-dof matrix int* M_rownnz = mj_stackAllocInt(d, nv); // actual nnz count @@ -1024,7 +1024,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, mj_makeMSparse(m, d, M, M_rownnz, NULL, M_colind); mj_addMSparse(m, d, dst, rownnz, rowadr, colind, M, M_rownnz, NULL, M_colind); - mjFREESTACK; + mj_freeStack(d); } // dense @@ -1111,7 +1111,7 @@ void mj_addMSparse(const mjModel* m, mjData* d, mjtNum* dst, M_rowadr = d->D_rowadr; } - mjMARKSTACK; + mj_markStack(d); int* buf_ind = mj_stackAllocInt(d, nv); mjtNum* sparse_buf = mj_stackAllocNum(d, nv); @@ -1121,7 +1121,7 @@ void mj_addMSparse(const mjModel* m, mjData* d, mjtNum* dst, rownnz[i], M_rownnz[i], colind + rowadr[i], M_colind + M_rowadr[i], sparse_buf, buf_ind); } - mjFREESTACK; + mj_freeStack(d); } @@ -1158,7 +1158,7 @@ void mj_addMDense(const mjModel* m, mjData* d, mjtNum* dst) { // dst[D] = src[M], handle different sparsity representations void mj_copyM2DSparse(const mjModel* m, mjData* d, mjtNum* dst, const mjtNum* src) { int nv = m->nv; - mjMARKSTACK; + mj_markStack(d); // init remaining int* remaining = mj_stackAllocInt(d, nv); @@ -1185,7 +1185,7 @@ void mj_copyM2DSparse(const mjModel* m, mjData* d, mjtNum* dst, const mjtNum* sr } } - mjFREESTACK; + mj_freeStack(d); } @@ -1223,7 +1223,7 @@ void mj_applyFT(const mjModel* m, mjData* d, int nv = m->nv; // allocate local variables - mjMARKSTACK; + mj_markStack(d); mjtNum* jacp = mj_stackAllocNum(d, 3*nv); mjtNum* jacr = mj_stackAllocNum(d, 3*nv); mjtNum* qforce = mj_stackAllocNum(d, nv); @@ -1246,7 +1246,7 @@ void mj_applyFT(const mjModel* m, mjData* d, mju_addTo(qfrc_target, qforce, nv); } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index 31a77a58..bb7d16f5 100644 --- a/src/engine/engine_util_blas.c +++ b/src/engine/engine_util_blas.c @@ -16,7 +16,6 @@ #include -#include #include #ifdef mjUSEPLATFORMSIMD diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index d5dafbb5..f82055a1 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -147,7 +147,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, mjData* d) { int rank = n; - mjMARKSTACK; + mj_markStack(d); int* buf_ind = mj_stackAllocInt(d, n); mjtNum* sparse_buf = mj_stackAllocNum(d, n); @@ -198,7 +198,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, } } - mjFREESTACK; + mj_freeStack(d); return rank; } @@ -253,7 +253,7 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, int* rownnz, int* rowadr, int* colind, int x_nnz, int* x_ind, mjData* d) { - mjMARKSTACK; + mj_markStack(d); int* buf_ind = mj_stackAllocInt(d, n); mjtNum* sparse_buf = mj_stackAllocNum(d, n); @@ -294,7 +294,7 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, i = i - 1 + (new_x_nnz - i); } - mjFREESTACK; + mj_freeStack(d); return rank; } diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index cde53635..193a1cb0 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -13,12 +13,11 @@ // limitations under the License. #include "engine/engine_util_sparse.h" -#include "engine/engine_util_sparse_avx.h" +#include "engine/engine_util_sparse_avx.h" // IWYU pragma: keep #include #include -#include #include #include "engine/engine_io.h" #include "engine/engine_util_blas.h" @@ -474,13 +473,12 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, mjData* d) { - mjMARKSTACK; + mj_markStack(d); int* chain = mj_stackAllocInt(d, 2*nc); int nchain = 0; int* res_colind = NULL; for (int r=0; r < nc; r++) { - // supernode; copy everything to next row if (rowsuperT && r > 0 && rowsuperT[r-1] > 0) { res_rownnz[r] = res_rownnz[r - 1]; @@ -556,7 +554,7 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, res_rowadr[r] = res_rowadr[r-1] + res_rownnz[r-1]; } - mjFREESTACK; + mj_freeStack(d); } @@ -580,7 +578,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, const int* colindT, const int* rowsuperT, mjData* d) { // allocate space for accumulation buffer and matT - mjMARKSTACK; + mj_markStack(d); // a dense row buffer that stores the current row in the resulting matrix mjtNum* buffer = mj_stackAllocNum(d, nc); @@ -689,5 +687,5 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, } } - mjFREESTACK; + mj_freeStack(d); } diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 069dbdba..98885330 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -19,7 +19,6 @@ #include #include -#include #include #include #include "engine/engine_core_smooth.h" @@ -518,7 +517,7 @@ void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, // copy perturb pos,quat from selected body; set scale for perturbation void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPerturb* pert) { - mjMARKSTACK; + mj_markStack(d); int nv = m->nv; int sel = pert->select; @@ -529,7 +528,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur // invalid selected body: return if (sel <= 0 || sel >= m->nbody) { - mjFREESTACK; + mj_freeStack(d); return; } @@ -558,7 +557,7 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur mju_sub3(dif, pert->refselpos, headpos); pert->scale = mjv_frustumHeight(scn) * mju_dot3(dif, forward); - mjFREESTACK; + mj_freeStack(d); } diff --git a/test/benchmark/engine_core_smooth_benchmark_test.cc b/test/benchmark/engine_core_smooth_benchmark_test.cc index 4decaa13..80f3a474 100644 --- a/test/benchmark/engine_core_smooth_benchmark_test.cc +++ b/test/benchmark/engine_core_smooth_benchmark_test.cc @@ -105,13 +105,13 @@ static void BM_solveLD(benchmark::State& state, bool new_function) { static mjModel* m = LoadModelFromPath("composite/cloth.xml"); mjData* d = mj_makeData(m); - // warm-up rollout to get a typcal state + // warm-up rollout to get a typical state for (int i=0; i < kNumWarmupSteps; i++) { mj_step(m, d); } - // allocate gadient - mjMARKSTACK; + // allocate gradient + mj_markStack(d); mjtNum *grad = mj_stackAllocNum(d, m->nv); mjtNum *Ma = mj_stackAllocNum(d, m->nv); mjtNum *res = mj_stackAllocNum(d, m->nv); @@ -145,7 +145,7 @@ static void BM_solveLD(benchmark::State& state, bool new_function) { } // finalize - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index f3880aa1..ee95b134 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -15,6 +15,9 @@ // A benchmark for comparing different implementations of mj_solveLD. #include +#include +#include + #include #include #include @@ -47,7 +50,7 @@ void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline( const int* rownnz, const int* rowadr, const int* colind, const int* rowsuper, const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, mjData* d) { - mjMARKSTACK; + mj_markStack(d); int* chain = mj_stackAllocInt(d, 2 * nc); mjtNum* buffer = mj_stackAllocNum(d, nc); @@ -149,7 +152,7 @@ void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline( } } - mjFREESTACK; + mj_freeStack(d); } // transpose sparse matrix (uncompressed) @@ -354,7 +357,7 @@ static void BM_MatVecSparse(benchmark::State& state, int unroll) { } // allocate gradient - mjMARKSTACK; + mj_markStack(d); mjtNum *Ma = mj_stackAllocNum(d, m->nv); mjtNum *vec = mj_stackAllocNum(d, m->nv); mjtNum *res = mj_stackAllocNum(d, d->nefc); @@ -395,7 +398,7 @@ static void BM_MatVecSparse(benchmark::State& state, int unroll) { } // finalize - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } @@ -433,7 +436,7 @@ static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { } // allocate - mjMARKSTACK; + mj_markStack(d); mjtNum* H = mj_stackAllocNum(d, m->nv*m->nv); int* rownnz = mj_stackAllocInt(d, m->nv); int* rowadr = mj_stackAllocInt(d, m->nv); @@ -478,7 +481,7 @@ static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { } // finalize - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } @@ -510,7 +513,7 @@ static void BM_transposeSparse(benchmark::State& state, TransposeFuncPtr func) { mj_step(m, d); } - mjMARKSTACK; + mj_markStack(d); // need uncompressed layout mjtNum* res = mj_stackAllocNum(d, m->nv * d->nefc); @@ -524,7 +527,7 @@ static void BM_transposeSparse(benchmark::State& state, TransposeFuncPtr func) { d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); } - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } @@ -556,7 +559,7 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { } // allocate - mjMARKSTACK; + mj_markStack(d); mjtNum* H = mj_stackAllocNum(d, m->nv * m->nv); int* rownnz = mj_stackAllocInt(d, m->nv); int* rowadr = mj_stackAllocInt(d, m->nv); @@ -598,7 +601,7 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { } // finalize - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 10af8856..2b9a4702 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -14,7 +14,10 @@ // Tests for engine/engine_derivative.c. +#include +#include #include +#include #include #include @@ -287,7 +290,7 @@ TEST_F(DerivativeTest, StepSkip) { static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) { int nv = m->nv, nu = m->nu; mjtNum dt = m->opt.timestep; - mjMARKSTACK; + mj_markStack(d); // === state-transition matrix A if (A) { @@ -330,7 +333,7 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) { mju_scl(B+nu*nv, BcT, dt, nu*nv); } - mjFREESTACK; + mj_freeStack(d); } // compare FD derivatives to analytic derivatives of linear dynamical system diff --git a/test/engine/engine_util_container_test.cc b/test/engine/engine_util_container_test.cc index 42408739..26466e43 100644 --- a/test/engine/engine_util_container_test.cc +++ b/test/engine/engine_util_container_test.cc @@ -50,7 +50,7 @@ TEST(TestMjArrayList, TestMjArrayListSingleThreaded) { mjModel* m = LoadModelFromString("", error.data(), error.size()); ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error.data(); mjData* d = mj_makeData(m); - mjMARKSTACK; + mj_markStack(d); using DataType = int; constexpr int kInitialCapacity = 10; @@ -76,7 +76,7 @@ TEST(TestMjArrayList, TestMjArrayListSingleThreaded) { EXPECT_EQ(mju_arrayListAt(array_list, kNumElements), nullptr); EXPECT_EQ(mju_arrayListAt(array_list, 100), nullptr); - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); mj_deleteModel(m); } @@ -85,7 +85,7 @@ TEST(TestMjArrayList, ZeroInitialCapacity) { mjModel* m = LoadModelFromString("", nullptr, 0); ASSERT_THAT(m, NotNull()) << "Failed to load model"; mjData* d = mj_makeData(m); - mjMARKSTACK; + mj_markStack(d); mjArrayList* array_list = mju_arrayListCreate(d, sizeof(double), /*initial_capacity=*/0); EXPECT_EQ(mju_arrayListSize(array_list), 0); @@ -101,7 +101,7 @@ TEST(TestMjArrayList, ZeroInitialCapacity) { } EXPECT_EQ(mju_arrayListAt(array_list, 35), nullptr); - mjFREESTACK; + mj_freeStack(d); mj_deleteData(d); mj_deleteModel(m); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 4891f9f8..475a894d 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -3659,7 +3659,7 @@ public static unsafe extern void mju_addToScl3(double* res, double* vec, double public static unsafe extern void mju_addScl3(double* res, double* vec1, double* vec2, double scl); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern double mju_normalize3(double* res); +public static unsafe extern double mju_normalize3(double* vec); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern double mju_norm3(double* vec); @@ -3689,7 +3689,7 @@ public static unsafe extern void mju_unit4(double* res); public static unsafe extern void mju_copy4(double* res, double* data); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern double mju_normalize4(double* res); +public static unsafe extern double mju_normalize4(double* vec); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_zero(double* res, int n); @@ -3698,7 +3698,7 @@ public static unsafe extern void mju_zero(double* res, int n); public static unsafe extern void mju_fill(double* res, double val, int n); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern void mju_copy(double* res, double* data, int n); +public static unsafe extern void mju_copy(double* res, double* vec, int n); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern double mju_sum(double* vec, int n); From ba66bd4f8bbde60804bad9393347804b858db679 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 5 Sep 2023 12:35:41 -0700 Subject: [PATCH 36/38] Fixes to documentation: 1. Cleaned up changelog, fixed some typos, removed nested admonitions. 2. Added missing documentation for `mjfResourceModified` function type. PiperOrigin-RevId: 562859015 Change-Id: I34dc4f3131561cbe95be3127e6e318cc4b803a24 --- doc/APIreference/APItypes.rst | 14 ++++++++++++++ doc/changelog.rst | 22 ++++++++-------------- doc/programming/extension.rst | 4 ++-- 3 files changed, 24 insertions(+), 16 deletions(-) diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index d8ff40ad..de6d59ed 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -1174,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 diff --git a/doc/changelog.rst b/doc/changelog.rst index 068db417..d166090e 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -28,7 +28,8 @@ New features - Added new SDF plugin for defining implicit geometries. The plugin must define methods computing an SDF and its gradient at query points See the :ref:`documentation` for more details. -3. Added :ref:`mjThreadPool` and :ref:`mjTask` which allow for multi-threaded operations within MuJoCo engine pipeline. +3. Added :ref:`mjThreadPool` and :ref:`mjTask` which allow for multi-threaded operations within the MuJoCo engine + pipeline. General ^^^^^^^ @@ -36,23 +37,16 @@ General .. admonition:: Breaking API changes :class: attention - 4. Removed macros ``mjMARKSTACK`` and ``mjFREESTACK``. + 4. Removed the macros ``mjMARKSTACK`` and ``mjFREESTACK``. - .. admonition:: Migration note - :class: note - - These macros have been replaced by new functions :ref:`mj_markStack` and :ref:`mj_freeStack`. These functions - manages ``mjData`` stack frames in a fully encapsulated way (i.e. without having to introduce a local variable - at the call site). + **Migration:** These macros have been replaced by new functions :ref:`mj_markStack` and + :ref:`mj_freeStack`. These functions manage the :ref:`mjData stack` 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 add an additional argument for specifying the alignment of the returned pointer. + ``sizeof(mjtNum)``, and added an additional argument for specifying the alignment of the returned pointer. - .. admonition:: Migration note - :class: note - - The old functionality for allocating ``mjtNum`` arrays is still available through a new function - :ref:`mj_stackAllocNum`. + **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. diff --git a/doc/programming/extension.rst b/doc/programming/extension.rst index 7ab8f7e2..2ab381ae 100644 --- a/doc/programming/extension.rst +++ b/doc/programming/extension.rst @@ -331,7 +331,7 @@ Resource prefix Callbacks There are three callbacks that a resource provider is required to implement: :ref:`open`, :ref:`read`, and :ref:`close`. The other two callback - :ref:`getdir` and :ref:`modified` are optional. More details on these callbacks + :ref:`getdir` and :ref:`modified` are optional. More details on these callbacks are given below. Data Pointer @@ -352,7 +352,7 @@ Resource providers work via callbacks: - :ref:`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:`mjfModifiedResource`: This callback is optional and is used to check if an existing +- :ref:`mjfResourceModified`: This callback is optional and is used to check if an existing opened resource has been modifed from its orginal source. .. _exProviderUsage: From 48d0bfde449ebd63bb1990c48429a2a263a9b8c3 Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Wed, 6 Sep 2023 09:51:32 -0700 Subject: [PATCH 37/38] Replace `glfw.get_time` with `time.time` in the Python viewer timing logic. PiperOrigin-RevId: 563134207 Change-Id: Ic548701fc78336d8d87e94ea48833f94ee6395b8 --- python/mujoco/simulate.cc | 3 ++- python/mujoco/viewer.py | 8 ++++---- src/engine/engine_vis_state.c | 6 +++--- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/python/mujoco/simulate.cc b/python/mujoco/simulate.cc index 1bbe5802..ec9f2dd3 100644 --- a/python/mujoco/simulate.cc +++ b/python/mujoco/simulate.cc @@ -31,6 +31,7 @@ namespace mujoco::python { namespace { +using UIAdapter = mujoco::GlfwAdapter; namespace py = ::pybind11; template @@ -196,7 +197,7 @@ PYBIND11_MODULE(_simulate, pymodule) { py::object pert, bool fully_managed, py::object key_callback) { return std::make_unique( - std::make_unique>( + std::make_unique>( key_callback), scn, cam, opt, pert, fully_managed); })) diff --git a/python/mujoco/viewer.py b/python/mujoco/viewer.py index ccf7243b..9784691f 100644 --- a/python/mujoco/viewer.py +++ b/python/mujoco/viewer.py @@ -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 diff --git a/src/engine/engine_vis_state.c b/src/engine/engine_vis_state.c index 36409a17..f2d0eb25 100644 --- a/src/engine/engine_vis_state.c +++ b/src/engine/engine_vis_state.c @@ -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 From 040d3efce2549700a812d8bc0178c329222180ef Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Wed, 6 Sep 2023 12:02:11 -0700 Subject: [PATCH 38/38] Add options for controlling early termination criteria of CG/Newton linesearch. Useful for performance tuning models. PiperOrigin-RevId: 563175078 Change-Id: Id64a510d6080038ea8d5876dabbf0b2b311435bb --- doc/XMLreference.rst | 12 ++++++++++++ doc/XMLschema.rst | 12 +++++++----- doc/changelog.rst | 6 ++++-- doc/includes/references.h | 2 ++ include/mujoco/mjmodel.h | 2 ++ include/mujoco/mjxmacro.h | 2 ++ introspect/structs.py | 10 ++++++++++ simulate/simulate.cc | 4 +++- src/engine/engine_io.c | 5 +++++ src/engine/engine_solver.c | 11 ++++------- src/xml/xml_native_reader.cc | 10 ++++++---- src/xml/xml_native_writer.cc | 2 ++ unity/Runtime/Bindings/MjBindings.cs | 2 ++ 13 files changed, 61 insertions(+), 19 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 6ce1ce42..ad9f85ba 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -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` times :ref:`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"` diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 2210168f..2951b722 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -218,15 +218,17 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`timestep` | :ref:`apirate` | :ref:`impratio` | :ref:`tolerance` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`noslip_tolerance` | :ref:`mpr_tolerance` | :ref:`gravity` | :ref:`wind` | | +| | | | :ref:`ls_tolerance` | :ref:`noslip_tolerance` | :ref:`mpr_tolerance` | :ref:`gravity` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`magnetic` | :ref:`density` | :ref:`viscosity` | :ref:`o_margin` | | +| | | | :ref:`wind` | :ref:`magnetic` | :ref:`density` | :ref:`viscosity` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`o_solref` | :ref:`o_solimp` | :ref:`integrator` | :ref:`collision` | | +| | | | :ref:`o_margin` | :ref:`o_solref` | :ref:`o_solimp` | :ref:`integrator` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`cone` | :ref:`jacobian` | :ref:`solver` | :ref:`iterations` | | +| | | | :ref:`collision` | :ref:`cone` | :ref:`jacobian` | :ref:`solver` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`noslip_iterations` | :ref:`mpr_iterations` | :ref:`sdf_iterations` | :ref:`sdf_initpoints` | | +| | | | :ref:`iterations` | :ref:`ls_iterations` | :ref:`noslip_iterations` | :ref:`mpr_iterations` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`sdf_iterations` | :ref:`sdf_initpoints` | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| option |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index d166090e..e6d03de4 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -62,17 +62,19 @@ General Euler integrator. See the :ref:`Numerical Integration` section for more details. 11. Added the flag :ref:`invdiscrete`, which enables discrete-time inverse dynamics for all :ref:`integrators` other than ``RK4``. See the flag documentation for more details. +12. Added :ref:`ls_iterations` and :ref:`ls_iterations` options for adjusting + linesearch stopping criteria in CG and Newton solvers. This can be useful for performance tuning. Python bindings ^^^^^^^^^^^^^^^ -12. Fixed `#870 `__ where calling ``update_scene`` with an invalid +13. Fixed `#870 `__ where calling ``update_scene`` with an invalid camera name used the default camera. Bug fixes ^^^^^^^^^ -13. 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) diff --git a/doc/includes/references.h b/doc/includes/references.h index bf956b54..eda352fe 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -674,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 @@ -696,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 diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 38f28f44..13b7fc5a 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -397,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 @@ -419,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 diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 2ef1c7ee..842d7914 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -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 ) \ diff --git a/introspect/structs.py b/introspect/structs.py index d399c6bc..2e5f08a0 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -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'), diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 404004dd..95fd574f 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -602,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"}, diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 21324fde..4d1b3d22 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -113,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; @@ -120,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; @@ -147,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; diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index c0f1e313..7eeb41f2 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -1093,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; @@ -1109,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 @@ -1180,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; @@ -1197,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; @@ -1216,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); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 3026dfb5..dc5129d6 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -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", @@ -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); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 31c3a1e4..bcd0ffff 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -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); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 475a894d..2500e8a6 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1769,6 +1769,7 @@ public unsafe struct mjOption_ { public double apirate; public double impratio; public double tolerance; + public double ls_tolerance; public double noslip_tolerance; public double mpr_tolerance; public fixed double gravity[3]; @@ -1785,6 +1786,7 @@ public unsafe struct mjOption_ { public int jacobian; public int solver; public int iterations; + public int ls_iterations; public int noslip_iterations; public int mpr_iterations; public int disableflags;