Kyle Bayes
dfd463cdb5
PiperOrigin-RevId: 517976421 Change-Id: I329c0096d368062f150c6dbeec181f5b0eeefcbf
1864 lines
57 KiB
C
1864 lines
57 KiB
C
// 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 <limits.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#include <mujoco/mjmodel.h>
|
|
#include <mujoco/mjplugin.h>
|
|
#include <mujoco/mjxmacro.h>
|
|
#include "engine/engine_array_safety.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 _MSC_VER
|
|
#pragma warning (disable: 4305) // disable MSVC warning: truncation from 'double' to 'float'
|
|
#endif
|
|
|
|
static const int MAX_ARRAY_SIZE = INT_MAX / 4;
|
|
|
|
//------------------------------ 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;
|
|
}
|
|
|
|
|
|
|
|
//------------------------------- 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.treedepth = 1;
|
|
|
|
// 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 indentify binary mjModel file/buffer
|
|
static const int ID = 54321;
|
|
|
|
|
|
|
|
// count ints in mjModel
|
|
static int getnint(void) {
|
|
int cnt = 0;
|
|
|
|
#define X(name) cnt++;
|
|
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) {
|
|
mju_error("NULL pointer passed to bufwrite");
|
|
}
|
|
|
|
// check size
|
|
if (*ptrbuf+num>szbuf) {
|
|
mju_error("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) {
|
|
mju_error("NULL pointer passed to bufread");
|
|
}
|
|
|
|
// check size
|
|
if (*ptrbuf+num>szbuf) {
|
|
mju_error("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;
|
|
int 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 = (int)(ptr - (char*)m->buffer);
|
|
if (m->nbuffer != sz) {
|
|
printf("expected size: %d, actual size: %d\n", m->nbuffer, sz);
|
|
mju_error("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, int* 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) {
|
|
mju_error("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);
|
|
mju_error("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) {
|
|
mju_error("Failed to make mjModel. Invalid sizes.");
|
|
}
|
|
|
|
// check sizes
|
|
if (dest->nbuffer != src->nbuffer) {
|
|
mj_deleteModel(dest);
|
|
mju_error("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[4] = {ID, sizeof(mjtNum), getnint(), 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), 4, fp);
|
|
fwrite(m, sizeof(int), getnint(), fp);
|
|
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)*4, buffer_sz, buffer, &ptrbuf);
|
|
bufwrite(m, sizeof(int)*getnint(), buffer_sz, buffer, &ptrbuf);
|
|
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 file
|
|
// if vfs is not NULL, look up file in vfs before reading from disk
|
|
mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
|
|
int i, header[4] = {0};
|
|
int expected_header[4] = {ID, sizeof(mjtNum), getnint(), getnptr()};
|
|
int info[2000];
|
|
int ptrbuf = 0;
|
|
mjModel *m = 0;
|
|
FILE* fp = 0;
|
|
|
|
// find file in VFS if given
|
|
const void* buffer = NULL;
|
|
int buffer_sz = 0;
|
|
if (vfs) {
|
|
i = mj_findFileVFS(vfs, filename);
|
|
if (i>=0) {
|
|
buffer_sz = vfs->filesize[i];
|
|
buffer = vfs->filedata[i];
|
|
}
|
|
}
|
|
|
|
// open file for reading if no buffer
|
|
if (!buffer) {
|
|
fp = fopen(filename, "rb");
|
|
if (!fp) {
|
|
mju_warning("Could not open file '%s'", filename);
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
// read header
|
|
if (fp) {
|
|
if (fread(header, 4, sizeof(int), fp) != 4) {
|
|
mju_warning("Model file has an incomplete header");
|
|
return 0;
|
|
}
|
|
} else {
|
|
bufread(header, 4*sizeof(int), buffer_sz, buffer, &ptrbuf);
|
|
}
|
|
|
|
// check header
|
|
for (i=0; i<4; i++) {
|
|
if (header[i]!=expected_header[i]) {
|
|
if (fp) {
|
|
fclose(fp);
|
|
}
|
|
|
|
switch (i) {
|
|
case 0:
|
|
mju_warning("Model missing header ID");
|
|
return 0;
|
|
|
|
case 1:
|
|
mju_warning("Model and executable have different floating point precision");
|
|
return 0;
|
|
|
|
case 2:
|
|
mju_warning("Model and executable have different number of ints in mjModel");
|
|
return 0;
|
|
|
|
default:
|
|
mju_warning("Model and executable have different number of pointers in mjModel");
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
// read mjModel structure: info only
|
|
if (fp) {
|
|
if (fread(info, sizeof(int), getnint(), fp) != getnint()) {
|
|
mju_warning("Model file does not contain enough ints");
|
|
return 0;
|
|
}
|
|
} else {
|
|
bufread(info, sizeof(int)*getnint(), buffer_sz, buffer, &ptrbuf);
|
|
}
|
|
|
|
// allocate new mjModel, check sizes
|
|
m = mj_makeModel(info[0], info[1], info[2], info[3], info[4], info[5], info[6],
|
|
info[7], info[8], info[9], info[10], info[11], info[12], info[13],
|
|
info[14], info[15], info[16], info[17], info[18], info[19], info[20],
|
|
info[21], info[22], info[23], info[24], info[25], info[26], info[27],
|
|
info[28], info[29], info[30], info[31], info[32], info[33], info[34],
|
|
info[35], info[36], info[37], info[38], info[39], info[40], info[41],
|
|
info[42], info[43], info[44], info[45], info[46], info[47], info[48],
|
|
info[49], info[50], info[51], info[52]);
|
|
if (!m || m->nbuffer!=info[getnint()-1]) {
|
|
if (fp) {
|
|
fclose(fp);
|
|
}
|
|
mju_warning("Corrupted model, wrong size parameters");
|
|
mj_deleteModel(m);
|
|
return 0;
|
|
}
|
|
|
|
// set info fields
|
|
memcpy(m, info, sizeof(int)*getnint());
|
|
|
|
// read options and buffer
|
|
if (fp) {
|
|
if (fread((void*)&m->opt, sizeof(mjOption), 1, fp) != 1) {
|
|
mju_warning("Model file does not have a complete mjOption");
|
|
mj_deleteModel(m);
|
|
return 0;
|
|
}
|
|
if (fread((void*)&m->vis, sizeof(mjVisual), 1, fp) != 1) {
|
|
mju_warning("Model file does not have a complete mjVisual");
|
|
mj_deleteModel(m);
|
|
return 0;
|
|
}
|
|
if (fread((void*)&m->stat, sizeof(mjStatistic), 1, fp) != 1) {
|
|
mju_warning("Model file does not have a complete mjStatistic");
|
|
mj_deleteModel(m);
|
|
return 0;
|
|
}
|
|
{
|
|
MJMODEL_POINTERS_PREAMBLE(m)
|
|
#define X(type, name, nr, nc) \
|
|
if (fread(m->name, sizeof(type), (m->nr)*(nc), fp) != (m->nr)*(nc)) { \
|
|
mju_warning("Model file does not contain a large enough buffer"); \
|
|
mj_deleteModel(m); \
|
|
return 0; \
|
|
}
|
|
MJMODEL_POINTERS
|
|
#undef X
|
|
}
|
|
} else {
|
|
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 file size is correct
|
|
if (fp) {
|
|
if (feof(fp)) {
|
|
fclose(fp);
|
|
mju_warning("Model file is too small");
|
|
mj_deleteModel(m);
|
|
return 0;
|
|
}
|
|
char dummy;
|
|
if (fread(&dummy, 1, 1, fp) || !feof(fp)) {
|
|
fclose(fp);
|
|
mju_warning("Model file is too large");
|
|
mj_deleteModel(m);
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
const char* validationError = mj_validateReferences(m);
|
|
if (validationError) {
|
|
mju_warning("%s", validationError);
|
|
mj_deleteModel(m);
|
|
return 0;
|
|
}
|
|
|
|
if (fp) {
|
|
fclose(fp);
|
|
}
|
|
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) {
|
|
return sizeof(int)*(4+getnint()) + sizeof(mjOption) +
|
|
sizeof(mjVisual) + sizeof(mjStatistic) + m->nbuffer;
|
|
}
|
|
|
|
|
|
|
|
|
|
//-------------------------- 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
|
|
memset(rownnz, 0, nv * sizeof(int));
|
|
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
|
|
memcpy(remaining, rownnz, nv * sizeof(int));
|
|
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) {
|
|
mju_error("Error in mj_makeDSparse: 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
|
|
memset(rownnz, 0, sizeof(int) * 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) {
|
|
mju_error("Error in mj_makeBSparse: 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]) {
|
|
mju_error("Error in mj_makeBSparse: sum of rownnz different from nB");
|
|
}
|
|
|
|
// allocate and clear incremental row counts
|
|
mjMARKSTACK;
|
|
int* cnt = mj_stackAllocInt(d, nbody);
|
|
memset(cnt, 0, sizeof(int) * 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]) {
|
|
mju_error("Error in mj_makeBSparse: 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]) {
|
|
mju_error("Error in checkDBSparse: 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]) {
|
|
mju_error("Error in checkDBSparse: 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) {
|
|
mju_error("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) {
|
|
mju_error("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->nstack = m->nstack;
|
|
|
|
// allocate buffer
|
|
d->buffer = mju_malloc(d->nbuffer);
|
|
if (!d->buffer) {
|
|
mju_free(d);
|
|
mju_error("Could not allocate mjData buffer");
|
|
}
|
|
|
|
// allocate arena
|
|
d->arena = mju_malloc(d->nstack * sizeof(mjtNum));
|
|
if (!d->arena) {
|
|
mju_free(d->buffer);
|
|
mju_free(d);
|
|
mju_error("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);
|
|
mju_error("plugin->init failed for plugin id %d", i);
|
|
}
|
|
}
|
|
}
|
|
|
|
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) {
|
|
mju_error("dest and src data buffers have different size");
|
|
}
|
|
if (dest->nstack != src->nstack) {
|
|
mju_error("dest and src stacks have different size");
|
|
}
|
|
|
|
// stack is in use
|
|
if (src->pstack) {
|
|
mju_error("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) {
|
|
mju_error("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
|
|
memcpy(dest->arena, src->arena, src->nstack * sizeof(mjtNum));
|
|
#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);
|
|
}
|
|
}
|
|
|
|
return dest;
|
|
}
|
|
|
|
|
|
|
|
// allocate memory from the mjData arena
|
|
void* mj_arenaAlloc(mjData* d, int bytes, int alignment) {
|
|
int misalignment = d->parena % alignment;
|
|
int padding = misalignment ? alignment - misalignment : 0;
|
|
|
|
// check size
|
|
size_t bytes_available = (d->nstack - d->pstack) * sizeof(mjtNum);
|
|
if (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*sizeof(mjtNum) + d->parena);
|
|
return result;
|
|
}
|
|
|
|
|
|
|
|
// allocate size mjtNums on the mjData stack
|
|
mjtNum* mj_stackAlloc(mjData* d, int size) {
|
|
// return NULL if empty
|
|
if (!size) {
|
|
return 0;
|
|
}
|
|
|
|
// check size
|
|
size_t stack_available_bytes = d->nstack * sizeof(mjtNum) - d->parena;
|
|
size_t stack_required_bytes = (d->pstack + size) * sizeof(mjtNum);
|
|
if (stack_required_bytes > stack_available_bytes) {
|
|
mju_error("stack overflow: max = %zu, available = %zu, requested = %zu "
|
|
"(ne = %d, nf = %d, nefc = %d, ncon = %d)",
|
|
d->nstack * sizeof(mjtNum), stack_available_bytes, stack_required_bytes,
|
|
d->ne, d->nf, d->nefc, d->ncon);
|
|
}
|
|
|
|
// allocate at end of arena
|
|
char* end_ptr = (char*)d->arena + d->nstack * sizeof(mjtNum);
|
|
char* result = end_ptr - (d->pstack + size + 1) * sizeof(mjtNum);
|
|
|
|
#ifdef ADDRESS_SANITIZER
|
|
if ((uintptr_t)result % sizeof(mjtNum)) {
|
|
mju_error("mj_stackAlloc fails to align to sizeof(mjtNum)");
|
|
}
|
|
#endif
|
|
|
|
// update max, return pointer to buffer
|
|
d->pstack += size;
|
|
d->maxuse_stack = mjMAX(d->maxuse_stack, d->pstack);
|
|
d->maxuse_arena = mjMAX(d->maxuse_arena, d->pstack*sizeof(mjtNum) + d->parena);
|
|
return (mjtNum*)result;
|
|
}
|
|
|
|
|
|
|
|
int* mj_stackAllocInt(mjData* d, int size) {
|
|
// optimize for mjtNum being twice the size of int
|
|
if (2*sizeof(int) == sizeof(mjtNum)) {
|
|
return (int*)mj_stackAlloc(d, (size + 1) >> 1);
|
|
}
|
|
|
|
// arbitrary bytes sizes
|
|
int new_size = (sizeof(int)*size + sizeof(mjtNum) - 1) / sizeof(mjtNum);
|
|
return (int*)mj_stackAlloc(d, new_size);
|
|
}
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
// clear arena pointers
|
|
d->parena = 0;
|
|
#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 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 variable sizes
|
|
d->ne = 0;
|
|
d->nf = 0;
|
|
d->nefc = 0;
|
|
d->nnzJ = 0;
|
|
d->ncon = 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) {
|
|
// 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_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_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_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; i<m->nadrs; 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) {
|
|
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 (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:
|
|
mju_error("mj_validateReferences: unknown equality constraint type.");
|
|
}
|
|
}
|
|
for (int i=0; i<m->nwrap; i++) {
|
|
int wrap_objid = m->wrap_objid[i];
|
|
switch (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 (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_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) {
|
|
mju_error("`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;
|
|
}
|