// Copyright 2021 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 "engine/engine_io.h" #include // IWYU pragma: keep #include #include #include #include #include #include #include #include #include // IWYU pragma: keep #include #include "engine/engine_core_smooth.h" #include "engine/engine_forward.h" #include "engine/engine_init.h" #include "engine/engine_macro.h" #include "engine/engine_memory.h" #include "engine/engine_plugin.h" #include "engine/engine_sleep.h" #include "engine/engine_thread.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #ifdef ADDRESS_SANITIZER #include #include #endif #ifdef MEMORY_SANITIZER #include #endif #ifdef _MSC_VER #pragma warning (disable: 4305) // disable MSVC warning: truncation from 'double' to 'float' #endif static const int MAX_ARRAY_SIZE = INT_MAX; //----------------------------------- static utility functions ------------------------------------- // id used to identify binary mjModel file/buffer static const int ID = 54321; // number of ints in the mjb header #define NHEADER 5 // macro for referring to a mjModel member in generic expressions #define MJMODEL_MEMBER(name) (((mjModel*) NULL)->name) // count mjtSize members in mjModel static int getnsize(void) { int cnt = 0; #define X(name) cnt += _Generic(MJMODEL_MEMBER(name), mjtSize: 1, default: 0); MJMODEL_SIZES #undef X return cnt; } // count pointers in mjModel static int getnptr(void) { int cnt = 0; #define X(type, name, nr, nc) cnt++; MJMODEL_POINTERS #undef X return cnt; } // write to memory buffer static void bufwrite(const void* src, int num, mjtSize szbuf, void* buf, mjtSize* ptrbuf) { // check pointers if (!src || !buf || !ptrbuf) { mjERROR("NULL pointer passed to bufwrite"); } // check size if (*ptrbuf+num > szbuf) { mjERROR("attempting to write outside model buffer"); } // write, advance pointer memcpy((char*)buf + *ptrbuf, src, num); *ptrbuf += num; } // read from memory buffer static void bufread(void* dest, int num, mjtSize szbuf, const void* buf, mjtSize* ptrbuf) { // check pointers if (!dest || !buf || !ptrbuf) { mjERROR("NULL pointer passed to bufread"); } // check size if (*ptrbuf+num > szbuf) { mjERROR("attempting to read outside model buffer"); } // read, advance pointer memcpy(dest, (char*)buf + *ptrbuf, num); *ptrbuf += num; } // number of bytes to be skipped to achieve 64-byte alignment static inline unsigned int SKIP(intptr_t offset) { const unsigned int align = 64; // compute skipped bytes return (align - (offset % align)) % align; } //----------------------------------- mjModel construction ----------------------------------------- // set pointers in mjModel buffer static void mj_setPtrModel(mjModel* m) { char* ptr = (char*)m->buffer; // prepare symbols needed by xmacro MJMODEL_POINTERS_PREAMBLE(m); // assign pointers with padding #define X(type, name, nr, nc) \ m->name = (type*)(ptr + SKIP((intptr_t)ptr)); \ ASAN_POISON_MEMORY_REGION(ptr, PTRDIFF(m->name, ptr)); \ ptr += SKIP((intptr_t)ptr) + sizeof(type)*(m->nr)*(nc); MJMODEL_POINTERS #undef X // check size ptrdiff_t sz = ptr - (char*)m->buffer; if (m->nbuffer != sz) { mjERROR( "mjModel buffer size mismatch, " "expected size: %" PRIu64 ", actual size: %td", m->nbuffer, sz); } } // increases buffer size without causing integer overflow, returns 0 if // operation would cause overflow // performs the following operations: // *nbuffer += SKIP(*offset) + type_size*nr*nc; // *offset += SKIP(*offset) + type_size*nr*nc; static mjtSize safeAddToBufferSize(intptr_t* offset, mjtSize* nbuffer, size_t type_size, mjtSize nr, mjtSize nc) { if (type_size < 0 || nr < 0 || nc < 0) { return 0; } #if (__has_builtin(__builtin_add_overflow) && __has_builtin(__builtin_mul_overflow)) \ || (defined(__GNUC__) && __GNUC__ >= 5) // supported by GCC and Clang size_t to_add = 0; if (__builtin_mul_overflow(nc, nr, &to_add)) return 0; if (__builtin_mul_overflow(to_add, type_size, &to_add)) return 0; if (__builtin_add_overflow(to_add, SKIP(*offset), &to_add)) return 0; if (__builtin_add_overflow(*nbuffer, to_add, nbuffer)) return 0; if (__builtin_add_overflow(*offset, to_add, offset)) return 0; #else // safe overflow checks for MSVC and other compilers without __builtin_*_overflow { size_t product; size_t to_add; size_t skip = SKIP(*offset); // nc * nr if (nr > 0 && (size_t)nc > SIZE_MAX / (size_t)nr) return 0; product = (size_t)nc * (size_t)nr; // product * type_size if (type_size > 0 && product > SIZE_MAX / type_size) return 0; product *= type_size; // product + SKIP(*offset) if (product > SIZE_MAX - skip) return 0; to_add = product + skip; // *nbuffer + to_add if ((size_t)*nbuffer > SIZE_MAX - to_add) return 0; *nbuffer += to_add; // *offset + to_add if (*offset > 0 && to_add > (size_t)(INTPTR_MAX - *offset)) return 0; *offset += to_add; } #endif return 1; } // free model memory without destroying the struct static void freeModelBuffers(mjModel* m) { mju_free(m->buffer); } // allocate and initialize mjModel structure void mj_makeModel(mjModel** dest, mjtSize nq, mjtSize nv, mjtSize nu, mjtSize na, mjtSize nbody, mjtSize nbvh, mjtSize nbvhstatic, mjtSize nbvhdynamic, mjtSize noct, mjtSize njnt, mjtSize ntree, mjtSize nM, mjtSize nB, mjtSize nC, mjtSize nD, mjtSize ngeom, mjtSize nsite, mjtSize ncam, mjtSize nlight, mjtSize nflex, mjtSize nflexnode, mjtSize nflexvert, mjtSize nflexedge, mjtSize nflexelem, mjtSize nflexelemdata, mjtSize nflexstiffness, mjtSize nflexbending, mjtSize nflexelemedge, mjtSize nflexshelldata, mjtSize nflexevpair, mjtSize nflextexcoord, mjtSize nJfe, mjtSize nJfv, mjtSize nmesh, mjtSize nmeshvert, mjtSize nmeshnormal, mjtSize nmeshtexcoord, mjtSize nmeshface, mjtSize nmeshgraph, mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, mjtSize nskin, mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone, mjtSize nskinbonevert, mjtSize nhfield, mjtSize nhfielddata, mjtSize ntex, mjtSize ntexdata, mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nJten, mjtSize nwrap, mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext, mjtSize ntextdata, mjtSize ntuple, mjtSize ntupledata, mjtSize nkey, mjtSize nmocap, mjtSize nplugin, mjtSize npluginattr, mjtSize nuser_body, mjtSize nuser_jnt, mjtSize nuser_geom, mjtSize nuser_site, mjtSize nuser_cam, mjtSize nuser_tendon, mjtSize nuser_actuator, mjtSize nuser_sensor, mjtSize nnames, mjtSize npaths) { intptr_t offset = 0; int allocate = *dest ? 0 : 1; mjModel* m = NULL; // CHECK SIZE PARAMETERS { // dummy variables for MJMODEL_SIZES set after mjModel construction int nnames_map = 0, nJmom = 0, ngravcomp = 0, nemax = 0, njmax = 0, nconmax=0; int nuserdata=0, nsensordata=0, npluginstate=0, nhistory=0, narena=0, nbuffer=0; // sizes must be non-negative and fit in int, except for the byte arrays texdata and textdata #define X(name) \ if (name < 0) { \ mju_warning("Invalid model: %s is negative (%lld).", #name, (long long)name); \ return; \ } \ if (name >= MAX_ARRAY_SIZE && \ strcmp(#name, "ntexdata") != 0 && strcmp(#name, "ntextdata") != 0) { \ mju_warning("Invalid model: %s is too large. Expected < %d. Got %lld.", \ #name, MAX_ARRAY_SIZE, (long long)name); \ return; \ } MJMODEL_SIZES #undef X // suppress unused variable warnings (void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nemax; (void)njmax; (void)nconmax; (void)nuserdata; (void)nsensordata; (void)npluginstate; (void)nhistory; (void)narena; (void)nbuffer; } // nbody should always be positive if (nbody == 0) { mju_warning("Invalid model: nbody == 0"); return; } // allocate mjModel if (!allocate) { m = *dest; freeModelBuffers(m); } else { m = (mjModel*)mju_malloc(sizeof(mjModel)); } if (!m) { mjERROR("could not allocate mjModel"); } memset(m, 0, sizeof(mjModel)); // set size parameters m->nq = nq; m->nv = nv; m->nu = nu; m->na = na; m->nbody = nbody; m->nbvh = nbvh; m->nbvhstatic = nbvhstatic; m->nbvhdynamic = nbvhdynamic; m->noct = noct; m->njnt = njnt; m->ntree = ntree; m->nM = nM; m->nB = nB; m->nC = nC; m->nD = nD; m->ngeom = ngeom; m->nsite = nsite; m->ncam = ncam; m->nlight = nlight; m->nflex = nflex; m->nflexnode = nflexnode; m->nflexvert = nflexvert; m->nflexedge = nflexedge; m->nflexelem = nflexelem; m->nflexelemdata = nflexelemdata; m->nflexstiffness = nflexstiffness; m->nflexbending = nflexbending; m->nflexelemedge = nflexelemedge; m->nflexshelldata = nflexshelldata; m->nflexevpair = nflexevpair; m->nflextexcoord = nflextexcoord; m->nJfe = nJfe; m->nJfv = nJfv; m->nmesh = nmesh; m->nmeshvert = nmeshvert; m->nmeshnormal = nmeshnormal; m->nmeshtexcoord = nmeshtexcoord; m->nmeshface = nmeshface; m->nmeshgraph = nmeshgraph; m->nmeshpoly = nmeshpoly; m->nmeshpolyvert = nmeshpolyvert; m->nmeshpolymap = nmeshpolymap; m->nskin = nskin; m->nskinvert = nskinvert; m->nskintexvert = nskintexvert; m->nskinface = nskinface; m->nskinbone = nskinbone; m->nskinbonevert = nskinbonevert; m->nhfield = nhfield; m->nhfielddata = nhfielddata; m->ntex = ntex; m->ntexdata = ntexdata; m->nmat = nmat; m->npair = npair; m->nexclude = nexclude; m->neq = neq; m->ntendon = ntendon; m->nJten = nJten; m->nwrap = nwrap; m->nsensor = nsensor; m->nnumeric = nnumeric; m->nnumericdata = nnumericdata; m->ntext = ntext; m->ntextdata = ntextdata; m->ntuple = ntuple; m->ntupledata = ntupledata; m->nkey = nkey; m->nmocap = nmocap; m->nplugin = nplugin; m->npluginattr = npluginattr; m->nuser_body = nuser_body; m->nuser_jnt = nuser_jnt; m->nuser_geom = nuser_geom; m->nuser_site = nuser_site; m->nuser_cam = nuser_cam; m->nuser_tendon = nuser_tendon; m->nuser_actuator = nuser_actuator; m->nuser_sensor = nuser_sensor; m->nnames = nnames; long nnames_map = (long)nbody + njnt + ngeom + nsite + ncam + nlight + nflex + nmesh + nskin + nhfield + ntex + nmat + npair + nexclude + neq + ntendon + nu + nsensor + nnumeric + ntext + ntuple + nkey + nplugin; if (nnames_map >= INT_MAX / mjLOAD_MULTIPLE) { if (allocate) mju_free(m); mju_warning("Invalid model: size of nnames_map is larger than INT_MAX"); return; } m->nnames_map = mjLOAD_MULTIPLE * nnames_map; m->npaths = npaths; // compute buffer size m->nbuffer = 0; #define X(type, name, nr, nc) \ if (!safeAddToBufferSize(&offset, &m->nbuffer, sizeof(type), m->nr, nc)) { \ if (allocate) mju_free(m); \ mju_warning("Invalid model: " #name " too large."); \ return; \ } MJMODEL_POINTERS #undef X // allocate buffer m->buffer = mju_malloc(m->nbuffer); if (!m->buffer) { if (allocate) mju_free(m); mjERROR("could not allocate mjModel buffer"); } // clear, set pointers in buffer memset(m->buffer, 0, m->nbuffer); #ifdef MEMORY_SANITIZER // tell msan to treat the entire buffer as uninitialized __msan_allocated_memory(m->buffer, m->nbuffer); #endif mj_setPtrModel(m); // set default options mj_defaultOption(&m->opt); mj_defaultVisual(&m->vis); mj_defaultStatistic(&m->stat); // copy pointer if allocated here if (allocate) { *dest = m; } } // copy mjModel, if dest==NULL create new model mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { // allocate new model if needed if (!dest) { mj_makeModel( &dest, src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh, src->nbvhstatic, src->nbvhdynamic, src->noct, src->njnt, src->ntree, src->nM, src->nB, src->nC, src->nD, src->ngeom, src->nsite, src->ncam, src->nlight, src->nflex, src->nflexnode, src->nflexvert, src->nflexedge, src->nflexelem, src->nflexelemdata, src->nflexstiffness, src->nflexbending, src->nflexelemedge, src->nflexshelldata, src->nflexevpair, src->nflextexcoord, src->nJfe, src->nJfv, src->nmesh, src->nmeshvert, src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph, src->nmeshpoly, src->nmeshpolyvert, src->nmeshpolymap, src->nskin, src->nskinvert, src->nskintexvert, src->nskinface, src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata, src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude, src->neq, src->ntendon, src->nJten, src->nwrap, src->nsensor, src->nnumeric, src->nnumericdata, src->ntext, src->ntextdata, src->ntuple, src->ntupledata, src->nkey, src->nmocap, src->nplugin, src->npluginattr, src->nuser_body, src->nuser_jnt, src->nuser_geom, src->nuser_site, src->nuser_cam, src->nuser_tendon, src->nuser_actuator, src->nuser_sensor, src->nnames, src->npaths); } if (!dest) { mjERROR("failed to make mjModel. Invalid sizes."); } // check sizes if (dest->nbuffer != src->nbuffer) { mj_deleteModel(dest); mjERROR("dest and src models have different buffer size"); } // save buffer ptr, copy struct, restore buffer and other pointers void* save_bufptr = dest->buffer; *dest = *src; dest->buffer = save_bufptr; mj_setPtrModel(dest); // copy buffer contents { MJMODEL_POINTERS_PREAMBLE(src) #define X(type, name, nr, nc) \ memcpy((char*)dest->name, (const char*)src->name, sizeof(type)*(src->nr)*nc); MJMODEL_POINTERS #undef X } return dest; } // copy mjModel, skip large arrays not required for abstract visualization void mjv_copyModel(mjModel* dest, const mjModel* src) { // check sizes if (dest->nbuffer != src->nbuffer) { mjERROR("dest and src models have different buffer size"); } // save buffer ptr, copy struct, restore buffer and other pointers void* save_bufptr = dest->buffer; *dest = *src; dest->buffer = save_bufptr; mj_setPtrModel(dest); // redefine XNV to do nothing #undef XNV #define XNV(type, name, nr, nc) // copy buffer contents, skipping arrays marked XNV { MJMODEL_POINTERS_PREAMBLE(src) #define X(type, name, nr, nc) \ memcpy((char*)dest->name, (const char*)src->name, sizeof(type)*(src->nr)*nc); MJMODEL_POINTERS #undef X } // redefine XNV to be the same as X #undef XNV #define XNV X } // save model to binary file, or memory buffer of szbuf>0 void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buffer_sz) { FILE* fp = 0; mjtSize ptrbuf = 0; // standard header int header[NHEADER] = {ID, sizeof(mjtNum), getnsize(), mj_version(), getnptr()}; // open file for writing if no buffer if (!buffer) { fp = fopen(filename, "wb"); if (!fp) { mju_warning("Could not open file '%s'", filename); return; } } // write standard header, info, options, buffer (omit pointers) if (fp) { fwrite(header, sizeof(int), NHEADER, fp); #define X(name) fwrite(&m->name, sizeof(m->name), 1, fp); MJMODEL_SIZES #undef X fwrite((void*)&m->opt, sizeof(mjOption), 1, fp); fwrite((void*)&m->vis, sizeof(mjVisual), 1, fp); fwrite((void*)&m->stat, sizeof(mjStatistic), 1, fp); { MJMODEL_POINTERS_PREAMBLE(m) #define X(type, name, nr, nc) \ fwrite((void*)m->name, sizeof(type), (m->nr)*(nc), fp); MJMODEL_POINTERS #undef X } } else { bufwrite(header, sizeof(header), buffer_sz, buffer, &ptrbuf); #define X(name) bufwrite(&m->name, sizeof(m->name), buffer_sz, buffer, &ptrbuf); MJMODEL_SIZES #undef X bufwrite((void*)&m->opt, sizeof(mjOption), buffer_sz, buffer, &ptrbuf); bufwrite((void*)&m->vis, sizeof(mjVisual), buffer_sz, buffer, &ptrbuf); bufwrite((void*)&m->stat, sizeof(mjStatistic), buffer_sz, buffer, &ptrbuf); { MJMODEL_POINTERS_PREAMBLE(m) #define X(type, name, nr, nc) \ bufwrite((void*)m->name, sizeof(type)*(m->nr)*(nc), buffer_sz, buffer, &ptrbuf); MJMODEL_POINTERS #undef X } } if (fp) { fclose(fp); } } // load binary MJB model mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) { mjtSize ptrbuf = 0; mjModel *m = 0; if (buffer_sz < NHEADER*sizeof(int)) { mju_warning("Model file has an incomplete header"); return NULL; } int header[NHEADER] = {0}; bufread(header, NHEADER*sizeof(int), buffer_sz, buffer, &ptrbuf); // check header int expected_header[NHEADER] = {ID, sizeof(mjtNum), getnsize(), mj_version(), getnptr()}; for (int i=0; i < NHEADER; i++) { if (header[i] != expected_header[i]) { switch (i) { case 0: mju_warning("Model missing header ID"); return NULL; case 1: mju_warning("Model and executable have different floating point precision"); return NULL; case 2: mju_warning("Model and executable have different number of sizes in mjModel"); return NULL; case 3: mju_warning("Model and executable use different MuJoCo version"); return NULL; default: mju_warning("Model and executable have different number of pointers in mjModel"); return NULL; } } } int nsize = getnsize(); // number of mjtSize fields in mjModel if (ptrbuf + sizeof(mjtSize)*nsize > buffer_sz) { mju_warning("Truncated model file - ran out of data while reading sizes"); return NULL; } // read mjModel construction fields mjtSize sizes[256]; bufread(sizes, sizeof(mjtSize)*nsize, buffer_sz, buffer, &ptrbuf); // allocate new mjModel mj_makeModel(&m, sizes[0], sizes[1], sizes[2], sizes[3], sizes[4], sizes[5], sizes[6], sizes[7], sizes[8], sizes[9], sizes[10], sizes[11], sizes[12], sizes[13], sizes[14], sizes[15], sizes[16], sizes[17], sizes[18], sizes[19], sizes[20], sizes[21], sizes[22], sizes[23], sizes[24], sizes[25], sizes[26], sizes[27], sizes[28], sizes[29], sizes[30], sizes[31], sizes[32], sizes[33], sizes[34], sizes[35], sizes[36], sizes[37], sizes[38], sizes[39], sizes[40], sizes[41], sizes[42], sizes[43], sizes[44], sizes[45], sizes[46], sizes[47], sizes[48], sizes[49], sizes[50], sizes[51], sizes[52], sizes[53], sizes[54], sizes[55], sizes[56], sizes[57], sizes[58], sizes[59], sizes[60], sizes[61], sizes[62], sizes[63], sizes[64], sizes[65], sizes[66], sizes[67], sizes[68], sizes[69], sizes[70], sizes[71], sizes[72], sizes[73], sizes[74], sizes[75], sizes[76], sizes[77], sizes[78], sizes[79]); // mj_makeModel may fail if the input buffer has invalid sizes if (!m) { mju_warning("Invalid sizes, unable to load model"); return NULL; } // check buffer size (last mjtSize field is nbuffer) if (m->nbuffer != sizes[nsize-1]) { mju_warning("Corrupted model, wrong nbuffer field"); mj_deleteModel(m); return NULL; } // set integer fields { int int_idx = 0; #define X(name) \ m->name = sizes[int_idx++]; MJMODEL_SIZES #undef X } // read options and buffer if (ptrbuf + sizeof(mjOption) + sizeof(mjVisual) + sizeof(mjStatistic) > buffer_sz) { mju_warning("Truncated model file - ran out of data while reading structs"); return NULL; } bufread((void*)&m->opt, sizeof(mjOption), buffer_sz, buffer, &ptrbuf); bufread((void*)&m->vis, sizeof(mjVisual), buffer_sz, buffer, &ptrbuf); bufread((void*)&m->stat, sizeof(mjStatistic), buffer_sz, buffer, &ptrbuf); { MJMODEL_POINTERS_PREAMBLE(m) #define X(type, name, nr, nc) \ if (ptrbuf + sizeof(type) * (m->nr) * (nc) > buffer_sz) { \ mju_warning( \ "Truncated model file - ran out of data while reading " #name); \ mj_deleteModel(m); \ return NULL; \ } \ bufread(m->name, sizeof(type)*(m->nr)*(nc), buffer_sz, buffer, &ptrbuf); MJMODEL_POINTERS #undef X } // make sure buffer is the correct size if (ptrbuf != buffer_sz) { mju_warning("Model file is too large"); mj_deleteModel(m); return NULL; } const char* validationError = mj_validateReferences(m); if (validationError) { mju_warning("%s", validationError); mj_deleteModel(m); return NULL; } return m; } // de-allocate mjModel void mj_deleteModel(mjModel* m) { if (m) { freeModelBuffers(m); mju_free(m); } } // size of buffer needed to hold model mjtSize mj_sizeModel(const mjModel* m) { mjtSize size = ( sizeof(int)*NHEADER + sizeof(mjtSize)*getnsize() + sizeof(mjOption) + sizeof(mjVisual) + sizeof(mjStatistic)); MJMODEL_POINTERS_PREAMBLE(m) #define X(type, name, nr, nc) \ size += sizeof(type)*(m->nr)*(nc); MJMODEL_POINTERS #undef X return size; } //-------------------------- sparse system matrix construction ------------------------------------- // construct sparse representation of dof-dof matrix void mj_makeDofDofSparse(int nv, int nC, int nD, int nM, const int* dof_parentid, const int* dof_simplenum, int* rownnz, int* rowadr, int* diag, int* colind, int reduced, int upper, int* remaining) { // no dofs, nothing to do if (!nv) { return; } // compute rownnz mju_zeroInt(rownnz, nv); for (int i = nv - 1; i >= 0; i--) { // init at diagonal int j = i; rownnz[i]++; // process below diagonal unless reduced and dof is simple if (!(reduced && dof_simplenum[i])) { while ((j = dof_parentid[j]) >= 0) { // both reduced and non-reduced have lower triangle rownnz[i]++; // add upper triangle if requested if (upper) rownnz[j]++; } } } // accumulate rowadr rowadr[0] = 0; for (int i = 1; i < nv; i++) { rowadr[i] = rowadr[i - 1] + rownnz[i - 1]; } // populate colind mju_copyInt(remaining, rownnz, nv); for (int i = nv - 1; i >= 0; i--) { // init at diagonal remaining[i]--; colind[rowadr[i] + remaining[i]] = i; // process below diagonal unless reduced and dof is simple if (!(reduced && dof_simplenum[i])) { int j = i; while ((j = dof_parentid[j]) >= 0) { remaining[i]--; colind[rowadr[i] + remaining[i]] = j; // add upper triangle if requested if (upper) { remaining[j]--; colind[rowadr[j] + remaining[j]] = i; } } } } // check for remaining; SHOULD NOT OCCUR for (int i = 0; i < nv; i++) { if (remaining[i] != 0) { mjERROR("unexpected remaining"); } } // check total nnz; SHOULD NOT OCCUR int expected_nnz = upper ? nD : (reduced ? nC : nM); if (rowadr[nv - 1] + rownnz[nv - 1] != expected_nnz) { mjERROR("sum of rownnz different from expected"); } // find diagonal indices if (diag) { for (int i = 0; i < nv; i++) { int adr = rowadr[i]; int j = 0; while (colind[adr + j] < i && j < rownnz[i]) { j++; } if (colind[adr + j] != i) { mjERROR("diagonal index not found"); } diag[i] = j; } } } // construct sparse representation of body-dof matrix void mj_makeBSparse(int nv, int nbody, int nB, const int* body_dofnum, const int* body_parentid, const int* body_dofadr, int* B_rownnz, int* B_rowadr, int* B_colind, int* count) { // set rownnz to subtree dofs counts, including self mju_zeroInt(B_rownnz, nbody); for (int i = nbody - 1; i > 0; i--) { B_rownnz[i] += body_dofnum[i]; B_rownnz[body_parentid[i]] += B_rownnz[i]; } // check if rownnz[0] != nv; SHOULD NOT OCCUR if (B_rownnz[0] != nv) { mjERROR("rownnz[0] different from nv"); } // add dofs in ancestors bodies for (int i = 0; i < nbody; i++) { int j = body_parentid[i]; while (j > 0) { B_rownnz[i] += body_dofnum[j]; j = body_parentid[j]; } } // compute rowadr B_rowadr[0] = 0; for (int i = 1; i < nbody; i++) { B_rowadr[i] = B_rowadr[i - 1] + B_rownnz[i - 1]; } // check if total nnz != nB; SHOULD NOT OCCUR if (nB != B_rowadr[nbody - 1] + B_rownnz[nbody - 1]) { mjERROR("sum of rownnz different from nB"); } // clear incremental row counts mju_zeroInt(count, nbody); // add subtree dofs to colind for (int i = nbody - 1; i > 0; i--) { // add this body's dofs to subtree for (int n = 0; n < body_dofnum[i]; n++) { B_colind[B_rowadr[i] + count[i]] = body_dofadr[i] + n; count[i]++; } // add body subtree to parent int par = body_parentid[i]; for (int n = 0; n < count[i]; n++) { B_colind[B_rowadr[par] + count[par]] = B_colind[B_rowadr[i] + n]; count[par]++; } } // add all ancestor dofs for (int i = 0; i < nbody; i++) { int par = body_parentid[i]; while (par > 0) { // add ancestor body dofs for (int n = 0; n < body_dofnum[par]; n++) { B_colind[B_rowadr[i] + count[i]] = body_dofadr[par] + n; count[i]++; } // advance to parent par = body_parentid[par]; } } // process all bodies for (int i = 0; i < nbody; i++) { // make sure cnt = rownnz; SHOULD NOT OCCUR if (B_rownnz[i] != count[i]) { mjERROR("cnt different from rownnz"); } // sort colind in each row if (count[i] > 1) { mju_insertionSortInt(B_colind + B_rowadr[i], count[i]); } } } // check D and B sparsity for consistency static void checkDBSparse(const mjModel* m) { // process all dofs for (int j = 0; j < m->nv; j++) { // get body for this dof int i = m->dof_bodyid[j]; // D[row j] and B[row i] should be identical if (m->D_rownnz[j] != m->B_rownnz[i]) { mjERROR("rows have different nnz"); } for (int k = 0; k < m->D_rownnz[j]; k++) { if (m->D_colind[m->D_rowadr[j] + k] != m->B_colind[m->B_rowadr[i] + k]) { mjERROR("rows have different colind"); } } } } // integer valued dst[D or C or M] = src[M (legacy)], handle different sparsity representations static void copyM2Sparse(int nv, const int* dof_Madr, const int* dof_simplenum, const int* dof_parentid, const int* rownnz, const int* rowadr, const int* src, int* dst, int reduced, int upper, int* remaining) { // init remaining mju_copyInt(remaining, rownnz, nv); // copy data for (int i = nv - 1; i >= 0; i--) { // init at diagonal int adr = dof_Madr[i]; remaining[i]--; dst[rowadr[i] + remaining[i]] = src[adr]; adr++; // process below diagonal unless reduced and dof is simple if (!(reduced && dof_simplenum[i])) { int j = i; while ((j = dof_parentid[j]) >= 0) { remaining[i]--; dst[rowadr[i] + remaining[i]] = src[adr]; // add upper triangle if requested if (upper) { remaining[j]--; dst[rowadr[j] + remaining[j]] = src[adr]; } adr++; } } } // check that none remaining for (int i=0; i < nv; i++) { if (remaining[i]) { mjERROR("unassigned index"); } } } // construct index mappings between M <-> D, M -> C, M (legacy) -> M (CSR) void mj_makeDofDofMaps(int nv, int nM, int nC, int nD, const int* dof_Madr, const int* dof_simplenum, const int* dof_parentid, const int* D_rownnz, const int* D_rowadr, const int* D_colind, const int* M_rownnz, const int* M_rowadr, const int* M_colind, int* mapM2D, int* mapD2M, int* mapM2M, int* M, int* scratch) { // make mapM2D: M -> D (lower to symmetric) mju_lower2SymMap(mapM2D, nv, D_rowadr, D_rownnz, D_colind, M_rowadr, M_rownnz, M_colind, scratch); // make mapD2M: D -> M (symmetric to lower) mju_sparseMap(mapD2M, nv, M_rowadr, M_rownnz, M_colind, D_rowadr, D_rownnz, D_colind); // make mapM2M for (int i=0; i < nM; i++) M[i] = i; mju_fillInt(mapM2M, -1, nC); copyM2Sparse(nv, dof_Madr, dof_simplenum, dof_parentid, M_rownnz, M_rowadr, M, mapM2M, /*reduced=*/1, /*upper=*/0, scratch); // check that all indices are filled in for (int i=0; i < nC; i++) { if (mapM2M[i] < 0) { mjERROR("unassigned index in mapM2M"); } } } //----------------------------------- mjData construction ------------------------------------------ // set pointers into mjData buffer static void mj_setPtrData(const mjModel* m, mjData* d) { char* ptr = (char*)d->buffer; // assign pointers with padding #define X(type, name, nr, nc) \ d->name = (type*)(ptr + SKIP((intptr_t)ptr)); \ ASAN_POISON_MEMORY_REGION(ptr, PTRDIFF(d->name, ptr)); \ ptr += SKIP((intptr_t)ptr) + sizeof(type)*(m->nr)*(nc); MJDATA_POINTERS #undef X // check size ptrdiff_t sz = ptr - (char*)d->buffer; if (d->nbuffer != sz) { mjERROR( "mjData buffer size mismatch, " "expected size: %" PRIu64 ", actual size: %td", d->nbuffer, sz); } // zero-initialize arena pointers #define X(type, name, nr, nc) d->name = NULL; MJDATA_ARENA_POINTERS #undef X d->contact = d->arena; } // initialize plugins, copy into d (required for deletion) void mj_initPlugin(const mjModel* m, mjData* d) { d->nplugin = m->nplugin; for (int i = 0; i < m->nplugin; ++i) { d->plugin[i] = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]); if (plugin->init && plugin->init(m, d, i) < 0) { mju_free(d->buffer); mju_free(d->arena); mju_free(d); mjERROR("plugin->init failed for plugin id %d", i); } } } // free mjData memory without destroying the struct static void freeDataBuffers(mjData* 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]); if (plugin->destroy) { plugin->destroy(d, i); } } mju_free(d->buffer); mju_free(d->arena); } // allocate and initialize raw mjData structure void mj_makeRawData(mjData** dest, const mjModel* m) { intptr_t offset = 0; int allocate = *dest ? 0 : 1; mjData* d = NULL; // allocate mjData if (!allocate) { d = *dest; freeDataBuffers(d); } else { d = (mjData*) mju_malloc(sizeof(mjData)); } if (!d) { mjERROR("could not allocate mjData"); } // prevent spurious timing print from mj_resetData before _resetData zeroes the struct d->timer[mjTIMER_STEP].number = 0; // compute buffer size d->nbuffer = 0; d->buffer = d->arena = NULL; #define X(type, name, nr, nc) \ if (!safeAddToBufferSize(&offset, &d->nbuffer, sizeof(type), m->nr, nc)) { \ if (allocate) mju_free(d); \ mju_warning("Invalid data: " #name " too large."); \ return; \ } MJDATA_POINTERS #undef X // copy stack size from model d->narena = m->narena; // allocate buffer d->buffer = mju_malloc(d->nbuffer); if (!d->buffer) { if (allocate) mju_free(d); mjERROR("could not allocate mjData buffer"); } // allocate arena d->arena = mju_malloc(d->narena); if (!d->arena) { mju_free(d->buffer); if (allocate) mju_free(d); mjERROR("could not allocate mjData arena"); } // set pointers into buffer mj_setPtrData(m, d); // clear threadpool d->threadpool = 0; d->threadlock = 0; // clear nplugin (overwritten by _initPlugin) d->nplugin = 0; // set awake array sizes to default (all awake) d->ntree_awake = m->ntree; d->nbody_awake = d->nparent_awake = m->nbody; d->nv_awake = m->nv; // copy pointer if allocated here if (allocate) { *dest = d; } } // allocate and initialize mjData structure mjData* mj_makeData(const mjModel* m) { mjData* d = NULL; mj_makeRawData(&d, m); if (d) { mj_initPlugin(m, d); mj_resetData(m, d); } return d; } // copy mjData, if dest==NULL create new data; // flg_all 1: copy all fields, 0: skip fields not required for visualization mjData* mj_copyDataVisual(mjData* dest, const mjModel* m, const mjData* src, int flg_all) { void* save_buffer; void* save_arena; // allocate new data if needed if (!dest) { mj_makeRawData(&dest, m); mj_initPlugin(m, dest); } // check sizes if (dest->nbuffer != src->nbuffer) { mjERROR("dest and src data buffers have different size"); } if (dest->narena != src->narena) { mjERROR("dest and src stacks have different size"); } // stack is in use if (src->pstack) { mjERROR("attempting to copy mjData while stack is in use"); } // save pointers, copy everything, restore pointers save_buffer = dest->buffer; save_arena = dest->arena; *dest = *src; dest->buffer = save_buffer; dest->arena = save_arena; dest->threadpool = 0; mj_setPtrData(m, dest); // save plugin_data, since the X macro copying block below will override it const size_t plugin_data_size = sizeof(*dest->plugin_data) * dest->nplugin; uintptr_t* save_plugin_data = NULL; if (plugin_data_size) { save_plugin_data = (uintptr_t*)mju_malloc(plugin_data_size); if (!save_plugin_data) { mjERROR("failed to allocate temporary memory for plugin_data"); } memcpy(save_plugin_data, dest->plugin_data, plugin_data_size); } // copy buffer { if (flg_all) { #define X(type, name, nr, nc) \ memcpy((char*)dest->name, (const char*)src->name, sizeof(type)*(m->nr)*nc); MJDATA_POINTERS #undef X } else { // redefine XNV to nothing #undef XNV #define XNV(type, name, nr, nc) #define X(type, name, nr, nc) \ memcpy((char*)dest->name, (const char*)src->name, sizeof(type)*(m->nr)*nc); MJDATA_POINTERS #undef X // redefine XNV to be the same as X #undef XNV #define XNV X } } // copy arena memory #undef MJ_D #define MJ_D(n) (src->n) #undef MJ_M #define MJ_M(n) (m->n) if (flg_all) { #define X(type, name, nr, nc) \ if (src->name) { \ dest->name = (type*)((char*)dest->arena + PTRDIFF(src->name, src->arena)); \ ASAN_UNPOISON_MEMORY_REGION(dest->name, sizeof(type) * nr * nc); \ memcpy((char*)dest->name, (const char*)src->name, sizeof(type) * nr * nc); \ } else { \ dest->name = NULL; \ } MJDATA_ARENA_POINTERS #undef X } else { // redefine XNV to nothing #undef XNV #define XNV(type, name, nr, nc) #define X(type, name, nr, nc) \ if (src->name) { \ dest->name = (type*)((char*)dest->arena + PTRDIFF(src->name, src->arena)); \ ASAN_UNPOISON_MEMORY_REGION(dest->name, sizeof(type) * nr * nc); \ memcpy((char*)dest->name, (const char*)src->name, sizeof(type) * nr * nc); \ } else { \ dest->name = NULL; \ } MJDATA_ARENA_POINTERS #undef X // redefine XNV to be the same as X #undef XNV #define XNV X } #undef MJ_M #define MJ_M(n) n #undef MJ_D #define MJ_D(n) n // restore contact pointer dest->contact = dest->arena; // restore plugin_data if (plugin_data_size) { memcpy(dest->plugin_data, save_plugin_data, plugin_data_size); mju_free(save_plugin_data); save_plugin_data = NULL; } // copy plugin instances dest->nplugin = m->nplugin; for (int i = 0; i < m->nplugin; ++i) { const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]); if (plugin->copy) { plugin->copy(dest, m, src, i); } } return dest; } mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) { return mj_copyDataVisual(dest, m, src, /*flg_all=*/1); } mjData* mjv_copyData(mjData* dest, const mjModel* m, const mjData* src) { return mj_copyDataVisual(dest, m, src, /*flg_all=*/0); } // clear data, set defaults static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { // error early if history buffers cannot be initialized mjtNum dt = m->opt.timestep; if (m->nhistory && dt <= 0) { mjERROR("history buffers require positive timestep, got %g", dt); } //------------------------------ save plugin state and data mjtNum* plugin_state; uintptr_t* plugindata; if (d->nplugin) { plugin_state = mju_malloc(sizeof(mjtNum) * m->npluginstate); memcpy(plugin_state, d->plugin_state, sizeof(mjtNum) * m->npluginstate); plugindata = mju_malloc(sizeof(uintptr_t) * m->nplugin); memcpy(plugindata, d->plugin_data, sizeof(uintptr_t) * m->nplugin); } //------------------------------ clear header // clear stack pointer if (!d->threadpool) { d->pstack = 0; } d->pbase = 0; // clear arena pointers d->parena = 0; // poison the entire arena+stack memory region when built with asan #ifdef ADDRESS_SANITIZER ASAN_POISON_MEMORY_REGION(d->arena, d->narena); #endif #ifdef MEMORY_SANITIZER __msan_allocated_memory(d->arena, d->narena); #endif #define X(type, name, nr, nc) d->name = NULL; MJDATA_ARENA_POINTERS #undef X d->contact = d->arena; // clear memory utilization stats d->maxuse_stack = 0; d->maxuse_arena = 0; d->maxuse_con = 0; d->maxuse_efc = 0; // clear solver diagnostics memset(d->warning, 0, mjNWARNING*sizeof(mjWarningStat)); memset(d->timer, 0, mjNTIMER*sizeof(mjTimerStat)); memset(d->solver, 0, mjNSOLVER*mjNISLAND*sizeof(mjSolverStat)); mju_zeroInt(d->solver_niter, mjNISLAND); mju_zeroInt(d->solver_nnz, mjNISLAND); mju_zero(d->solver_fwdinv, 2); // clear variable sizes d->ncon = 0; d->ne = 0; d->nf = 0; d->nl = 0; d->nefc = 0; d->nJ = 0; d->nY = 0; d->nA = 0; d->nisland = 0; d->nidof = 0; // clear global properties d->time = 0; mju_zero(d->energy, 2); // clear lazy evaluation flags d->flg_energypos = 0; d->flg_energyvel = 0; d->flg_subtreevel = 0; d->flg_rnepost = 0; //------------------------------ clear buffer, set defaults // fill buffer with debug_value (normally 0) #ifdef ADDRESS_SANITIZER { #define X(type, name, nr, nc) memset(d->name, (int)debug_value, sizeof(type)*(m->nr)*(nc)); MJDATA_POINTERS #undef X } #else memset(d->buffer, (int)debug_value, d->nbuffer); #endif #ifdef MEMORY_SANITIZER // under MSAN, mark the entire buffer as uninitialized __msan_allocated_memory(d->buffer, d->nbuffer); #endif // zero out user-settable state and input arrays (MSAN: mark as initialized) mju_zero(d->qpos, m->nq); mju_zero(d->qvel, m->nv); mju_zero(d->act, m->na); mju_zero(d->ctrl, m->nu); for (int i=0; i < m->neq; i++) d->eq_active[i] = m->eq_active0[i]; mju_zero(d->qfrc_applied, m->nv); mju_zero(d->xfrc_applied, 6*m->nbody); mju_zero(d->qacc, m->nv); // input to inverse dynamics mju_zero(d->qacc_warmstart, m->nv); mju_zero(d->act_dot, m->na); mju_zero(d->userdata, m->nuserdata); mju_zero(d->mocap_pos, 3*m->nmocap); mju_zero(d->mocap_quat, 4*m->nmocap); // initialize ctrl history buffers: timestamps at [-n*dt, ..., -dt] for (int i = 0; i < m->nu; i++) { int n = m->actuator_history[2*i]; if (n > 0) { mjtNum* buf = d->history + m->actuator_historyadr[i]; buf[0] = 0; // user slot buf[1] = n - 1; // cursor: newest at logical index n-1 mjtNum* times = buf + 2; for (int j = 0; j < n; j++) { times[j] = -(n-j)*dt; } // clear values mjtNum* values = buf + 2 + n; mju_zero(values, n); } } // initialize sensor history buffers for (int i = 0; i < m->nsensor; i++) { int n = m->sensor_history[2*i]; if (n > 0) { int dim = m->sensor_dim[i]; mjtNum period = m->sensor_interval[2*i]; mjtNum phase = m->sensor_interval[2*i+1]; mjtNum* buf = d->history + m->sensor_historyadr[i]; // user slot: last compute time (phase=0 means -period, i.e. first compute at t=0) buf[0] = (period > 0) ? (phase == 0 ? -period : phase) : -dt; buf[1] = n - 1; // cursor: newest at logical index n-1 mjtNum* times = buf + 2; if (period > 0) { // samples spaced at period intervals, rounded up to dt grid mjtNum t0 = (phase == 0) ? -period : phase; for (int j = 0; j < n; j++) { mjtNum continuous_t = t0 - (n-1-j)*period; times[j] = mju_ceil(continuous_t / dt) * dt; } } else { // no period: timestamps at [-n*dt, ..., -dt] for (int j = 0; j < n; j++) { times[j] = -(n-j)*dt; } } // clear values mjtNum* values = buf + 2 + n; mju_zero(values, n*dim); } } // zero out qM, special case because scattering from M skips simple body off-diagonals mju_zero(d->qM, m->nM); // copy qpos0 from model if (m->qpos0) { mju_copy(d->qpos, m->qpos0, m->nq); } static int kAwake = -(1+mjMINAWAKE); // tree_asleep value for fully awake tree // set all trees to awake mju_fillInt(d->tree_asleep, kAwake, m->ntree); // sleep enabled: handle static bodies and trees marked as mjSLEEP_INIT if (mjENABLED(mjENBL_SLEEP)) { // count trees initialized as asleep int num_asleep_init = 0; for (int i=0; i < m->ntree; i++) { num_asleep_init += (m->tree_sleep_policy[i] == mjSLEEP_INIT); } // update sleep arrays, treat static bodies as awake mj_updateSleepInit(m, d, /*flg_staticawake*/ 1); // partial mj_fwdPosition, functions that update STATIC values if (!num_asleep_init) { mj_kinematics(m, d); mj_comPos(m, d); mj_camlight(m, d); mj_tendon(m, d); } // if any trees initialized as sleeping call entire mj_forward, put them to sleep else { mj_forward(m, d); // mark asleep-init trees as ready to sleep for (int i=0; i < m->ntree; i++) { int init = m->tree_sleep_policy[i] == mjSLEEP_INIT; d->tree_asleep[i] = init ? -1 : kAwake; } int nslept = mj_sleep(m, d); // raise error if any failed to sleep if (nslept != num_asleep_init) { // find root body of the first tree that could not be slept int root = -1; for (int i=0; i < m->ntree; i++) { if (m->tree_sleep_policy[i] == mjSLEEP_INIT && d->tree_asleep[i] < 0) { root = m->tree_bodyadr[i]; break; } } // free all memory held by d just before aborting mj_deleteData(d); // raise error and abort const char* hasname = mj_id2name(m, mjOBJ_BODY, root); const char* name = hasname ? hasname : ""; mjERROR("%d trees were marked as sleep='init' but only %d could be slept.\n" "Body '%s' (id=%d) is the root of the first tree that could not be slept.", num_asleep_init, nslept, name, root); } // clear arrays to avoid MSAN errors upon mid-step wake mju_zero(d->qacc_smooth, m->nv); mju_zero(d->qfrc_smooth, m->nv); // clear arena mj_clearEfc(d); } } // update sleep arrays and counters mj_updateSleep(m, d); // set mocap_pos/quat = body_pos/quat for mocap bodies if (m->body_mocapid) { for (int i=0; i < m->nbody; i++) { int id = m->body_mocapid[i]; if (id >= 0) { mju_copy3(d->mocap_pos+3*id, m->body_pos+3*i); mju_copy4(d->mocap_quat+4*id, m->body_quat+4*i); } } } else { // set the mocap_quats to {1, 0, 0, 0} for (int i=0; i < m->nmocap; i++) { d->mocap_quat[4*i] = 1.0; } } // check consistency of sparse matrix representations checkDBSparse(m); // restore pluginstate and plugindata if (d->nplugin) { memcpy(d->plugin_state, plugin_state, sizeof(mjtNum) * m->npluginstate); mju_free(plugin_state); memcpy(d->plugin_data, plugindata, sizeof(uintptr_t) * m->nplugin); mju_free(plugindata); // restore the plugin array back into d and reset the instances for (int i = 0; i < m->nplugin; ++i) { d->plugin[i] = m->plugin[i]; const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]); if (plugin->reset) { plugin->reset(m, &d->plugin_state[m->plugin_stateadr[i]], (void*)(d->plugin_data[i]), i); } } } // copy signature from model d->signature = m->signature; } // emit step timing diagnostics static void mj_logTimingDiagnostics(const mjData* d) { int nstep = d->timer[mjTIMER_STEP].number; if (nstep <= 0) { return; } mjtNum tstep = d->timer[mjTIMER_STEP].duration / nstep; if (tstep <= 0) { return; } char buf[2048]; int pos = 0; mjtNum components = 0; for (int i = mjTIMER_POSITION; i <= mjTIMER_ADVANCE; i++) { if (d->timer[i].number > 0) { mjtNum istep = d->timer[i].duration / d->timer[i].number; components += istep; pos += snprintf(buf + pos, sizeof(buf) - pos, "%s %-15s %8.1f (%5.1f%%)", pos > 0 ? "\n" : "", mjTIMERSTRING[i], istep * 1000, 100 * istep / tstep); // position sub-breakdown if (i == mjTIMER_POSITION) { for (int p = mjTIMER_POS_KINEMATICS; p <= mjTIMER_POS_PROJECT; p++) { if (d->timer[p].number > 0) { mjtNum pstep = d->timer[p].duration / d->timer[p].number; pos += snprintf(buf + pos, sizeof(buf) - pos, "\n %-13s %8.1f (%5.1f%%)", mjTIMERSTRING[p] + 4, pstep * 1000, 100 * pstep / tstep); // collision sub-breakdown if (p == mjTIMER_POS_COLLISION) { for (int c = mjTIMER_COL_BROAD; c <= mjTIMER_COL_NARROW; c++) { if (d->timer[c].number > 0) { mjtNum cstep = d->timer[c].duration / d->timer[c].number; pos += snprintf(buf + pos, sizeof(buf) - pos, "\n %-11s %8.1f (%5.1f%%)", mjTIMERSTRING[c] + 4, cstep * 1000, 100 * cstep / tstep); } } } } } } } } mjtNum other = tstep - components; pos += snprintf(buf + pos, sizeof(buf) - pos, "%s %-15s %8.1f (%5.1f%%)", pos > 0 ? "\n" : "", "other", other * 1000, 100 * other / tstep); pos += snprintf(buf + pos, sizeof(buf) - pos, "%s %-15s %8.1f", pos > 0 ? "\n" : "", "total", tstep * 1000); mjLogMessage msg = {.level = mjLOG_INFO, .topic = mjTOPIC_TIME_STP, .body = buf}; snprintf(msg.subject, sizeof(msg.subject), "average time per step (%d steps, units: \u00B5s)", nstep); mju_message(&msg); } // clear data, set data->qpos = model->qpos0 void mj_resetData(const mjModel* m, mjData* d) { // emit step timing diagnostics before timers are cleared mj_logTimingDiagnostics(d); _resetData(m, d, 0); } // clear data, set data->qpos = model->qpos0, fill with debug_value void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value) { _resetData(m, d, debug_value); } // Reset data. If 0 <= key < nkey, set fields from specified keyframe. void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key) { _resetData(m, d, 0); // copy keyframe data if key is valid if (key >= 0 && key < m->nkey) { d->time = m->key_time[key]; mju_copy(d->qpos, m->key_qpos+key*m->nq, m->nq); mju_copy(d->qvel, m->key_qvel+key*m->nv, m->nv); mju_copy(d->act, m->key_act+ key*m->na, m->na); mju_copy(d->mocap_pos, m->key_mpos+key*3*m->nmocap, 3*m->nmocap); mju_copy(d->mocap_quat, m->key_mquat+key*4*m->nmocap, 4*m->nmocap); mju_copy(d->ctrl, m->key_ctrl+key*m->nu, m->nu); } } // de-allocate mjData void mj_deleteData(mjData* d) { if (d) { mju_threadpool(d, 0); freeDataBuffers(d); mju_free(d); } } // number of position and velocity coordinates for each joint type const int nPOS[4] = {7, 4, 1, 1}; const int nVEL[4] = {6, 3, 1, 1}; static int sensorSize(mjtSensor sensor_type, int sensor_dim) { switch (sensor_type) { case mjSENS_TOUCH: case mjSENS_RANGEFINDER: case mjSENS_JOINTPOS: case mjSENS_JOINTVEL: case mjSENS_TENDONPOS: case mjSENS_TENDONVEL: case mjSENS_ACTUATORPOS: case mjSENS_ACTUATORVEL: case mjSENS_ACTUATORFRC: case mjSENS_JOINTACTFRC: case mjSENS_TENDONACTFRC: case mjSENS_JOINTLIMITPOS: case mjSENS_JOINTLIMITVEL: case mjSENS_JOINTLIMITFRC: case mjSENS_TENDONLIMITPOS: case mjSENS_TENDONLIMITVEL: case mjSENS_TENDONLIMITFRC: case mjSENS_GEOMDIST: case mjSENS_INSIDESITE: case mjSENS_E_POTENTIAL: case mjSENS_E_KINETIC: case mjSENS_CLOCK: return 1; case mjSENS_CAMPROJECTION: return 2; case mjSENS_ACCELEROMETER: case mjSENS_VELOCIMETER: case mjSENS_GYRO: case mjSENS_FORCE: case mjSENS_TORQUE: case mjSENS_MAGNETOMETER: case mjSENS_BALLANGVEL: case mjSENS_FRAMEPOS: case mjSENS_FRAMEXAXIS: case mjSENS_FRAMEYAXIS: case mjSENS_FRAMEZAXIS: case mjSENS_FRAMELINVEL: case mjSENS_FRAMEANGVEL: case mjSENS_FRAMELINACC: case mjSENS_FRAMEANGACC: case mjSENS_SUBTREECOM: case mjSENS_SUBTREELINVEL: case mjSENS_SUBTREEANGMOM: case mjSENS_GEOMNORMAL: return 3; case mjSENS_GEOMFROMTO: return 6; case mjSENS_BALLQUAT: case mjSENS_FRAMEQUAT: return 4; case mjSENS_CONTACT: case mjSENS_TACTILE: case mjSENS_USER: return sensor_dim; case mjSENS_PLUGIN: return -1; // don't use a 'default' case, so compiler warns about missing values } return -1; } // returns the number of objects of the given type // -1: mjOBJ_UNKNOWN // -2: invalid objtype static int numObjects(const mjModel* m, mjtObj objtype) { switch (objtype) { case mjOBJ_DEFAULT: case mjOBJ_FRAME: case mjOBJ_UNKNOWN: case mjOBJ_MODEL: return -1; case mjOBJ_BODY: case mjOBJ_XBODY: return m->nbody; case mjOBJ_JOINT: return m->njnt; case mjOBJ_DOF: return m->nv; case mjOBJ_GEOM: return m->ngeom; case mjOBJ_SITE: return m->nsite; case mjOBJ_CAMERA: return m->ncam; case mjOBJ_LIGHT: return m->nlight; case mjOBJ_FLEX: return m->nflex; case mjOBJ_MESH: return m->nmesh; case mjOBJ_SKIN: return m->nskin; case mjOBJ_HFIELD: return m->nhfield; case mjOBJ_TEXTURE: return m->ntex; case mjOBJ_MATERIAL: return m->nmat; case mjOBJ_PAIR: return m->npair; case mjOBJ_EXCLUDE: return m->nexclude; case mjOBJ_EQUALITY: return m->neq; case mjOBJ_TENDON: return m->ntendon; case mjOBJ_ACTUATOR: return m->nu; case mjOBJ_SENSOR: return m->nsensor; case mjOBJ_NUMERIC: return m->nnumeric; case mjOBJ_TEXT: return m->ntext; case mjOBJ_TUPLE: return m->ntuple; case mjOBJ_KEY: return m->nkey; case mjOBJ_PLUGIN: return m->nplugin; case mjNOBJECT: return -2; } return -2; } // validate reference fields in a model; return null if valid, error message otherwise const char* mj_validateReferences(const mjModel* m) { // for each field in mjModel that refers to another field, call X with: // adrarray: array containing the references // nadrs: number of elements in refarray // ntarget: number of elements in array where references are pointing // numarray: if refarray is an adr array, numarray is the corresponding num array, otherwise 0 #define MJMODEL_REFERENCES \ X(body_parentid, nbody, nbody , 0 ) \ X(body_rootid, nbody, nbody , 0 ) \ X(body_weldid, nbody, nbody , 0 ) \ X(body_mocapid, nbody, nmocap , 0 ) \ X(body_jntadr, nbody, njnt , m->body_jntnum ) \ X(body_dofadr, nbody, nv , m->body_dofnum ) \ X(body_geomadr, nbody, ngeom , m->body_geomnum ) \ X(body_bvhadr, nbody, nbvh , m->body_bvhnum ) \ X(body_plugin, nbody, nplugin , 0 ) \ X(jnt_qposadr, njnt, nq , 0 ) \ X(jnt_dofadr, njnt, nv , 0 ) \ X(jnt_bodyid, njnt, nbody , 0 ) \ X(dof_bodyid, nv, nbody , 0 ) \ X(dof_jntid, nv, njnt , 0 ) \ X(dof_parentid, nv, nv , 0 ) \ X(dof_Madr, nv, nM , 0 ) \ X(tree_bodyadr, ntree, nbody , m->tree_bodynum ) \ X(tree_dofadr, ntree, nv , m->tree_dofnum ) \ X(geom_bodyid, ngeom, nbody , 0 ) \ X(geom_matid, ngeom, nmat , 0 ) \ X(site_bodyid, nsite, nbody , 0 ) \ X(site_matid, nsite, nmat , 0 ) \ X(cam_bodyid, ncam, nbody , 0 ) \ X(cam_targetbodyid, ncam, nbody , 0 ) \ X(light_bodyid, nlight, nbody , 0 ) \ X(light_targetbodyid, nlight, nbody , 0 ) \ X(mesh_vertadr, nmesh, nmeshvert , m->mesh_vertnum ) \ X(mesh_normaladr, nmesh, nmeshnormal , m->mesh_normalnum ) \ X(mesh_texcoordadr, nmesh, nmeshtexcoord , m->mesh_texcoordnum ) \ X(mesh_faceadr, nmesh, nmeshface , m->mesh_facenum ) \ X(mesh_bvhadr, nmesh, nbvh , m->mesh_bvhnum ) \ X(mesh_graphadr, nmesh, nmeshgraph , 0 ) \ X(mesh_polyadr, nmesh, nmeshpoly , m->mesh_polynum ) \ X(mesh_polyvertadr, nmeshpoly, nmeshpolyvert , m->mesh_polyvertnum ) \ X(mesh_polymapadr, nmeshvert, nmeshpolymap , m->mesh_polymapnum ) \ X(flex_vertadr, nflex, nflexvert , m->flex_vertnum ) \ X(flex_edgeadr, nflex, nflexedge , m->flex_edgenum ) \ X(flex_elemadr, nflex, nflexelem , m->flex_elemnum ) \ X(flex_evpairadr, nflex, nflexevpair , m->flex_evpairnum ) \ X(flex_texcoordadr, nflex, nflextexcoord , 0 ) \ X(flex_elemdataadr, nflex, nflexelemdata , 0 ) \ X(flex_elemedgeadr, nflex, nflexelemedge , 0 ) \ X(flex_shelldataadr, nflex, nflexshelldata, 0 ) \ X(flex_edge, nflexedge*2, nflexvert , 0 ) \ X(flex_elem, nflexelemdata, nflexvert , 0 ) \ X(flex_elemedge, nflexelemedge, nflexedge , 0 ) \ X(flex_shell, nflexshelldata, nflexvert , 0 ) \ X(flex_bvhadr, nflex, nbvh , m->flex_bvhnum ) \ X(skin_matid, nskin, nmat , 0 ) \ X(skin_vertadr, nskin, nskinvert , m->skin_vertnum ) \ X(skin_texcoordadr, nskin, nskintexvert , 0 ) \ X(skin_faceadr, nskin, nskinface , m->skin_facenum ) \ X(skin_boneadr, nskin, nskinbone , m->skin_bonenum ) \ X(skin_bonevertadr, nskinbone, nskinbonevert , m->skin_bonevertnum ) \ X(skin_bonebodyid, nskinbone, nbody , 0 ) \ X(skin_bonevertid, nskinbonevert, nskinvert , 0 ) \ X(pair_geom1, npair, ngeom , 0 ) \ X(pair_geom2, npair, ngeom , 0 ) \ X(actuator_plugin, nu, nplugin , 0 ) \ X(actuator_actadr, nu, na , m->actuator_actnum ) \ X(sensor_plugin, nsensor, nplugin , 0 ) \ X(plugin_stateadr, nplugin, npluginstate , m->plugin_statenum ) \ X(plugin_attradr, nplugin, npluginattr , 0 ) \ X(tendon_adr, ntendon, nwrap , m->tendon_num ) \ X(tendon_matid, ntendon, nmat , 0 ) \ X(tendon_treeid, ntendon*2, ntree , 0 ) \ X(numeric_adr, nnumeric, nnumericdata , m->numeric_size ) \ X(text_adr, ntext, ntextdata , m->text_size ) \ X(tuple_adr, ntuple, ntupledata , m->tuple_size ) \ X(name_bodyadr, nbody, nnames , 0 ) \ X(name_jntadr, njnt, nnames , 0 ) \ X(name_geomadr, ngeom, nnames , 0 ) \ X(name_siteadr, nsite, nnames , 0 ) \ X(name_camadr, ncam, nnames , 0 ) \ X(name_lightadr, nlight, nnames , 0 ) \ X(name_meshadr, nmesh, nnames , 0 ) \ X(name_skinadr, nskin, nnames , 0 ) \ X(name_hfieldadr, nhfield, nnames , 0 ) \ X(name_texadr, ntex, nnames , 0 ) \ X(name_matadr, nmat, nnames , 0 ) \ X(name_pairadr, npair, nnames , 0 ) \ X(name_excludeadr, nexclude, nnames , 0 ) \ X(name_eqadr, neq, nnames , 0 ) \ X(name_tendonadr, ntendon, nnames , 0 ) \ X(name_actuatoradr, nu, nnames , 0 ) \ X(name_sensoradr, nsensor, nnames , 0 ) \ X(name_numericadr, nnumeric, nnames , 0 ) \ X(name_textadr, ntext, nnames , 0 ) \ X(name_tupleadr, ntuple, nnames , 0 ) \ X(name_keyadr, nkey, nnames , 0 ) \ X(hfield_pathadr, nhfield, npaths , 0 ) \ X(mesh_pathadr, nmesh, npaths , 0 ) \ X(skin_pathadr, nskin, npaths , 0 ) \ X(tex_pathadr, ntex, npaths , 0 ) #define X(adrarray, nadrs, ntarget, numarray) { \ int *nums = (numarray); \ for (int i=0; inadrs; i++) { \ int adrsmin = m->adrarray[i]; \ int num = (nums ? nums[i] : 1); \ if (num < 0) { \ return "Invalid model: " #numarray " is negative."; \ } \ if (num > MAX_ARRAY_SIZE) { \ return "Invalid model: " #numarray " is too large."; \ } \ int adrsmax = m->adrarray[i] + num; \ if (adrsmax > m->ntarget || adrsmin < -1) { \ return "Invalid model: " #adrarray " out of bounds."; \ } \ } \ } MJMODEL_REFERENCES; #undef X #undef MJMODEL_REFERENCES // special logic that doesn't fit in the macro: for (int i=0; i < m->nbody; i++) { if (i > 0 && m->body_parentid[i] >= i) { return "Invalid model: bad body_parentid."; } if (m->body_rootid[i] > i) { return "Invalid model: bad body_rootid."; } if (m->body_weldid[i] > i) { return "Invalid model: bad body_weldid."; } } for (int i=0; i < m->njnt; i++) { if (m->jnt_type[i] >= 4 || m->jnt_type[i] < 0) { return "Invalid model: jnt_type out of bounds."; } int jnt_qposadr = m->jnt_qposadr[i] + nPOS[m->jnt_type[i]]; if (jnt_qposadr > m->nq || m->jnt_qposadr[i] < 0) { return "Invalid model: jnt_qposadr out of bounds."; } int jnt_dofadr = m->jnt_dofadr[i] + nVEL[m->jnt_type[i]]; if (jnt_dofadr > m->nv || m->jnt_dofadr[i] < 0) { return "Invalid model: jnt_dofadr out of bounds."; } } for (int i=0; i < m->nv; i++) { if (m->dof_parentid[i] >= i) { return "Invalid model: bad dof_parentid."; } } for (int i=0; i < m->ngeom; i++) { if (m->geom_condim[i] > 6 || m->geom_condim[i] < 0) { return "Invalid model: geom_condim out of bounds."; } if (m->geom_type[i] == mjGEOM_HFIELD) { if (m->geom_dataid[i] >= m->nhfield || m->geom_dataid[i] < -1) { return "Invalid model: geom_dataid out of bounds."; } } else if ((m->geom_type[i] == mjGEOM_MESH) || (m->geom_type[i] == mjGEOM_SDF)) { if (m->geom_dataid[i] >= m->nmesh || m->geom_dataid[i] < -1) { return "Invalid model: geom_dataid out of bounds."; } } } for (int i=0; i < m->nhfield; i++) { mjtSize hfield_adr = m->hfield_adr[i] + ((mjtSize) m->hfield_nrow[i]) * m->hfield_ncol[i]; if (hfield_adr > m->nhfielddata || m->hfield_adr[i] < 0) { return "Invalid model: hfield_adr out of bounds."; } } for (int i=0; i < m->ntex; i++) { mjtSize nbytes = ((mjtSize) m->tex_nchannel[i]) * m->tex_height[i] * m->tex_width[i]; if (m->tex_adr[i] + nbytes > m->ntexdata || m->tex_adr[i] < 0) { return "Invalid model: tex_adr out of bounds."; } } for (int i=0; i < m->npair; i++) { int pair_body1 = (m->pair_signature[i] & 0xFFFF); if (pair_body1 >= m->nbody || pair_body1 < 0) { return "Invalid model: pair_body1 out of bounds."; } int pair_body2 = (m->pair_signature[i] >> 16); if (pair_body2 >= m->nbody || pair_body2 < 0) { return "Invalid model: pair_body2 out of bounds."; } } for (int i=0; i < m->neq; i++) { int obj1id = m->eq_obj1id[i]; int obj2id = m->eq_obj2id[i]; int objtype = m->eq_objtype[i]; switch ((mjtEq) m->eq_type[i]) { case mjEQ_JOINT: if (obj1id >= m->njnt || obj1id < 0) { return "Invalid model: eq_obj1id out of bounds."; } // -1 is the value used if second object is omitted. if (obj2id >= m->njnt || obj2id < -1) { return "Invalid model: eq_obj2id out of bounds."; } break; case mjEQ_TENDON: if (obj1id >= m->ntendon || obj1id < 0) { return "Invalid model: eq_obj1id out of bounds."; } // -1 is the value used if second object is omitted. if (obj2id >= m->ntendon || obj2id < -1) { return "Invalid model: eq_obj2id out of bounds."; } break; case mjEQ_WELD: case mjEQ_CONNECT: if (objtype == mjOBJ_BODY) { if (obj1id >= m->nbody || obj1id < 0) { return "Invalid model: eq_obj1id out of bounds."; } if (obj2id >= m->nbody || obj2id < 0) { return "Invalid model: eq_obj2id out of bounds."; } } else if (objtype == mjOBJ_SITE) { if (obj1id >= m->nsite || obj1id < 0) { return "Invalid model: eq_obj1id out of bounds."; } if (obj2id >= m->nsite || obj2id < 0) { return "Invalid model: eq_obj2id out of bounds."; } } else { return "Invalid model: eq_objtype is not body or site."; } break; case mjEQ_FLEX: case mjEQ_FLEXVERT: case mjEQ_FLEXSTRAIN: if (obj1id >= m->nflex || obj1id < 0) { return "Invalid model: eq_obj1id out of bounds."; } // -1 is the value used if second object is omitted if (obj2id != -1) { return "Invalid model: eq_obj2id must be -1."; } break; default: // might occur in case of the now-removed distance equality constraint mjERROR("unknown equality constraint type."); } } for (int i=0; i < m->nwrap; i++) { int wrap_objid = m->wrap_objid[i]; switch ((mjtWrap) m->wrap_type[i]) { case mjWRAP_NONE: case mjWRAP_PULLEY: // wrap_objid not used. break; case mjWRAP_JOINT: if (wrap_objid >= m->njnt || wrap_objid < 0) { return "Invalid model: wrap_objid out of bounds."; } break; case mjWRAP_SITE: if (wrap_objid >= m->nsite || wrap_objid < 0) { return "Invalid model: wrap_objid out of bounds."; } break; case mjWRAP_SPHERE: case mjWRAP_CYLINDER: if (wrap_objid >= m->ngeom || wrap_objid < 0) { return "Invalid model: wrap_objid out of bounds."; } break; } } for (int i=0; i < m->nu; i++) { int actuator_trntype = m->actuator_trntype[i]; int id = m->actuator_trnid[2*i]; int idslider = m->actuator_trnid[2*i+1]; switch ((mjtTrn) actuator_trntype) { case mjTRN_JOINT: case mjTRN_JOINTINPARENT: if (id < 0 || id >= m->njnt) { return "Invalid model: actuator_trnid out of bounds."; } break; case mjTRN_TENDON: if (id < 0 || id >= m->ntendon) { return "Invalid model: actuator_trnid out of bounds."; } break; case mjTRN_SITE: if (id < 0 || id >= m->nsite) { return "Invalid model: actuator_trnid out of bounds."; } break; case mjTRN_SLIDERCRANK: if (id < 0 || id >= m->nsite) { return "Invalid model: actuator_trnid out of bounds."; } if (idslider < 0 || idslider >= m->nsite) { return "Invalid model: actuator_trnid out of bounds."; } break; case mjTRN_BODY: if (id < 0 || id >= m->nbody) { return "Invalid model: actuator_trnid out of bounds."; } break; case mjTRN_UNDEFINED: // actuator_trnid not used. break; } } for (int i=0; i < m->nsensor; i++) { mjtSensor sensor_type = m->sensor_type[i]; int sensor_size; if (sensor_type == mjSENS_PLUGIN) { const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[m->sensor_plugin[i]]); if (!plugin->nsensordata) { mjERROR("`nsensordata` is a null function pointer for plugin at slot %d", m->plugin[m->sensor_plugin[i]]); } sensor_size = plugin->nsensordata(m, m->sensor_plugin[i], i); } else { sensor_size = sensorSize(sensor_type, m->sensor_dim[i]); } if (sensor_size < 0) { return "Invalid model: Bad sensor_type."; } int sensor_adr = m->sensor_adr[i]; if (sensor_adr < 0 || sensor_adr + sensor_size > m->nsensordata) { return "Invalid model: sensor_adr out of bounds."; } int nobj = numObjects(m, m->sensor_objtype[i]); if (nobj == -2) { return "Invalid model: invalid sensor_objtype"; } if (nobj != -1 && (m->sensor_objid[i] < 0 || m->sensor_objid[i] >= nobj)) { return "Invalid model: invalid sensor_objid"; } nobj = numObjects(m, m->sensor_reftype[i]); if (nobj == -2) { return "Invalid model: invalid sensor_reftype"; } if (nobj != -1 && (m->sensor_refid[i] < -1 || m->sensor_refid[i] >= nobj)) { return "Invalid model: invalid sensor_refid"; } if (sensor_type == mjSENS_TACTILE) { int parent_body = m->geom_bodyid[m->sensor_refid[i]]; int collision_geoms = 0; for (int b = 0; b < m->body_geomnum[parent_body]; ++b) { int geom_id = m->body_geomadr[parent_body]+b; if (m->geom_contype[geom_id] || m->geom_conaffinity[geom_id]) { collision_geoms++; } } if (collision_geoms == 0) { return "Touch sensor requires a body with at least one collision geom"; } } } for (int i=0; i < m->nexclude; i++) { int exclude_body1 = (m->exclude_signature[i] & 0xFFFF); if (exclude_body1 >= m->nbody || exclude_body1 < 0) { return "Invalid model: exclude_body1 out of bounds."; } int exclude_body2 = (m->exclude_signature[i] >> 16); if (exclude_body2 >= m->nbody || exclude_body2 < 0) { return "Invalid model: exclude_body2 out of bounds."; } } for (int i=0; i < m->ntuple; i++) { for (int j=0; j < m->tuple_size[i]; j++) { int adr = m->tuple_adr[i] + j; int nobj = numObjects(m, m->tuple_objtype[adr]); if (nobj == -2) { return "Invalid model: invalid tuple_objtype"; } if (nobj != -1 && (m->tuple_objid[adr] < 0 || m->tuple_objid[adr] >= nobj)) { return "Invalid model: invalid tuple_objid"; } } } return NULL; }