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

Add Flex component.

Browse Source 
PiperOrigin-RevId: 572830650
Change-Id: I6908228087b7b9683be3506c8d9cdc725ed5dcd5
This commit is contained in:
Alessio Quaglino
2023-10-12 10:15:46 +01:00
committed by Saran Tunyasuvunakool
parent 649a474788
commit 5a70ad08ab
82 changed files with 9658 additions and 1581 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_io.c
+69 -39
View File
@@ -144,6 +144,11 @@ void mj_defaultOption(mjOption* opt) {
// solver overrides
opt->o_margin = 0;
mj_defaultSolRefImp(opt->o_solref, opt->o_solimp);
opt->o_friction[0] = 1;
opt->o_friction[1] = 1;
opt->o_friction[2] = 0.005;
opt->o_friction[3] = 0.0001;
opt->o_friction[4] = 0.0001;
// discrete options
opt->integrator = mjINT_EULER;
@@ -420,7 +425,7 @@ static int safeAddToBufferSize(intptr_t* offset, size_t* nbuffer,
#if (__has_builtin(__builtin_add_overflow) && __has_builtin(__builtin_mul_overflow)) \
|| (defined(__GNUC__) && __GNUC__ >= 5)
// supported by GCC and Clang
int to_add = 0;
size_t to_add = 0;
if (__builtin_mul_overflow(nc, nr, &to_add)) return 0;
if (__builtin_mul_overflow(to_add, type_size, &to_add)) return 0;
if (__builtin_add_overflow(to_add, SKIP(*offset), &to_add)) return 0;
@@ -438,18 +443,20 @@ static int safeAddToBufferSize(intptr_t* offset, size_t* nbuffer,
// 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, int npaths) {
mjModel* mj_makeModel(
int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic,
int nbvhdynamic, int njnt, int ngeom, int nsite, int ncam, int nlight,
int nflex, int nflexvert, int nflexedge, int nflexelem, int nflexelemdata,
int nflexshelldata, int nflexevpair, int nflextexcoord, 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, int npaths) {
intptr_t offset = 0;
// allocate mjModel
@@ -466,11 +473,21 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int n
m->na = na;
m->nbody = nbody;
m->nbvh = nbvh;
m->nbvhstatic = nbvhstatic;
m->nbvhdynamic = nbvhdynamic;
m->njnt = njnt;
m->ngeom = ngeom;
m->nsite = nsite;
m->ncam = ncam;
m->nlight = nlight;
m->nflex = nflex;
m->nflexvert = nflexvert;
m->nflexedge = nflexedge;
m->nflexelem = nflexelem;
m->nflexelemdata = nflexelemdata;
m->nflexshelldata = nflexshelldata;
m->nflexevpair = nflexevpair;
m->nflextexcoord = nflextexcoord;
m->nmesh = nmesh;
m->nmeshvert = nmeshvert;
m->nmeshnormal = nmeshnormal;
@@ -514,7 +531,7 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int n
m->nuser_sensor = nuser_sensor;
m->nnames = nnames;
m->nnames_map = mjLOAD_MULTIPLE
* (nbody + njnt + ngeom + nsite + ncam + nlight + nmesh
* (nbody + njnt + ngeom + nsite + ncam + nlight + nflex + nmesh
+ nskin + nhfield + ntex + nmat + npair + nexclude + neq
+ ntendon + nu + nsensor + nnumeric + ntext + ntuple
+ nkey + nplugin);
@@ -578,26 +595,28 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int n
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, src->npaths);
dest = mj_makeModel(
src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh,
src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite,
src->ncam, src->nlight, src->nflex, src->nflexvert, src->nflexedge,
src->nflexelem, src->nflexelemdata, src->nflexshelldata,
src->nflexevpair, src->nflextexcoord, 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, src->npaths);
}
if (!dest) {
mjERROR("failed to make mjModel. Invalid sizes.");
@@ -762,7 +781,9 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
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], ints[53]);
ints[49], ints[50], ints[51], ints[52], ints[53], ints[54], ints[55],
ints[56], ints[57], ints[58], ints[59], ints[60], ints[61], ints[62],
ints[63]);
if (!m || m->nbuffer != sizes[getnsize()-1]) {
mju_closeResource(r);
mju_warning("Corrupted model, wrong size parameters");
@@ -1508,12 +1529,6 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
mju_zeroInt(d->solver_nnz, mjNISLAND);
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;
@@ -1751,6 +1766,8 @@ static int numObjects(const mjModel* m, mjtObj objtype) {
return m->ncam;
case mjOBJ_LIGHT:
return m->nlight;
case mjOBJ_FLEX:
return m->nflex;
case mjOBJ_MESH:
return m->nmesh;
case mjOBJ_SKIN:
@@ -1795,6 +1812,8 @@ const char* mj_validateReferences(const mjModel* m) {
// 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
// add flex fields (b/303056369)
#define MJMODEL_REFERENCES \
X(body_parentid, nbody, nbody , 0 ) \
X(body_rootid, nbody, nbody , 0 ) \
@@ -1948,11 +1967,11 @@ const char* mj_validateReferences(const mjModel* m) {
}
}
for (int i=0; i < m->npair; i++) {
int pair_body1 = (m->pair_signature[i] & 0xFFFF) - 1;
int pair_body1 = (m->pair_signature[i] & 0xFFFF);
if (pair_body1 >= m->nbody || pair_body1 < 0) {
return "Invalid model: pair_body1 out of bounds.";
}
int pair_body2 = (m->pair_signature[i] >> 16) - 1;
int pair_body2 = (m->pair_signature[i] >> 16);
if (pair_body2 >= m->nbody || pair_body2 < 0) {
return "Invalid model: pair_body2 out of bounds.";
}
@@ -1991,6 +2010,17 @@ const char* mj_validateReferences(const mjModel* m) {
}
break;
case mjEQ_FLEX:
if (obj1id >= m->nflex || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
// -1 is the value used if second object is omitted
if (obj2id != -1) {
return "Invalid model: eq_obj2id must be -1.";
}
break;
default:
// might occur in case of the now-removed distance equality constraint
mjERROR("unknown equality constraint type.");
@@ -2096,11 +2126,11 @@ const char* mj_validateReferences(const mjModel* m) {
}
}
for (int i=0; i < m->nexclude; i++) {
int exclude_body1 = (m->exclude_signature[i] & 0xFFFF) - 1;
int exclude_body1 = (m->exclude_signature[i] & 0xFFFF);
if (exclude_body1 >= m->nbody || exclude_body1 < 0) {
return "Invalid model: exclude_body1 out of bounds.";
}
int exclude_body2 = (m->exclude_signature[i] >> 16) - 1;
int exclude_body2 = (m->exclude_signature[i] >> 16);
if (exclude_body2 >= m->nbody || exclude_body2 < 0) {
return "Invalid model: exclude_body2 out of bounds.";
}