// 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 #include #include #include #include #include #include #include #include #include #include "engine/engine_crossplatform.h" #include "engine/engine_resource.h" #include "engine/engine_macro.h" #include "engine/engine_plugin.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include "engine/engine_vfs.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 // add red zone padding when built with asan, to detect out-of-bound accesses #ifdef ADDRESS_SANITIZER #define mjREDZONE 32 #else #define mjREDZONE 0 #endif static const int MAX_ARRAY_SIZE = INT_MAX / 4; // compute a % b with a fast code path if the second argument is a power of 2 static inline size_t fastmod(size_t a, size_t b) { // (b & (b - 1)) == 0 implies that b is a power of 2 if (mjLIKELY((b & (b - 1)) == 0)) { return a & (b - 1); } return a % b; } typedef struct { size_t pbase; // value of d->pbase immediately before mj_markStack size_t pstack; // value of d->pstack immediately before mj_markStack void* pc; // program counter of the call site of mj_markStack (only set when under asan) } mjStackFrame; //------------------------------ mjLROpt ----------------------------------------------------------- // set default options for length range computation void mj_defaultLROpt(mjLROpt* opt) { opt->mode = mjLRMODE_MUSCLE; opt->useexisting = 1; opt->uselimit = 0; opt->accel = 20; opt->maxforce = 0; opt->timeconst = 1; opt->timestep = 0.01; opt->inttotal = 10; opt->interval = 2; opt->tolrange = 0.05; } //------------------------------- mjOption --------------------------------------------------------- // set default solver paramters void mj_defaultSolRefImp(mjtNum* solref, mjtNum* solimp) { if (solref) { solref[0] = 0.02; // timeconst solref[1] = 1; // dampratio } if (solimp) { solimp[0] = 0.9; // dmin solimp[1] = 0.95; // dmax solimp[2] = 0.001; // width solimp[3] = 0.5; // midpoint solimp[4] = 2; // power } } // set model options to default values void mj_defaultOption(mjOption* opt) { // timing parameters opt->timestep = 0.002; opt->apirate = 100; // solver parameters opt->impratio = 1; opt->tolerance = 1e-8; opt->noslip_tolerance = 1e-6; opt->mpr_tolerance = 1e-6; // physical constants opt->gravity[0] = 0; opt->gravity[1] = 0; opt->gravity[2] = -9.81; opt->wind[0] = 0; opt->wind[1] = 0; opt->wind[2] = 0; opt->magnetic[0] = 0; opt->magnetic[1] = -0.5; opt->magnetic[2] = 0; opt->density = 0; opt->viscosity = 0; // solver overrides opt->o_margin = 0; mj_defaultSolRefImp(opt->o_solref, opt->o_solimp); // discrete options opt->integrator = mjINT_EULER; opt->collision = mjCOL_ALL; opt->cone = mjCONE_PYRAMIDAL; opt->jacobian = mjJAC_AUTO; opt->solver = mjSOL_NEWTON; opt->iterations = 100; opt->noslip_iterations = 0; opt->mpr_iterations = 50; opt->disableflags = 0; opt->enableflags = 0; // sdf collisions opt->sdf_initpoints = 40; opt->sdf_iterations = 10; } //------------------------------- mjVisual --------------------------------------------------------- // set 4 floats static void setf4(float* rgba, float r, float g, float b, float a) { rgba[0] = r; rgba[1] = g; rgba[2] = b; rgba[3] = a; } // set visual options to default values void mj_defaultVisual(mjVisual* vis) { // global vis->global.fovy = 45; vis->global.ipd = 0.068; vis->global.azimuth = 90; vis->global.elevation = -45; vis->global.linewidth = 1.0; vis->global.glow = 0.3; vis->global.offwidth = 640; vis->global.offheight = 480; vis->global.realtime = 1.0; vis->global.ellipsoidinertia = 0; // rendering quality vis->quality.shadowsize = 4096; vis->quality.offsamples = 4; vis->quality.numslices = 28; vis->quality.numstacks = 16; vis->quality.numquads = 4; // head light vis->headlight.ambient[0] = 0.1; vis->headlight.ambient[1] = 0.1; vis->headlight.ambient[2] = 0.1; vis->headlight.diffuse[0] = 0.4; vis->headlight.diffuse[1] = 0.4; vis->headlight.diffuse[2] = 0.4; vis->headlight.specular[0] = 0.5; vis->headlight.specular[1] = 0.5; vis->headlight.specular[2] = 0.5; vis->headlight.active = 1; // map parameters vis->map.stiffness = 100; vis->map.stiffnessrot = 500; vis->map.force = 0.005; vis->map.torque = 0.1; vis->map.alpha = 0.3; vis->map.fogstart = 3.0; vis->map.fogend = 10.0; vis->map.znear = 0.01; vis->map.zfar = 50.0; vis->map.haze = 0.3; vis->map.shadowclip = 1.0; vis->map.shadowscale = 0.6; vis->map.actuatortendon = 2.0; // size parameters vis->scale.forcewidth = 0.1; vis->scale.contactwidth = 0.3; vis->scale.contactheight = 0.1; vis->scale.connect = 0.2; vis->scale.com = 0.4; vis->scale.camera = 0.3; vis->scale.light = 0.3; vis->scale.selectpoint = 0.2; vis->scale.jointlength = 1.0; vis->scale.jointwidth = 0.1; vis->scale.actuatorlength = 0.7; vis->scale.actuatorwidth = 0.2; vis->scale.framelength = 1.0; vis->scale.framewidth = 0.1; vis->scale.constraint = 0.1; vis->scale.slidercrank = 0.2; // colors setf4(vis->rgba.fog, 0., 0., 0., 1.); setf4(vis->rgba.haze, 1., 1., 1., 1.); setf4(vis->rgba.force, 1., .5, .5, 1.); setf4(vis->rgba.inertia, .8, .2, .2, .6); setf4(vis->rgba.joint, .2, .6, .8, 1.); setf4(vis->rgba.actuator, .2, .25, .2, 1); setf4(vis->rgba.actuatornegative, .2, .6, .9, 1.); setf4(vis->rgba.actuatorpositive, .9, .4, .2, 1.); setf4(vis->rgba.com, .9, .9, .9, 1.); setf4(vis->rgba.camera, .6, .9, .6, 1.); setf4(vis->rgba.light, .6, .6, .9, 1.); setf4(vis->rgba.selectpoint, .9, .9, .1, 1.); setf4(vis->rgba.connect, .2, .2, .8, 1.); setf4(vis->rgba.contactpoint, .9, .6, .2, 1.); setf4(vis->rgba.contactforce, .7, .9, .9, 1.); setf4(vis->rgba.contactfriction, .9, .8, .4, 1.); setf4(vis->rgba.contacttorque, .9, .7, .9, 1.); setf4(vis->rgba.contactgap, .5, .8, .9, 1.); setf4(vis->rgba.rangefinder, 1., 1., .1, 1.); setf4(vis->rgba.constraint, .9, .0, .0, 1.); setf4(vis->rgba.slidercrank, .5, .3, .8, 1.); setf4(vis->rgba.crankbroken, .9, .0, .0, 1.); } //------------------------------- mjStatistic ------------------------------------------------------ // set statistics to default values; compute later in compiler void mj_defaultStatistic(mjStatistic* stat) { mju_zero3(stat->center); stat->extent = 2; stat->meaninertia = 1; stat->meanmass = 1; stat->meansize = 0.2; } //----------------------------------- 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 ints in mjModel static int getnint(void) { int cnt = 0; #define X(name) cnt += _Generic(MJMODEL_MEMBER(name), int: 1, default: 0); MJMODEL_INTS #undef X return cnt; } // count size_t members in mjModel static int getnsize(void) { int cnt = 0; #define X(name) cnt += _Generic(MJMODEL_MEMBER(name), size_t: 1, default: 0); MJMODEL_INTS #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, int szbuf, void* buf, int* 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, int szbuf, const void* buf, int* 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; ptrdiff_t sz; // 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 sz = ptr - (char*)m->buffer; if (m->nbuffer != sz) { printf("expected size: %zu, actual size: %zu\n", m->nbuffer, sz); mjERROR("mjModel buffer size mismatch"); } } // 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 int safeAddToBufferSize(intptr_t* offset, size_t* nbuffer, size_t type_size, int nr, int 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 int 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 // TODO: offer a safe implementation for MSVC or other compilers that don't have the builtins *nbuffer += SKIP(*offset) + type_size*nr*nc; *offset += SKIP(*offset) + type_size*nr*nc; #endif return 1; } // allocate and initialize mjModel structure mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int njnt, int ngeom, int nsite, int ncam, int nlight, int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface, int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface, int nskinbone, int nskinbonevert, int nhfield, int nhfielddata, int ntex, int ntexdata, int nmat, int npair, int nexclude, int neq, int ntendon, int nwrap, int nsensor, int nnumeric, int nnumericdata, int ntext, int ntextdata, int ntuple, int ntupledata, int nkey, int nmocap, int nplugin, int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom, int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator, int nuser_sensor, int nnames) { intptr_t offset = 0; // allocate mjModel mjModel* 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->njnt = njnt; m->ngeom = ngeom; m->nsite = nsite; m->ncam = ncam; m->nlight = nlight; m->nmesh = nmesh; m->nmeshvert = nmeshvert; m->nmeshnormal = nmeshnormal; m->nmeshtexcoord = nmeshtexcoord; m->nmeshface = nmeshface; m->nmeshgraph = nmeshgraph; 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->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; m->nnames_map = mjLOAD_MULTIPLE * (nbody + njnt + ngeom + nsite + ncam + nlight + nmesh + nskin + nhfield + ntex + nmat + npair + nexclude + neq + ntendon + nu + nsensor + nnumeric + ntext + ntuple + nkey + nplugin); #define X(name) \ if ((m->name) < 0) { \ mju_free(m); \ mju_warning("Invalid model: negative " #name); \ return 0; \ } MJMODEL_INTS; #undef X // nbody should always be positive if (m->nbody == 0) { mju_free(m); mju_warning("Invalid model: nbody == 0"); return 0; } // nmocap is going to get multiplied by 4, and shouldn't overflow if (m->nmocap >= MAX_ARRAY_SIZE) { mju_free(m); mju_warning("Invalid model: nmocap too large"); return 0; } // compute buffer size m->nbuffer = 0; #define X(type, name, nr, nc) \ if (!safeAddToBufferSize(&offset, &m->nbuffer, sizeof(type), m->nr, nc)) { \ mju_free(m); \ mju_warning("Invalid model: " #name " too large."); \ return 0; \ } MJMODEL_POINTERS #undef X // allocate buffer m->buffer = mju_malloc(m->nbuffer); if (!m->buffer) { 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); return m; } // copy mjModel, if dest==NULL create new model mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { void* save_bufptr; // allocate new model if needed if (!dest) { dest = mj_makeModel(src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh, src->njnt, src->ngeom, src->nsite, src->ncam, src->nlight, src->nmesh, src->nmeshvert, src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph, 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->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); } 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 everything, restore buffer and other pointers save_bufptr = dest->buffer; *dest = *src; dest->buffer = save_bufptr; mj_setPtrModel(dest); // copy buffer { 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; } // 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; int ptrbuf = 0; // standard header int header[NHEADER] = {ID, sizeof(mjtNum), getnint(), getnsize(), 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_INTS #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(int)*sizeof(header) / sizeof(int), buffer_sz, buffer, &ptrbuf); #define X(name) bufwrite(&m->name, sizeof(m->name), buffer_sz, buffer, &ptrbuf); MJMODEL_INTS #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 model from binary MJB resource 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]; size_t sizes[8]; int ptrbuf = 0; mjModel *m = 0; mjResource* r = 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; int buffer_sz = mju_readResource(r, &buffer); if (buffer_sz <= 0) { mju_closeResource(r); return NULL; } if (buffer_sz < NHEADER*sizeof(int)) { mju_warning("Model file has an incomplete header"); mju_closeResource(r); return NULL; } bufread(header, NHEADER*sizeof(int), buffer_sz, buffer, &ptrbuf); // check header for (int i=0; i < NHEADER; i++) { if (header[i] != expected_header[i]) { switch (i) { case 0: mju_warning("Model missing header ID"); mju_closeResource(r); return NULL; case 1: mju_warning("Model and executable have different floating point precision"); mju_closeResource(r); return NULL; case 2: mju_warning("Model and executable have different number of ints in mjModel"); mju_closeResource(r); return NULL; case 3: mju_warning("Model and executable have different number of size_t members in mjModel"); mju_closeResource(r); return NULL; default: mju_warning("Model and executable have different number of pointers in mjModel"); mju_closeResource(r); return NULL; } } } // read mjModel structure: info only bufread(ints, sizeof(int)*getnint(), buffer_sz, buffer, &ptrbuf); bufread(sizes, sizeof(size_t)*getnsize(), buffer_sz, buffer, &ptrbuf); // allocate new mjModel, check sizes m = mj_makeModel(ints[0], ints[1], ints[2], ints[3], ints[4], ints[5], ints[6], ints[7], ints[8], ints[9], ints[10], ints[11], ints[12], ints[13], ints[14], ints[15], ints[16], ints[17], ints[18], ints[19], ints[20], ints[21], ints[22], ints[23], ints[24], ints[25], ints[26], ints[27], ints[28], ints[29], ints[30], ints[31], ints[32], ints[33], ints[34], ints[35], ints[36], ints[37], ints[38], ints[39], ints[40], ints[41], ints[42], ints[43], ints[44], ints[45], ints[46], ints[47], ints[48], ints[49], ints[50], ints[51], ints[52]); if (!m || m->nbuffer != sizes[getnsize()-1]) { mju_closeResource(r); mju_warning("Corrupted model, wrong size parameters"); mj_deleteModel(m); return NULL; } // set integer fields { int int_idx = 0; int size_idx = 0; #define X(name) \ m->name = _Generic(m->name, size_t: sizes[size_idx++], default: ints[int_idx++]); MJMODEL_INTS #undef X } // read options and buffer 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) \ 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_closeResource(r); mju_warning("Model file is too large"); mj_deleteModel(m); return NULL; } const char* validationError = mj_validateReferences(m); if (validationError) { mju_closeResource(r); mju_warning("%s", validationError); mj_deleteModel(m); return NULL; } mju_closeResource(r); return m; } // de-allocate mjModel void mj_deleteModel(mjModel* m) { if (m) { mju_free(m->buffer); mju_free(m); } } // size of buffer needed to hold model int mj_sizeModel(const mjModel* m) { int size = ( sizeof(int)*(NHEADER+getnint()) + sizeof(size_t)*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 static void makeDSparse(const mjModel* m, mjData* d) { int nv = m->nv; int* rownnz = d->D_rownnz; int* rowadr = d->D_rowadr; int* colind = d->D_colind; mjMARKSTACK; int* remaining = mj_stackAllocInt(d, nv); // 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 while ((j = m->dof_parentid[j]) >= 0) { rownnz[i]++; 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 int j = i; while ((j = m->dof_parentid[j]) >= 0) { remaining[i]--; colind[rowadr[i] + remaining[i]] = j; remaining[j]--; colind[rowadr[j] + remaining[j]] = i; } } // sanity check; SHOULD NOT OCCUR for (int i = 0; i < nv; i++) { if (remaining[i] != 0) { mjERROR("unexpected remaining"); } } mjFREESTACK; } // construct sparse representation of body-dof matrix static void makeBSparse(const mjModel* m, mjData* d) { int nv = m->nv, nbody = m->nbody; int* rownnz = d->B_rownnz; int* rowadr = d->B_rowadr; int* colind = d->B_colind; // set rownnz to subtree dofs counts, including self 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]; } // sanity check; SHOULD NOT OCCUR if (rownnz[0] != nv) { mjERROR("rownnz[0] different from nv"); } // add dofs in ancestors bodies for (int i = 0; i < nbody; i++) { int j = m->body_parentid[i]; while (j > 0) { rownnz[i] += m->body_dofnum[j]; j = m->body_parentid[j]; } } // compute rowadr rowadr[0] = 0; for (int i = 1; i < nbody; i++) { rowadr[i] = rowadr[i - 1] + rownnz[i - 1]; } // sanity check; SHOULD NOT OCCUR if (m->nB != rowadr[nbody - 1] + rownnz[nbody - 1]) { mjERROR("sum of rownnz different from nB"); } // allocate and clear incremental row counts mjMARKSTACK; int* cnt = mj_stackAllocInt(d, nbody); mju_zeroInt(cnt, 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 < m->body_dofnum[i]; n++) { colind[rowadr[i] + cnt[i]] = m->body_dofadr[i] + n; cnt[i]++; } // add body subtree to parent int par = m->body_parentid[i]; for (int n = 0; n < cnt[i]; n++) { colind[rowadr[par] + cnt[par]] = colind[rowadr[i] + n]; cnt[par]++; } } // add all ancestor dofs for (int i = 0; i < nbody; i++) { int par = m->body_parentid[i]; while (par > 0) { // add ancestor body dofs for (int n = 0; n < m->body_dofnum[par]; n++) { colind[rowadr[i] + cnt[i]] = m->body_dofadr[par] + n; cnt[i]++; } // advance to parent par = m->body_parentid[par]; } } // process all bodies for (int i = 0; i < nbody; i++) { // make sure cnt = rownnz; SHOULD NOT OCCUR if (rownnz[i] != cnt[i]) { mjERROR("cnt different from rownnz"); } // sort colind in each row if (cnt[i] > 1) { mju_insertionSortInt(colind + rowadr[i], cnt[i]); } } mjFREESTACK; } // check D and B sparsity for consistency static void checkDBSparse(const mjModel* m, mjData* d) { // 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 (d->D_rownnz[j] != d->B_rownnz[i]) { mjERROR("rows have different nnz"); } for (int k = 0; k < d->D_rownnz[j]; k++) { if (d->D_colind[d->D_rowadr[j] + k] != d->B_colind[d->B_rowadr[i] + k]) { mjERROR("rows have different colind"); } } } } //----------------------------------- mjData construction ------------------------------------------ // set pointers into mjData buffer static void mj_setPtrData(const mjModel* m, mjData* d) { char* ptr = (char*)d->buffer; int sz; // prepare symbols needed by xmacro MJDATA_POINTERS_PREAMBLE(m); // 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 sz = (int)(ptr - (char*)d->buffer); if (d->nbuffer != sz) { mjERROR("mjData buffer size mismatch"); } // zero-initialize arena pointers #define X(type, name, nr, nc) d->name = NULL; MJDATA_ARENA_POINTERS #undef X d->contact = d->arena; } // allocate and initialize mjData structure static mjData* _makeData(const mjModel* m) { intptr_t offset = 0; // allocate mjData mjData* d = (mjData*) mju_malloc(sizeof(mjData)); if (!d) { mjERROR("could not allocate mjData"); } // prepare symbols needed by xmacro MJDATA_POINTERS_PREAMBLE(m); // 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)) { \ mju_free(d); \ mju_warning("Invalid data: " #name " too large."); \ return 0; \ } MJDATA_POINTERS #undef X // copy stack size from model d->narena = m->narena; // allocate buffer d->buffer = mju_malloc(d->nbuffer); if (!d->buffer) { mju_free(d); mjERROR("could not allocate mjData buffer"); } // allocate arena d->arena = mju_malloc(d->narena); if (!d->arena) { mju_free(d->buffer); mju_free(d); mjERROR("could not allocate mjData arena"); } // set pointers into buffer, reset data mj_setPtrData(m, d); // copy plugins into d, required for deletion 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) { if (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); } } } d->threadpool = 0; return d; } mjData* mj_makeData(const mjModel* m) { mjData* d = _makeData(m); if (d) { mj_resetData(m, d); } return d; } // copy mjData, if dest==NULL create new data mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) { void* save_buffer; void* save_arena; // allocate new data if needed if (!dest) { dest = _makeData(m); } // 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; 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 { MJDATA_POINTERS_PREAMBLE(m) #define X(type, name, nr, nc) \ memcpy((char*)dest->name, (const char*)src->name, sizeof(type)*(m->nr)*nc); MJDATA_POINTERS #undef X } // copy arena memory #define X(type, name, nr, nc) \ dest->name = src->name ? (type*)((char*)dest->arena + PTRDIFF(src->name, src->arena)) : NULL; MJDATA_ARENA_POINTERS #undef X // restore contact pointer dest->contact = dest->arena; // restore plugin_data if (plugin_data_size) { memcpy(dest->plugin_data, save_plugin_data, plugin_data_size); 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); } } dest->threadpool = src->threadpool; return dest; } // allocate memory from the mjData arena void* mj_arenaAlloc(mjData* d, size_t bytes, size_t 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 (mjUNLIKELY(d->parena + padding + bytes > bytes_available)) { return NULL; } // allocate, update max, return pointer to buffer void* result = (char*)d->arena + d->parena + padding; d->parena += padding + bytes; d->maxuse_arena = mjMAX(d->maxuse_arena, d->pstack + d->parena); #ifdef ADDRESS_SANITIZER ASAN_UNPOISON_MEMORY_REGION(result, bytes); #endif #ifdef MEMORY_SANITIZER __msan_allocated_memory(result, bytes); #endif return result; } // internal: allocate size bytes on the mjData stack // 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 (mjUNLIKELY(!size)) { return NULL; } // size of entire arena/stack in bytes size_t stack_size_bytes = d->narena; // end of the arena uintptr_t end_of_arena_ptr = (uintptr_t)d->arena + stack_size_bytes; // current top of the stack uintptr_t end_ptr = end_of_arena_ptr - d->pstack; // start of the memory to be allocated to the buffer uintptr_t start_ptr = end_ptr - (size + mjREDZONE); // align the pointer start_ptr -= fastmod(start_ptr, alignment); // new top of the stack uintptr_t new_pstack_ptr = start_ptr - mjREDZONE; size_t new_pstack = end_of_arena_ptr - new_pstack_ptr; // exclude red zone from stack usage statistics size_t current_alloc_usage = end_ptr - new_pstack_ptr - 2 * mjREDZONE; size_t usage = current_alloc_usage + d->pstack; // 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 (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); } #ifdef ADDRESS_SANITIZER // actual stack usage (without red zone bytes) is stored in the red zone if (d->pstack) { size_t* prev_usage_ptr = (size_t*)(end_of_arena_ptr - d->pstack); ASAN_UNPOISON_MEMORY_REGION(prev_usage_ptr, sizeof(size_t)); usage = current_alloc_usage + *prev_usage_ptr; ASAN_POISON_MEMORY_REGION(prev_usage_ptr, sizeof(size_t)); } // store new stack usage in the red zone ASAN_UNPOISON_MEMORY_REGION((void*)new_pstack_ptr, sizeof(size_t)); *(size_t*)new_pstack_ptr = usage; ASAN_POISON_MEMORY_REGION((void*)new_pstack_ptr, sizeof(size_t)); // unpoison the actual usable allocation ASAN_UNPOISON_MEMORY_REGION((void*)start_ptr, size); #endif // update pstack and max usage statistics d->pstack = new_pstack; d->maxuse_stack = mjMAX(d->maxuse_stack, usage); d->maxuse_arena = mjMAX(d->maxuse_arena, usage + d->parena); 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); } mjtNum* mj_stackAllocNum(mjData* d, int size) { return (mjtNum*) stackalloc(d, size * sizeof(mjtNum), _Alignof(mjtNum)); } int* mj_stackAllocInt(mjData* d, int size) { return (int*) stackalloc(d, size * sizeof(int), _Alignof(int)); } // clear data, set defaults static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { //------------------------------ save plugin state and data mjtNum* plugin_state = mju_malloc(sizeof(mjtNum) * m->npluginstate); memcpy(plugin_state, d->plugin_state, sizeof(mjtNum) * m->npluginstate); uintptr_t* plugindata = mju_malloc(sizeof(uintptr_t) * m->nplugin); memcpy(plugindata, d->plugin_data, sizeof(uintptr_t) * m->nplugin); //------------------------------ clear header // clear stack pointer 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*sizeof(mjSolverStat)); d->solver_iter = 0; d->solver_nnz = 0; mju_zero(d->solver_fwdinv, 2); // clear collision diagnostics d->nbodypair_broad = 0; d->nbodypair_narrow = 0; d->ngeompair_mid = 0; d->ngeompair_narrow = 0; // clear variable sizes d->ne = 0; d->nf = 0; d->nl = 0; d->nefc = 0; d->nnzJ = 0; d->ncon = 0; d->nisland = 0; // clear global properties d->time = 0; mju_zero(d->energy, 2); //------------------------------ 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_PREAMBLE(m) MJDATA_POINTERS #undef X } #else memset(d->buffer, (int)debug_value, d->nbuffer); #endif #ifdef MEMORY_SANITIZER // Tell msan to treat the entire buffer as uninitialized __msan_allocated_memory(d->buffer, d->nbuffer); #endif // zero out arrays that are not affected by mj_forward mju_zero(d->qpos, m->nq); mju_zero(d->qvel, m->nv); mju_zero(d->act, m->na); mju_zero(d->ctrl, m->nu); mju_zero(d->qfrc_applied, m->nv); mju_zero(d->xfrc_applied, 6*m->nbody); mju_zero(d->qacc, m->nv); mju_zero(d->qacc_warmstart, m->nv); mju_zero(d->act_dot, m->na); mju_zero(d->userdata, m->nuserdata); mju_zero(d->sensordata, m->nsensordata); mju_zero(d->mocap_pos, 3*m->nmocap); mju_zero(d->mocap_quat, 4*m->nmocap); // copy qpos0 from model if (m->qpos0) { memcpy(d->qpos, m->qpos0, m->nq*sizeof(mjtNum)); } // 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; } } // construct sparse matrix representations if (m->body_dofadr) { makeDSparse(m, d); makeBSparse(m, d); checkDBSparse(m, d); } // restore pluginstate and plugindata 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); } } } // clear data, set data->qpos = model->qpos0 void mj_resetData(const mjModel* m, mjData* 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, 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) { #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); 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_JOINTLIMITPOS: case mjSENS_JOINTLIMITVEL: case mjSENS_JOINTLIMITFRC: case mjSENS_TENDONLIMITPOS: case mjSENS_TENDONLIMITVEL: case mjSENS_TENDONLIMITFRC: case mjSENS_CLOCK: return 1; 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: return 3; case mjSENS_BALLQUAT: case mjSENS_FRAMEQUAT: return 4; 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_UNKNOWN: 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_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; } 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(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(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(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 ) #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++) { int hfield_adr = m->hfield_adr[i] + 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++) { int tex_adr = m->tex_adr[i] + 3*m->tex_height[i]*m->tex_width[i]; if (tex_adr > 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) - 1; if (pair_body1 >= m->nbody || pair_body1 < 0) { return "Invalid model: pair_body1 out of bounds."; } int pair_body2 = (m->pair_signature[i] >> 16) - 1; 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]; 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 (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."; } 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"; } } for (int i=0; i < m->nexclude; i++) { int exclude_body1 = (m->exclude_signature[i] & 0xFFFF) - 1; if (exclude_body1 >= m->nbody || exclude_body1 < 0) { return "Invalid model: exclude_body1 out of bounds."; } int exclude_body2 = (m->exclude_signature[i] >> 16) - 1; 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; }