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

Add mjmGeom to C API.

Browse Source 
PiperOrigin-RevId: 605302719
Change-Id: Ib4208e60cf4299473a96f62db2c9dc8c7a5b0b36
This commit is contained in:
Alessio Quaglino
2024-02-08 06:51:44 -08:00
committed by Copybara-Service
parent 3baab3784a
commit 1fe258fb60
14 changed files with 384 additions and 276 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.cc
+2 -2
View File
@@ -77,11 +77,11 @@ void* mjm_addFreeJoint(mjmBody* bodyspec) {
// add geom to body
void* mjm_addGeom(mjmBody* bodyspec, void* defspec) {
mjmGeom* mjm_addGeom(mjmBody* bodyspec, void* defspec) {
mjCDef* def = static_cast<mjCDef*>(defspec);
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjCGeom* geom = body->AddGeom(def);
return geom;
return &geom->spec;
}
src/user/user_api.h
+48 -1
View File
@@ -31,6 +31,13 @@ extern "C" {
//---------------------------------- Public structs ------------------------------------------------
// type of mesh
typedef enum _mjtGeomInertia {
mjINERTIA_VOLUME,
mjINERTIA_SHELL,
} mjtGeomInertia;
typedef struct _mjmOrientation {
double axisangle[4]; // rotation axis and angle
double xyaxes[6]; // x and y axes
@@ -69,6 +76,43 @@ typedef struct _mjmBody {
} mjmBody;
typedef struct _mjmGeom {
mjElement element; // compiler only, do not modify
mjString name; // name
mjString classname; // classname
mjtGeom type; // geom type
double pos[3]; // position
double quat[4]; // orientation
mjmOrientation alt; // alternative orientation specifications
int contype; // contact type
int conaffinity; // contact affinity
int condim; // contact dimensionality
int group; // used for rendering
int priority; // contact priority
double size[3]; // geom-specific size parameters
double friction[3]; // one-sided friction coefficients: slide, roll, spin
double solmix; // solver mixing for contact pairs
mjtNum solref[mjNREF]; // solver reference
mjtNum solimp[mjNIMP]; // solver impedance
double mass; // used to compute density
double density; // used to compute mass and inertia (from volume)
double fromto[6]; // alternative for capsule, cylinder, box, ellipsoid
double margin; // margin for contact detection
double gap; // include in solver if dist<margin-gap
mjtNum fluid_ellipsoid; // whether ellipsoid-fluid model is active
mjtNum fluid_coefs[5]; // ellipsoid-fluid interaction coefs
mjString material; // name of material used for rendering
mjString hfieldname; // hfield attached to geom
mjString meshname; // mesh attached to geom
double fitscale; // scale mesh uniformly
mjDouble userdata; // user data
float rgba[4]; // rgba when material is omitted
mjtGeomInertia typeinertia; // selects between surface and volume inertia
mjmPlugin plugin; // sdf plugin
mjString info;
} mjmGeom;
typedef struct _mjmSite {
mjElement element; // compiler only, do not modify
mjString name; // name
@@ -108,7 +152,7 @@ MJAPI void* mjm_addJoint(mjmBody* body, void* defspec);
MJAPI void* mjm_addFreeJoint(mjmBody* body);
// Add geom to body.
MJAPI void* mjm_addGeom(mjmBody* body, void* defspec);
MJAPI mjmGeom* mjm_addGeom(mjmBody* body, void* defspec);
// Add camera to body.
MJAPI void* mjm_addCamera(mjmBody* body, void* defspec);
@@ -164,6 +208,9 @@ MJAPI const char* mjm_setFullInertia(mjmBody* body, double quat[4], double inert
// Default body attributes.
MJAPI void mjm_defaultBody(mjmBody& body);
// Default geom attributes.
MJAPI void mjm_defaultGeom(mjmGeom& geom);
// Default site attributes.
MJAPI void mjm_defaultSite(mjmSite& site);
src/user/user_composite.cc
+47 -47
View File
@@ -137,7 +137,7 @@ void mjCComposite::SetDefault(void) {
// set all deafult groups to 3
for (int i=0; i<mjNCOMPKINDS; i++) {
def[i].geom.group = 3;
def[i].geom.spec.group = 3;
def[i].site.spec.group = 3;
def[i].tendon.group = 3;
}
@@ -153,7 +153,7 @@ void mjCComposite::SetDefault(void) {
type==mjCOMPTYPE_CABLE ||
(type==mjCOMPTYPE_GRID && tmpdim==1)) {
for (int i=0; i<mjNCOMPKINDS; i++) {
def[i].geom.group = 0;
def[i].geom.spec.group = 0;
def[i].tendon.group = 0;
}
}
@@ -163,8 +163,8 @@ void mjCComposite::SetDefault(void) {
case mjCOMPTYPE_PARTICLE: // particle
// no friction with anything
def[0].geom.condim = 1;
def[0].geom.priority = 1;
def[0].geom.spec.condim = 1;
def[0].geom.spec.priority = 1;
break;
case mjCOMPTYPE_GRID: // grid
@@ -193,7 +193,7 @@ void mjCComposite::SetDefault(void) {
case mjCOMPTYPE_ELLIPSOID:
// no self-collisions
def[0].geom.contype = 0;
def[0].geom.spec.contype = 0;
// soft smoothing
AdjustSoft(solrefsmooth, solimpsmooth, 1);
@@ -219,9 +219,9 @@ void mjCComposite::SetDefault(void) {
// make composite object
bool mjCComposite::Make(mjCModel* model, mjmBody* body, char* error, int error_sz) {
// check geom type
if ((def[0].geom.type!=mjGEOM_SPHERE &&
def[0].geom.type!=mjGEOM_CAPSULE &&
def[0].geom.type!=mjGEOM_ELLIPSOID) &&
if ((def[0].geom.spec.type!=mjGEOM_SPHERE &&
def[0].geom.spec.type!=mjGEOM_CAPSULE &&
def[0].geom.spec.type!=mjGEOM_ELLIPSOID) &&
type!=mjCOMPTYPE_PARTICLE && type!=mjCOMPTYPE_CABLE) {
return comperr(error, "Composite geom type must be sphere, capsule or ellipsoid", error_sz);
}
@@ -240,8 +240,8 @@ bool mjCComposite::Make(mjCModel* model, mjmBody* body, char* error, int error_s
// check spacing
if (type==mjCOMPTYPE_GRID || (type==mjCOMPTYPE_PARTICLE && uservert.empty())) {
if (spacing < mju_max(def[0].geom.size[0],
mju_max(def[0].geom.size[1], def[0].geom.size[2]))) {
if (spacing < mju_max(def[0].geom.spec.size[0],
mju_max(def[0].geom.spec.size[1], def[0].geom.spec.size[2]))) {
return comperr(error, "Spacing must be larger than geometry size",
error_sz);
}
@@ -333,8 +333,8 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjmBody* body, char* error, int
// populate vertices and names
if (uservert.empty()) {
if (spacing < mju_max(def[0].geom.size[0],
mju_max(def[0].geom.size[1], def[0].geom.size[2])))
if (spacing < mju_max(def[0].geom.spec.size[0],
mju_max(def[0].geom.spec.size[1], def[0].geom.spec.size[2])))
return comperr(error, "Spacing must be larger than geometry size", error_sz);
for (int ix=0; ix<count[0]; ix++) {
@@ -475,8 +475,8 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjmBody* body, char* error, int
}
// add geom
mjCGeom* g = (mjCGeom*)mjm_addGeom(b, def);
g->def = (mjCDef*)mjm_getDefault(body->element);
mjmGeom* g = mjm_addGeom(b, def);
mjm_setDefault(g->element, mjm_getDefault(body->element));
// add site
mjmSite* s = mjm_addSite(b, def);
@@ -600,11 +600,11 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjmBody* body, char* error, int err
b->pos[2] = offset[2];
// add geom
mjCGeom* g = (mjCGeom*)mjm_addGeom(b, def);
g->def = (mjCDef*)mjm_getDefault(body->element);
mjmGeom* g = mjm_addGeom(b, def);
mjm_setDefault(g->element, mjm_getDefault(body->element));
g->type = mjGEOM_SPHERE;
mju::sprintf_arr(txt, "%sG%d_%d", prefix.c_str(), ix, iy);
g->name = txt;
mjm_setString(g->name, txt);
// add site
mjmSite* s = mjm_addSite(b, def);
@@ -691,9 +691,9 @@ bool mjCComposite::MakeCable(mjCModel* model, mjmBody* body, char* error, int er
}
// check geom type
if (def[0].geom.type!=mjGEOM_CYLINDER &&
def[0].geom.type!=mjGEOM_CAPSULE &&
def[0].geom.type!=mjGEOM_BOX) {
if (def[0].geom.spec.type!=mjGEOM_CYLINDER &&
def[0].geom.spec.type!=mjGEOM_CAPSULE &&
def[0].geom.spec.type!=mjGEOM_BOX) {
return comperr(error, "Cable geom type must be sphere, capsule or box", error_sz);
}
@@ -739,14 +739,14 @@ bool mjCComposite::MakeCable(mjCModel* model, mjmBody* body, char* error, int er
}
// add skin
if (def[0].geom.type==mjGEOM_BOX) {
if (def[0].geom.spec.type==mjGEOM_BOX) {
if (skinsubgrid>0) {
count[1]+=2;
MakeSkin2Subgrid(model, 2*def[0].geom.size[2]);
MakeSkin2Subgrid(model, 2*def[0].geom.spec.size[2]);
count[1]-=2;
} else {
count[1]++;
MakeSkin2(model, 2*def[0].geom.size[2]);
MakeSkin2(model, 2*def[0].geom.spec.size[2]);
count[1]--;
}
}
@@ -826,14 +826,14 @@ mjmBody* mjCComposite::AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtN
}
// add geom
mjCGeom* geom = (mjCGeom*)mjm_addGeom(body, def);
geom->def = (mjCDef*)mjm_getDefault(body->element);
geom->name = txt_geom;
if (def[0].geom.type==mjGEOM_CYLINDER ||
def[0].geom.type==mjGEOM_CAPSULE) {
mjmGeom* geom = mjm_addGeom(body, def);
mjm_setDefault(geom->element, mjm_getDefault(body->element));
mjm_setString(geom->name, txt_geom);
if (def[0].geom.spec.type==mjGEOM_CYLINDER ||
def[0].geom.spec.type==mjGEOM_CAPSULE) {
mjuu_zerovec(geom->fromto, 6);
geom->fromto[3] = length;
} else if (def[0].geom.type==mjGEOM_BOX) {
} else if (def[0].geom.spec.type==mjGEOM_BOX) {
mjuu_zerovec(geom->pos, 3);
geom->pos[0] = length/2;
geom->size[0] = length/2;
@@ -982,10 +982,10 @@ mjmBody* mjCComposite::AddRopeBody(mjCModel* model, mjmBody* body, int ix, int i
}
// add geom
mjCGeom* geom = (mjCGeom*)mjm_addGeom(body, def);
geom->def = (mjCDef*)mjm_getDefault(body->element);
mjmGeom* geom = mjm_addGeom(body, def);
mjm_setDefault(geom->element, mjm_getDefault(body->element));
mju::sprintf_arr(txt, "%sG%d", prefix.c_str(), ix1);
geom->name = txt;
mjm_setString(geom->name, txt);
mjuu_setvec(geom->pos, 0, 0, 0);
mjuu_setvec(geom->quat, sqrt(0.5), 0, sqrt(0.5), 0);
@@ -1094,11 +1094,11 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro
}
// center geom: two times bigger
mjCGeom* geom = (mjCGeom*)mjm_addGeom(body, def);
geom->def = (mjCDef*)mjm_getDefault(body->element);
mjmGeom* geom = mjm_addGeom(body, def);
mjm_setDefault(geom->element, mjm_getDefault(body->element));
geom->type = mjGEOM_SPHERE;
mju::sprintf_arr(txt, "%sGcenter", prefix.c_str());
geom->name = txt;
mjm_setString(geom->name, txt);
mjuu_setvec(geom->pos, 0, 0, 0);
geom->size[0] *= 2;
geom->size[1] = 0;
@@ -1135,10 +1135,10 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro
mjuu_normvec(b->alt.zaxis, 3);
// add geom
mjCGeom* g = (mjCGeom*) mjm_addGeom(b, def);
g->def = (mjCDef*)mjm_getDefault(body->element);
mjmGeom* g = mjm_addGeom(b, def);
mjm_setDefault(g->element, mjm_getDefault(body->element));
mju::sprintf_arr(txt, "%sG%d_%d_%d", prefix.c_str(), ix, iy, iz);
g->name = txt;
mjm_setString(g->name, txt);
// offset inwards, enforce sphere or capsule
if (g->type==mjGEOM_CAPSULE) {
@@ -1469,15 +1469,15 @@ void mjCComposite::MakeCableBones(mjCModel* model, mjCSkin* skin) {
// bind pose
if (iy==0) {
skin->bodyname.push_back(this_body);
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(-def[0].geom.size[1]);
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.spec.size[0] : 0);
skin->bindpos.push_back(-def[0].geom.spec.size[1]);
skin->bindpos.push_back(0);
skin->bindquat.push_back(1); skin->bindquat.push_back(0);
skin->bindquat.push_back(0); skin->bindquat.push_back(0);
} else {
skin->bodyname.push_back(this_body);
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(def[0].geom.size[1]);
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.spec.size[0] : 0);
skin->bindpos.push_back(def[0].geom.spec.size[1]);
skin->bindpos.push_back(0);
skin->bindquat.push_back(1); skin->bindquat.push_back(0);
skin->bindquat.push_back(0); skin->bindquat.push_back(0);
@@ -1509,15 +1509,15 @@ void mjCComposite::MakeCableBonesSubgrid(mjCModel* model, mjCSkin* skin) {
// bind pose
if (iy==0) {
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(-def[0].geom.size[1]);
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.spec.size[0] : 0);
skin->bindpos.push_back(-def[0].geom.spec.size[1]);
skin->bindpos.push_back(0);
} else if (iy==2) {
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back(def[0].geom.size[1]);
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.spec.size[0] : 0);
skin->bindpos.push_back(def[0].geom.spec.size[1]);
skin->bindpos.push_back(0);
} else {
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.size[0] : 0);
skin->bindpos.push_back((ix==count[0]-1) ? -2*def[0].geom.spec.size[0] : 0);
skin->bindpos.push_back(0);
skin->bindpos.push_back(0);
}
src/user/user_init.cc
+35 -4
View File
@@ -19,14 +19,14 @@
#include "user/user_util.h"
// Default body parameters
// default body attributes
void mjm_defaultBody(mjmBody& body) {
memset(&body, 0, sizeof(mjmBody));
// set non-zero defaults
body.fullinertia[0] = mjNAN;
body.explicitinertial = (mjtByte)false;
body.mocap = (mjtByte)false;
body.explicitinertial = 0;
body.mocap = 0;
body.quat[0] = 1;
body.iquat[0] = 1;
body.pos[0] = mjNAN;
@@ -38,8 +38,39 @@ void mjm_defaultBody(mjmBody& body) {
}
// default geom attributes
MJAPI void mjm_defaultGeom(mjmGeom& geom) {
memset(&geom, 0, sizeof(mjmGeom));
// Default site parameters
// set non-zero defaults
geom.fromto[0] = mjNAN;
geom.mass = mjNAN;
geom.type = mjGEOM_SPHERE;
geom.contype = 1;
geom.conaffinity = 1;
geom.condim = 3;
geom.friction[0] = 1;
geom.friction[1] = 0.005;
geom.friction[2] = 0.0001;
geom.solmix = 1.0;
mj_defaultSolRefImp(geom.solref, geom.solimp);
geom.density = 1000; // water density (1000 kg / m^3)
geom.fitscale = 1;
geom.rgba[0] = geom.rgba[1] = geom.rgba[2] = 0.5f;
geom.rgba[3] = 1.0f;
geom.typeinertia = mjINERTIA_VOLUME;
geom.quat[0] = 1;
geom.alt.axisangle[0] = geom.alt.xyaxes[0] = geom.alt.zaxis[0] = geom.alt.euler[0] = mjNAN;
geom.fluid_coefs[0] = 0.5; // blunt drag coefficient
geom.fluid_coefs[1] = 0.25; // slender drag coefficient
geom.fluid_coefs[2] = 1.5; // angular drag coefficient
geom.fluid_coefs[3] = 1.0; // kutta lift coefficient
geom.fluid_coefs[4] = 1.0; // magnus lift coefficient
}
// default site attributes
void mjm_defaultSite(mjmSite& site) {
memset(&site, 0, sizeof(mjmSite));
src/user/user_mesh.cc
+22 -22
View File
@@ -647,8 +647,8 @@ void mjCMesh::SetBoundingVolume(int faceid) {
// get position
double* mjCMesh::GetPosPtr(mjtMeshType type) {
if (type==mjSHELL_MESH) {
double* mjCMesh::GetPosPtr(mjtGeomInertia type) {
if (type==mjINERTIA_SHELL) {
return pos_surface_;
} else {
return pos_volume_;
@@ -658,8 +658,8 @@ double* mjCMesh::GetPosPtr(mjtMeshType type) {
// get orientation
double* mjCMesh::GetQuatPtr(mjtMeshType type) {
if (type==mjSHELL_MESH) {
double* mjCMesh::GetQuatPtr(mjtGeomInertia type) {
if (type==mjINERTIA_SHELL) {
return quat_surface_;
} else {
return quat_volume_;
@@ -1162,7 +1162,7 @@ void mjCMesh::LoadMSH(mjResource* resource) {
}
void mjCMesh::ComputeVolume(double CoM[3], mjtMeshType type,
void mjCMesh::ComputeVolume(double CoM[3], mjtGeomInertia type,
const double facecen[3], bool exactmeshinertia) {
double nrm[3];
double cen[3];
@@ -1174,10 +1174,10 @@ void mjCMesh::ComputeVolume(double CoM[3], mjtMeshType type,
// compute and add volume
const double vec[3] = {cen[0]-facecen[0], cen[1]-facecen[1], cen[2]-facecen[2]};
double vol = type==mjSHELL_MESH ? a : mjuu_dot3(vec, nrm) * a / 3;
double vol = type==mjINERTIA_SHELL ? a : mjuu_dot3(vec, nrm) * a / 3;
// if legacy computation requested, then always positive
if (!exactmeshinertia && type==mjVOLUME_MESH) {
if (!exactmeshinertia && type==mjINERTIA_VOLUME) {
vol = fabs(vol);
}
@@ -1316,7 +1316,7 @@ void mjCMesh::Process() {
ComputeFaceCentroid(facecen);
// compute inertial properties for both inertia types
for ( const auto type : { mjtMeshType::mjVOLUME_MESH, mjtMeshType::mjSHELL_MESH } ) {
for ( const auto type : { mjtGeomInertia::mjINERTIA_VOLUME, mjtGeomInertia::mjINERTIA_SHELL } ) {
double CoM[3] = {0, 0, 0};
double inert[6] = {0, 0, 0, 0, 0, 0};
bool exactmeshinertia = model->exactmeshinertia;
@@ -1344,7 +1344,7 @@ void mjCMesh::Process() {
mjuu_copyvec(GetPosPtr(type), CoM, 3);
// re-center mesh at CoM
if (type==mjVOLUME_MESH || validvolume_<=0) {
if (type==mjINERTIA_VOLUME || validvolume_<=0) {
for (int i=0; i<nvert_; i++) {
for (int j=0; j<3; j++) {
vert_[3*i+j] -= CoM[j];
@@ -1363,10 +1363,10 @@ void mjCMesh::Process() {
// get area, normal and center; update volume
double a = _triangle(nrm, cen, D, E, F);
double vol = type==mjSHELL_MESH ? a : mjuu_dot3(cen, nrm) * a / 3;
double vol = type==mjINERTIA_SHELL ? a : mjuu_dot3(cen, nrm) * a / 3;
// if legacy computation requested, then always positive
if (!exactmeshinertia && type==mjVOLUME_MESH) {
if (!exactmeshinertia && type==mjINERTIA_VOLUME) {
vol = fabs(vol);
}
@@ -1374,7 +1374,7 @@ void mjCMesh::Process() {
GetVolumeRef(type) += vol;
for (int j=0; j<6; j++) {
P[j] += def->geom.density*vol /
(type==mjSHELL_MESH ? 12 : 20) * (
(type==mjINERTIA_SHELL ? 12 : 20) * (
2*(D[k[j][0]] * D[k[j][1]] +
E[k[j][0]] * E[k[j][1]] +
F[k[j][0]] * F[k[j][1]]) +
@@ -1422,8 +1422,8 @@ void mjCMesh::Process() {
// if volume was valid, copy volume quat to shell and stop,
// otherwise use shell quat for coordinate transformations
if (type==mjSHELL_MESH && validvolume_>0) {
mju_copy4(GetQuatPtr(type), GetQuatPtr(mjVOLUME_MESH));
if (type==mjINERTIA_SHELL && validvolume_>0) {
mju_copy4(GetQuatPtr(type), GetQuatPtr(mjINERTIA_VOLUME));
continue;
}
@@ -1459,7 +1459,7 @@ void mjCMesh::Process() {
// check that the mesh is valid
void mjCMesh::CheckMesh(mjtMeshType type) {
void mjCMesh::CheckMesh(mjtGeomInertia type) {
if (!processed_) {
return;
}
@@ -1468,11 +1468,11 @@ void mjCMesh::CheckMesh(mjtMeshType type) {
"faces of mesh '%s' have inconsistent orientation. Please check the "
"faces containing the vertices %d and %d.",
name.c_str(), invalidorientation_.first, invalidorientation_.second);
if (!validarea_ && type==mjSHELL_MESH)
if (!validarea_ && type==mjINERTIA_SHELL)
throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
if (validvolume_<0 && type==mjVOLUME_MESH)
if (validvolume_<0 && type==mjINERTIA_VOLUME)
throw mjCError(this, "mesh volume is negative (misoriented triangles): %s", name.c_str());
if (!validvolume_ && type==mjVOLUME_MESH)
if (!validvolume_ && type==mjINERTIA_VOLUME)
throw mjCError(this, "mesh volume is too small: %s", name.c_str());
if (!valideigenvalue_)
throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
@@ -1482,15 +1482,15 @@ void mjCMesh::CheckMesh(mjtMeshType type) {
// get inertia pointer
double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) {
double* mjCMesh::GetInertiaBoxPtr(mjtGeomInertia type) {
CheckMesh(type);
return type==mjSHELL_MESH ? boxsz_surface_ : boxsz_volume_;
return type==mjINERTIA_SHELL ? boxsz_surface_ : boxsz_volume_;
}
double& mjCMesh::GetVolumeRef(mjtMeshType type) {
double& mjCMesh::GetVolumeRef(mjtGeomInertia type) {
CheckMesh(type);
return type==mjSHELL_MESH ? surface_ : volume_;
return type==mjINERTIA_SHELL ? surface_ : volume_;
}
src/user/user_model.cc
+17 -16
View File
@@ -565,6 +565,7 @@ mjCDef* mjCModel::AddDef(string name, int parentid) {
if (parentid>=0 && parentid<thisid) {
*def = *defaults[parentid];
defaults[parentid]->childid.push_back(thisid);
def->MakePointerLocal();
}
def->parentid = parentid;
def->name = name;
@@ -654,8 +655,8 @@ void mjCModel::MakeLists(mjCBody* body) {
template <class T>
void mjCModel::DeleteMaterial(std::vector<T*>& list, std::string_view name) {
for (T* plist : list) {
if (name.empty() || plist->material_ == name) {
plist->material_.clear();
if (name.empty() || plist->get_material() == name) {
plist->del_material();
}
}
}
@@ -776,18 +777,18 @@ void mjCModel::IndexAssets(bool discard) {
mjCGeom* pgeom = geoms[i];
// find material by name
if (!pgeom->material_.empty()) {
mjCBase* m = FindObject(mjOBJ_MATERIAL, pgeom->material_);
if (!pgeom->get_material().empty()) {
mjCBase* m = FindObject(mjOBJ_MATERIAL, pgeom->get_material());
if (m) {
pgeom->matid = m->id;
} else {
throw mjCError(pgeom, "material '%s' not found in geom %d", pgeom->material_.c_str(), i);
throw mjCError(pgeom, "material '%s' not found in geom %d", pgeom->get_material().c_str(), i);
}
}
// find mesh by name
if (!pgeom->meshname.empty()) {
mjCBase* m = FindObject(mjOBJ_MESH, pgeom->meshname);
if (!pgeom->get_meshname().empty()) {
mjCBase* m = FindObject(mjOBJ_MESH, pgeom->get_meshname());
if (m) {
if (discard && geoms[i]->visual_) {
// do not associate with a mesh
@@ -803,17 +804,17 @@ void mjCModel::IndexAssets(bool discard) {
}
}
} else {
throw mjCError(pgeom, "mesh '%s' not found in geom %d", pgeom->meshname.c_str(), i);
throw mjCError(pgeom, "mesh '%s' not found in geom %d", pgeom->get_meshname().c_str(), i);
}
}
// find hfield by name
if (!pgeom->hfieldname.empty()) {
mjCBase* m = FindObject(mjOBJ_HFIELD, pgeom->hfieldname);
if (!pgeom->get_hfieldname().empty()) {
mjCBase* m = FindObject(mjOBJ_HFIELD, pgeom->get_hfieldname());
if (m) {
pgeom->hfield = (mjCHField*)m;
} else {
throw mjCError(pgeom, "hfield '%s' not found in geom %d", pgeom->hfieldname.c_str(), i);
throw mjCError(pgeom, "hfield '%s' not found in geom %d", pgeom->get_hfieldname().c_str(), i);
}
}
}
@@ -1683,7 +1684,7 @@ void mjCModel::CopyTree(mjModel* m) {
m->geom_margin[gid] = (mjtNum)pg->margin;
m->geom_gap[gid] = (mjtNum)pg->gap;
copyvec(m->geom_fluid+mjNFLUID*gid, pg->fluid, mjNFLUID);
copyvec(m->geom_user+nuser_geom*gid, pg->userdata.data(), nuser_geom);
copyvec(m->geom_user+nuser_geom*gid, pg->get_userdata().data(), nuser_geom);
copyvec(m->geom_rgba+4*gid, pg->rgba, 4);
// determine sameframe
@@ -2868,8 +2869,8 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
// mark meshes that need convex hull
for (int i=0; i<geoms.size(); i++) {
if (geoms[i]->mesh && geoms[i]->type==mjGEOM_MESH &&
(geoms[i]->contype || geoms[i]->conaffinity)) {
if (geoms[i]->mesh && geoms[i]->spec.type==mjGEOM_MESH &&
(geoms[i]->spec.contype || geoms[i]->spec.conaffinity)) {
geoms[i]->mesh->set_needhull(true);
}
}
@@ -2895,7 +2896,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
if (nuser_geom == -1) {
nuser_geom = 0;
for (int i=0; i<geoms.size(); i++) {
nuser_geom = mjMAX(nuser_geom, geoms[i]->userdata.size());
nuser_geom = mjMAX(nuser_geom, geoms[i]->spec_userdata_.size());
}
}
if (nuser_site == -1) {
@@ -3369,7 +3370,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
pg->gap = (double)m->geom_gap[i];
if (nuser_geom) {
copyvec(pg->userdata.data(), m->geom_user + nuser_geom*i, nuser_geom);
copyvec(pg->userdata_.data(), m->geom_user + nuser_geom*i, nuser_geom);
}
}
src/user/user_objects.cc
+101 -80
View File
@@ -471,7 +471,7 @@ mjCDef::mjCDef(void) {
void mjCDef::Compile(const mjCModel* model) {
// enforce length of all default userdata arrays
joint.userdata.resize(model->nuser_jnt);
geom.userdata.resize(model->nuser_geom);
geom.userdata_.resize(model->nuser_geom);
site.userdata_.resize(model->nuser_site);
camera.userdata.resize(model->nuser_cam);
tendon.userdata.resize(model->nuser_tendon);
@@ -480,6 +480,13 @@ void mjCDef::Compile(const mjCModel* model) {
// assignment operator (TODO: use overloading)
void mjCDef::MakePointerLocal() {
geom.MakePointerLocal();
}
//------------------------- class mjCBase implementation -------------------------------------------
// constructor
@@ -867,7 +874,7 @@ void mjCBody::GeomFrame(void) {
if (sz==1) {
mjuu_copyvec(ipos, sel[0]->pos, 3);
mjuu_copyvec(iquat, sel[0]->quat, 4);
mass = sel[0]->mass;
mass = sel[0]->mass_;
mjuu_copyvec(inertia, sel[0]->inertia, 3);
}
@@ -876,10 +883,10 @@ void mjCBody::GeomFrame(void) {
// compute total mass and center of mass
mass = 0;
for (int i=0; i<sz; i++) {
mass += sel[i]->mass;
com[0] += sel[i]->mass * sel[i]->pos[0];
com[1] += sel[i]->mass * sel[i]->pos[1];
com[2] += sel[i]->mass * sel[i]->pos[2];
mass += sel[i]->mass_;
com[0] += sel[i]->mass_ * sel[i]->pos[0];
com[1] += sel[i]->mass_ * sel[i]->pos[1];
com[2] += sel[i]->mass_ * sel[i]->pos[2];
}
// check for small mass
@@ -902,7 +909,7 @@ void mjCBody::GeomFrame(void) {
};
mjuu_globalinertia(inert0, sel[i]->inertia, sel[i]->quat);
mjuu_offcenter(inert1, sel[i]->mass, dpos);
mjuu_offcenter(inert1, sel[i]->mass_, dpos);
for (int j=0; j<6; j++) {
toti[j] = toti[j] + inert0[j] + inert1[j];
}
@@ -1003,8 +1010,8 @@ void mjCBody::Compile(void) {
for (int i=0; i<geoms.size(); i++) {
geoms[i]->inferinertia = id>0 &&
(!explicitinertial || model->inertiafromgeom == mjINERTIAFROMGEOM_TRUE) &&
geoms[i]->group >= model->inertiagrouprange[0] &&
geoms[i]->group <= model->inertiagrouprange[1];
geoms[i]->spec.group >= model->inertiagrouprange[0] &&
geoms[i]->spec.group <= model->inertiagrouprange[1];
geoms[i]->Compile();
}
@@ -1367,57 +1374,29 @@ int mjCJoint::Compile(void) {
// initialize default geom
mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) {
// clear alternatives
fromto[0] = mjNAN;
_mass = mjNAN;
mjm_defaultGeom(spec);
// set defaults
type = mjGEOM_SPHERE;
mjuu_setvec(size, 0, 0, 0);
contype = 1;
conaffinity = 1;
condim = 3;
group = 0;
priority = 0;
mjuu_setvec(friction, 1, 0.005, 0.0001);
solmix = 1.0;
mj_defaultSolRefImp(solref, solimp);
margin = 0;
gap = 0;
fluid_switch = 0.0;
// user-tunable ellipsoid-fluid interaction coefs sorted from most to least likely to need tuning
// defaults are tuned for slender bodies in a Re 50-1000 environment
fluid_coefs[0] = 0.5; // blunt_drag_coef
fluid_coefs[1] = 0.25; // slender_drag_coef
fluid_coefs[2] = 1.5; // ang_drag_coef
fluid_coefs[3] = 1.0; // kutta_lift_coef
fluid_coefs[4] = 1.0; // magnus_lift_coef
for (int i = 0; i < mjNFLUID; i++){
fluid[i] = 0;
}
density = 1000; // water density (1000 kg / m^3)
meshname.clear();
fitscale = 1;
material_.clear();
rgba[0] = rgba[1] = rgba[2] = 0.5f;
rgba[3] = 1.0f;
userdata.clear();
typeinertia = mjVOLUME_MESH;
inferinertia = true;
// clear internal variables
mjuu_setvec(quat, 1, 0, 0, 0);
mjuu_setvec(pos, 0, 0, 0);
mass = 0;
mjuu_setvec(inertia, 0, 0, 0);
mass_ = 0;
body = 0;
matid = -1;
mesh = nullptr;
hfield = nullptr;
visual_ = false;
mjuu_setvec(inertia, 0, 0, 0);
inferinertia = true;
spec_material_.clear();
spec_userdata_.clear();
spec_meshname_.clear();
spec_hfieldname_.clear();
spec_userdata_.clear();
for (int i = 0; i < mjNFLUID; i++){
fluid[i] = 0;
}
// reset to default if given
if (_def) {
_def->geom.CopyFromSpec();
*this = _def->geom;
}
@@ -1426,8 +1405,48 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) {
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
// point to local (needs to be after defaults)
plugin.name = (mjString)&plugin_name;
plugin.instance_name = (mjString)&plugin_instance_name;
MakePointerLocal();
// in case this geom is not compiled
CopyFromSpec();
}
// to be called after any default copy constructor
void mjCGeom::MakePointerLocal(void) {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.info = (mjString)&info;
spec.classname = (mjString)&classname;
spec.userdata = (mjDouble)&spec_userdata_;
spec.material = (mjString)&spec_material_;
spec.meshname = (mjString)&spec_meshname_;
spec.hfieldname = (mjString)&spec_hfieldname_;
spec.plugin.name = (mjString)&plugin_name;
spec.plugin.instance_name = (mjString)&plugin_instance_name;
}
void mjCGeom::CopyFromSpec() {
*static_cast<mjmGeom*>(this) = spec;
userdata_ = spec_userdata_;
hfieldname_ = spec_hfieldname_;
meshname_ = spec_meshname_;
material_ = spec_material_;
userdata = (mjDouble)&userdata_;
hfieldname = (mjString)&hfieldname_;
meshname = (mjString)&meshname_;
material = (mjString)&material_;
mju_copy4(alt_.axisangle, alt.axisangle);
mju_copy(alt_.xyaxes, alt.xyaxes, 6);
mju_copy3(alt_.zaxis, alt.zaxis);
mju_copy3(alt_.euler, alt.euler);
plugin.active = spec.plugin.active;
plugin.instance = spec.plugin.instance;
plugin.name = spec.plugin.name;
plugin.instance_name = spec.plugin.instance_name;
}
@@ -1496,9 +1515,9 @@ void mjCGeom::SetInertia(void) {
}
double* boxsz = mesh->GetInertiaBoxPtr(typeinertia);
inertia[0] = mass*(boxsz[1]*boxsz[1] + boxsz[2]*boxsz[2]) / 3;
inertia[1] = mass*(boxsz[0]*boxsz[0] + boxsz[2]*boxsz[2]) / 3;
inertia[2] = mass*(boxsz[0]*boxsz[0] + boxsz[1]*boxsz[1]) / 3;
inertia[0] = mass_*(boxsz[1]*boxsz[1] + boxsz[2]*boxsz[2]) / 3;
inertia[1] = mass_*(boxsz[0]*boxsz[0] + boxsz[2]*boxsz[2]) / 3;
inertia[2] = mass_*(boxsz[0]*boxsz[0] + boxsz[1]*boxsz[1]) / 3;
}
// compute from geom shape
@@ -1508,14 +1527,14 @@ void mjCGeom::SetInertia(void) {
name.c_str(), id);
switch (type) {
case mjGEOM_SPHERE:
inertia[0] = inertia[1] = inertia[2] = 2*mass*size[0]*size[0]/5;
inertia[0] = inertia[1] = inertia[2] = 2*mass_*size[0]*size[0]/5;
return;
case mjGEOM_CAPSULE: {
height = 2*size[1];
double radius = size[0];
double sphere_mass = mass*4*radius/(4*radius + 3*height); // mass*(sphere_vol/total_vol)
double cylinder_mass = mass - sphere_mass;
double sphere_mass = mass_*4*radius/(4*radius + 3*height); // mass*(sphere_vol/total_vol)
double cylinder_mass = mass_ - sphere_mass;
// cylinder part
inertia[0] = inertia[1] = cylinder_mass*(3*radius*radius + height*height)/12;
inertia[2] = cylinder_mass*radius*radius/2;
@@ -1529,21 +1548,21 @@ void mjCGeom::SetInertia(void) {
case mjGEOM_CYLINDER:
height = 2*size[1];
inertia[0] = inertia[1] = mass*(3*size[0]*size[0]+height*height)/12;
inertia[2] = mass*size[0]*size[0]/2;
inertia[0] = inertia[1] = mass_*(3*size[0]*size[0]+height*height)/12;
inertia[2] = mass_*size[0]*size[0]/2;
return;
case mjGEOM_ELLIPSOID:
inertia[0] = mass*(size[1]*size[1]+size[2]*size[2])/5;
inertia[1] = mass*(size[0]*size[0]+size[2]*size[2])/5;
inertia[2] = mass*(size[0]*size[0]+size[1]*size[1])/5;
inertia[0] = mass_*(size[1]*size[1]+size[2]*size[2])/5;
inertia[1] = mass_*(size[0]*size[0]+size[2]*size[2])/5;
inertia[2] = mass_*(size[0]*size[0]+size[1]*size[1])/5;
return;
case mjGEOM_HFIELD:
case mjGEOM_BOX:
inertia[0] = mass*(size[1]*size[1]+size[2]*size[2])/3;
inertia[1] = mass*(size[0]*size[0]+size[2]*size[2])/3;
inertia[2] = mass*(size[0]*size[0]+size[1]*size[1])/3;
inertia[0] = mass_*(size[1]*size[1]+size[2]*size[2])/3;
inertia[1] = mass_*(size[0]*size[0]+size[2]*size[2])/3;
inertia[2] = mass_*(size[0]*size[0]+size[1]*size[1])/3;
return;
default:
@@ -1700,7 +1719,7 @@ void mjCGeom::SetFluidCoefs(void) {
volume * kz / std::max(mjMINVAL, 2-kz)};
const mjtNum virtual_inertia[3] = {volume*Ixfac/5, volume*Iyfac/5, volume*Izfac/5};
writeFluidGeomInteraction(fluid, &fluid_switch, &fluid_coefs[0],
writeFluidGeomInteraction(fluid, &fluid_ellipsoid, &fluid_coefs[0],
&fluid_coefs[1], &fluid_coefs[2],
&fluid_coefs[3], &fluid_coefs[4],
virtual_mass, virtual_inertia);
@@ -1767,12 +1786,14 @@ void mjCGeom::ComputeAABB(void) {
// compiler
void mjCGeom::Compile(void) {
CopyFromSpec();
// resize userdata
if (userdata.size() > model->nuser_geom) {
if (userdata_.size() > model->nuser_geom) {
throw mjCError(this, "user has more values than nuser_geom in geom '%s' (id = %d)",
name.c_str(), id);
}
userdata.resize(model->nuser_geom);
userdata_.resize(model->nuser_geom);
// check type
if (type<0 || type>=mjNGEOMTYPES) {
@@ -1853,7 +1874,7 @@ void mjCGeom::Compile(void) {
// not 'fromto': try alternative
else {
const char* err = alt.Set(quat, model->degree, model->euler);
const char* err = alt_.Set(quat, model->degree, model->euler);
if (err) {
throw mjCError(this, "orientation specification error '%s' in geom %d", err, id);
}
@@ -1875,7 +1896,7 @@ void mjCGeom::Compile(void) {
mesh->FitGeom(this, meshpos);
// remove reference to mesh
meshname.clear();
meshname_.clear();
mesh = nullptr;
} else {
mjuu_copyvec(meshpos, mesh->GetPosPtr(typeinertia), 3);
@@ -1913,28 +1934,28 @@ void mjCGeom::Compile(void) {
// compute geom mass and inertia
if (inferinertia) {
if (mjuu_defined(_mass)) {
if (_mass==0) {
mass = 0;
if (mjuu_defined(mass)) {
if (mass==0) {
mass_ = 0;
density = 0;
} else if (GetVolume()>mjMINVAL) {
mass = _mass;
density = _mass / GetVolume();
mass_ = mass;
density = mass / GetVolume();
SetInertia();
}
} else {
mass = density * GetVolume();
mass_ = density * GetVolume();
SetInertia();
}
// check for negative values
if (mass<0 || inertia[0]<0 || inertia[1]<0 || inertia[2]<0 || density<0)
if (mass_<0 || inertia[0]<0 || inertia[1]<0 || inertia[2]<0 || density<0)
throw mjCError(this, "mass, inertia or density are negative in geom '%s' (id = %d)",
name.c_str(), id);
}
// fluid-interaction coefficients, requires computed inertia and mass
if (fluid_switch > 0) {
if (fluid_ellipsoid > 0) {
SetFluidCoefs();
}
src/user/user_objects.h
+41 -52
View File
@@ -86,13 +86,6 @@ typedef enum _mjtMark {
} mjtMark;
// type of mesh
typedef enum _mjtMeshType {
mjVOLUME_MESH,
mjSHELL_MESH,
} mjtMeshType;
// error information
class [[nodiscard]] mjCError {
public:
@@ -378,23 +371,25 @@ class mjCJoint : public mjCBase {
//------------------------- class mjCGeom ----------------------------------------------------------
// Describes a geometric shape belonging to a body
class mjCGeom : public mjCBase {
class mjCGeom : public mjCBase, private mjmGeom {
friend class mjCDef;
friend class mjCMesh;
friend class mjCPair;
friend class mjCBody;
friend class mjCModel;
friend class mjCWrap;
friend class mjXWriter;
friend class mjXURDF;
public:
using mjCBase::name;
mjmGeom spec; // variables set by user
double GetVolume(void); // compute geom volume
void SetInertia(void); // compute and set geom inertia
bool IsVisual(void) const { return visual_; }
void SetNotVisual(void) { visual_ = false; }
void set_material(std::string _material) { material_ = _material; }
std::string& get_material() { return material_; }
bool inferinertia; // true if inertia should be computed from geom
// Compute all coefs modeling the interaction with the surrounding fluid.
void SetFluidCoefs(void);
@@ -404,56 +399,46 @@ class mjCGeom : public mjCBase {
// sets properties of a bounding volume
void SetBoundingVolume(mjCBoundingVolume* bv) const;
// variables set by user and copied into mjModel
mjtGeom type; // geom type
int contype; // contact type
int conaffinity; // contact affinity
int condim; // contact dimensionality
int group; // used for rendering
int priority; // contact priority
double size[3]; // geom-specific size parameters
double friction[3]; // one-sided friction coefficients: slide, roll, spin
double solmix; // solver mixing for contact pairs
mjtNum solref[mjNREF]; // solver reference
mjtNum solimp[mjNIMP]; // solver impedance
double margin; // margin for contact detection
double gap; // include in solver if dist<margin-gap
mjtNum fluid_switch; // whether ellipsoid-fluid model is active
mjtNum fluid_coefs[5]; // tunable ellipsoid-fluid interaction coefs
mjtNum fluid[mjNFLUID]; // compile-time fluid-interaction parameters
std::string hfieldname; // hfield attached to geom
std::string meshname; // mesh attached to geom
double fitscale; // scale mesh uniformly
std::vector<double> userdata; // user data
float rgba[4]; // rgba when material is omitted
mjtMeshType typeinertia; // selects between surface and volume inertia
bool inferinertia; // true if inertia has to be computed from geom
// variables set by user and used during compilation
double _mass; // used to compute density
double density; // used to compute mass and inertia (from volume)
double fromto[6]; // alternative for capsule, cylinder, box, ellipsoid
mjCAlternative alt; // alternative orientation specifications
// variables set by user or 'Compile'
double pos[3]; // position
double quat[4]; // orientation
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
const std::string& get_hfieldname() { return spec_hfieldname_; }
const std::string& get_meshname() { return spec_meshname_; }
const std::string& get_material() { return spec_material_; }
void del_material() { spec_material_.clear(); }
private:
mjCGeom(mjCModel* = 0, mjCDef* = 0);
void Compile(void); // compiler
double GetRBound(void); // compute bounding sphere radius
void ComputeAABB(void); // compute axis-aligned bounding box
void CopyFromSpec(void);
void MakePointerLocal(void);
std::string material_; // name of material used for rendering
mjCAlternative alt_;
bool visual_; // true: geom does not collide and is unreferenced
int matid; // id of geom's material
mjCMesh* mesh; // geom's mesh
mjCHField* hfield; // geom's hfield
double mass; // mass
double mass_; // mass
double inertia[3]; // local diagonal inertia
double aabb[6]; // axis-aligned bounding box (center, size)
mjCBody* body; // geom's body
mjtNum fluid[mjNFLUID]; // compile-time fluid-interaction parameters
// variable-size data
std::string hfieldname_;
std::string meshname_;
std::string material_;
std::vector<double> userdata_;
std::string spec_hfieldname_;
std::string spec_meshname_;
std::string spec_material_;
std::vector<double> spec_userdata_;
// inherited
using mjCBase::classname;
using mjCBase::info;
using mjCBase::plugin;
};
@@ -479,6 +464,7 @@ class mjCSite : public mjCBase, private mjmSite {
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
const std::string& get_material() { return material_; }
void del_material() { material_.clear(); }
private:
mjCSite(mjCModel* = 0, mjCDef* = 0); // constructor
@@ -701,12 +687,12 @@ class mjCMesh: public mjCBase {
void set_userface(std::optional<std::vector<int>>&& userface);
void Compile(const mjVFS* vfs); // compiler
double* GetPosPtr(mjtMeshType type); // get position
double* GetQuatPtr(mjtMeshType type); // get orientation
double* GetPosPtr(mjtGeomInertia type); // get position
double* GetQuatPtr(mjtGeomInertia type); // get orientation
double* GetOffsetPosPtr(); // get position offset for geom
double* GetOffsetQuatPtr(); // get orientation offset for geom
double* GetInertiaBoxPtr(mjtMeshType type); // get inertia box
double& GetVolumeRef(mjtMeshType type); // get volume
double* GetInertiaBoxPtr(mjtGeomInertia type); // get inertia box
double& GetVolumeRef(mjtGeomInertia type); // get volume
void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom
bool HasTexcoord() const; // texcoord not null
void DelTexcoord(); // delete texcoord
@@ -753,10 +739,10 @@ class mjCMesh: public mjCBase {
void ApplyTransformations(); // apply user transformations
void ComputeFaceCentroid(double[3]); // compute centroid of all faces
void RemoveRepeated(void); // remove repeated vertices
void CheckMesh(mjtMeshType type); // check if the mesh is valid
void CheckMesh(mjtGeomInertia type); // check if the mesh is valid
// compute the volume and center-of-mass of the mesh given the face center
void ComputeVolume(double CoM[3], mjtMeshType type, const double facecen[3],
void ComputeVolume(double CoM[3], mjtGeomInertia type, const double facecen[3],
bool exactmeshinertia);
// mesh properties that indicate a well-formed mesh
@@ -814,6 +800,7 @@ class mjCSkin: public mjCBase {
std::string get_file() const { return file; }
void set_material(std::string _material) { material_ = _material; }
std::string& get_material() { return material_; }
void del_material() { material_.clear(); }
std::string file; // skin file
float rgba[4]; // rgba when material is omitted
@@ -1076,6 +1063,7 @@ class mjCTendon : public mjCBase {
public:
void set_material(std::string _material) { material_ = _material; }
std::string& get_material() { return material_; }
void del_material() { material_.clear(); }
// API for adding wrapping objects
void WrapSite(std::string name, std::string_view info = ""); // site
@@ -1326,6 +1314,7 @@ class mjCDef {
public:
mjCDef(void); // constructor
void Compile(const mjCModel* model); // compiler
void MakePointerLocal();
// identifiers
std::string name; // class name
src/xml/xml_native_reader.cc
+53 -34
View File
@@ -740,8 +740,8 @@ const mjMap tkind_map[2] = {
// mesh type
const mjMap meshtype_map[2] = {
{"false", mjVOLUME_MESH},
{"true", mjSHELL_MESH},
{"false", mjINERTIA_VOLUME},
{"true", mjINERTIA_SHELL},
};
@@ -1504,13 +1504,19 @@ void mjXReader::OneJoint(XMLElement* elem, mjCJoint* pjoint) {
// geom element parser
void mjXReader::OneGeom(XMLElement* elem, mjCGeom* pgeom) {
string text;
void mjXReader::OneGeom(XMLElement* elem, mjmGeom* pgeom) {
string text, name, classname;
std::vector<double> userdata;
std::string hfieldname, meshname, material;
int n;
// read attributes
ReadAttrTxt(elem, "name", pgeom->name);
ReadAttrTxt(elem, "class", pgeom->classname);
if (ReadAttrTxt(elem, "name", name)) {
mjm_setString(pgeom->name, name.c_str());
}
if (ReadAttrTxt(elem, "class", classname)) {
mjm_setString(pgeom->classname, classname.c_str());
}
if (MapValue(elem, "type", &n, geom_map, mjNGEOMTYPES)) {
pgeom->type = (mjtGeom)n;
}
@@ -1526,18 +1532,26 @@ void mjXReader::OneGeom(XMLElement* elem, mjCGeom* pgeom) {
ReadAttr(elem, "solimp", mjNIMP, pgeom->solimp, text, false, false);
ReadAttr(elem, "margin", 1, &pgeom->margin, text);
ReadAttr(elem, "gap", 1, &pgeom->gap, text);
ReadAttrTxt(elem, "hfield", pgeom->hfieldname);
ReadAttrTxt(elem, "mesh", pgeom->meshname);
if (ReadAttrTxt(elem, "hfield", hfieldname)) {
mjm_setString(pgeom->hfieldname, hfieldname.c_str());
}
if (ReadAttrTxt(elem, "mesh", meshname)) {
mjm_setString(pgeom->meshname, meshname.c_str());
}
ReadAttr(elem, "fitscale", 1, &pgeom->fitscale, text);
ReadAttrTxt(elem, "material", pgeom->get_material());
if (ReadAttrTxt(elem, "material", material)) {
mjm_setString(pgeom->material, material.c_str());
}
ReadAttr(elem, "rgba", 4, pgeom->rgba, text);
if (MapValue(elem, "fluidshape", &n, fluid_map, 2)) {
pgeom->fluid_switch = (n == 1);
pgeom->fluid_ellipsoid = (n == 1);
}
ReadAttr(elem, "fluidcoef", 5, pgeom->fluid_coefs, text, false, false);
// read userdata
ReadVector(elem, "user", pgeom->userdata, text);
if (ReadVector(elem, "user", userdata, text)) {
mjm_setDouble(pgeom->userdata, userdata.data(), userdata.size());
}
// plugin sub-element
XMLElement* eplugin = elem->FirstChildElement("plugin");
@@ -1546,7 +1560,7 @@ void mjXReader::OneGeom(XMLElement* elem, mjCGeom* pgeom) {
}
// remaining attributes
ReadAttr(elem, "mass", 1, &pgeom->_mass, text);
ReadAttr(elem, "mass", 1, &pgeom->mass, text);
ReadAttr(elem, "density", 1, &pgeom->density, text);
ReadAttr(elem, "fromto", 6, pgeom->fromto, text);
ReadAttr(elem, "pos", 3, pgeom->pos, text);
@@ -1555,10 +1569,11 @@ void mjXReader::OneGeom(XMLElement* elem, mjCGeom* pgeom) {
// compute inertia using either solid or shell geometry
if (MapValue(elem, "shellinertia", &n, meshtype_map, 2)) {
pgeom->typeinertia = (mjtMeshType)n;
pgeom->typeinertia = (mjtGeomInertia)n;
}
GetXMLPos(elem, pgeom);
mjm_setString(pgeom->info,
std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str());
}
@@ -2153,25 +2168,29 @@ void mjXReader::OneComposite(XMLElement* elem, mjmBody* pbody, mjCDef* def) {
// geom
XMLElement* egeom = elem->FirstChildElement("geom");
if (egeom) {
std::string material;
mjmGeom& dgeom = comp.def[0].geom.spec;
if (MapValue(egeom, "type", &n, geom_map, mjNGEOMTYPES)) {
comp.def[0].geom.type = (mjtGeom)n;
dgeom.type = (mjtGeom)n;
}
ReadAttr(egeom, "size", 3, comp.def[0].geom.size, text, false, false);
ReadAttrInt(egeom, "contype", &comp.def[0].geom.contype);
ReadAttrInt(egeom, "conaffinity", &comp.def[0].geom.conaffinity);
ReadAttrInt(egeom, "condim", &comp.def[0].geom.condim);
ReadAttrInt(egeom, "group", &comp.def[0].geom.group);
ReadAttrInt(egeom, "priority", &comp.def[0].geom.priority);
ReadAttr(egeom, "friction", 3, comp.def[0].geom.friction, text, false, false);
ReadAttr(egeom, "solmix", 1, &comp.def[0].geom.solmix, text);
ReadAttr(egeom, "solref", mjNREF, comp.def[0].geom.solref, text, false, false);
ReadAttr(egeom, "solimp", mjNIMP, comp.def[0].geom.solimp, text, false, false);
ReadAttr(egeom, "margin", 1, &comp.def[0].geom.margin, text);
ReadAttr(egeom, "gap", 1, &comp.def[0].geom.gap, text);
ReadAttrTxt(egeom, "material", comp.def[0].geom.get_material());
ReadAttr(egeom, "rgba", 4, comp.def[0].geom.rgba, text);
ReadAttr(egeom, "mass", 1, &comp.def[0].geom._mass, text);
ReadAttr(egeom, "density", 1, &comp.def[0].geom.density, text);
ReadAttr(egeom, "size", 3, dgeom.size, text, false, false);
ReadAttrInt(egeom, "contype", &dgeom.contype);
ReadAttrInt(egeom, "conaffinity", &dgeom.conaffinity);
ReadAttrInt(egeom, "condim", &dgeom.condim);
ReadAttrInt(egeom, "group", &dgeom.group);
ReadAttrInt(egeom, "priority", &dgeom.priority);
ReadAttr(egeom, "friction", 3, dgeom.friction, text, false, false);
ReadAttr(egeom, "solmix", 1, &dgeom.solmix, text);
ReadAttr(egeom, "solref", mjNREF, dgeom.solref, text, false, false);
ReadAttr(egeom, "solimp", mjNIMP, dgeom.solimp, text, false, false);
ReadAttr(egeom, "margin", 1, &dgeom.margin, text);
ReadAttr(egeom, "gap", 1, &dgeom.gap, text);
if (ReadAttrTxt(egeom, "material", material)) {
mjm_setString(dgeom.material, material.c_str());
}
ReadAttr(egeom, "rgba", 4, dgeom.rgba, text);
ReadAttr(egeom, "mass", 1, &dgeom.mass, text);
ReadAttr(egeom, "density", 1, &dgeom.density, text);
}
// site
@@ -2494,7 +2513,7 @@ void mjXReader::Default(XMLElement* section, int parentid) {
else if (name=="joint") OneJoint(elem, &def->joint);
// read geom
else if (name=="geom") OneGeom(elem, &def->geom);
else if (name=="geom") OneGeom(elem, &def->geom.spec);
// read site
else if (name=="site") OneSite(elem, def->site.spec);
@@ -3067,9 +3086,9 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjCFrame* frame) {
// geom sub-element
else if (name=="geom") {
// create geom and parse
mjCGeom* pgeom = (mjCGeom*)mjm_addGeom(pbody, def);
mjmGeom* pgeom = mjm_addGeom(pbody, def);
OneGeom(elem, pgeom);
pgeom->SetFrame(frame);
mjm_setFrame(pgeom->element, frame);
}
// site sub-element
src/xml/xml_native_reader.h
+1 -1
View File
@@ -58,7 +58,7 @@ class mjXReader : public mjXBase {
void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin);
void OneMaterial(tinyxml2::XMLElement* elem, mjCMaterial* pmaterial);
void OneJoint(tinyxml2::XMLElement* elem, mjCJoint* pjoint);
void OneGeom(tinyxml2::XMLElement* elem, mjCGeom* pgeom);
void OneGeom(tinyxml2::XMLElement* elem, mjmGeom* pgeom);
void OneSite(tinyxml2::XMLElement* elem, mjmSite& site);
void OneCamera(tinyxml2::XMLElement* elem, mjCCamera* pcamera);
void OneLight(tinyxml2::XMLElement* elem, mjCLight* plight);
src/xml/xml_native_writer.cc
+8 -8
View File
@@ -342,7 +342,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
if (mjGEOMINFO[pgeom->type]) {
WriteAttr(elem, "size", mjGEOMINFO[pgeom->type], pgeom->size, def->geom.size);
}
if (mjuu_defined(pgeom->_mass)) {
if (mjuu_defined(pgeom->mass)) {
mass = pgeom->GetVolume() * def->geom.density;
}
@@ -389,11 +389,11 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
WriteAttr(elem, "margin", 1, &pgeom->margin, &def->geom.margin);
WriteAttr(elem, "gap", 1, &pgeom->gap, &def->geom.gap);
WriteAttr(elem, "gap", 1, &pgeom->gap, &def->geom.gap);
WriteAttrKey(elem, "fluidshape", fluid_map, 2, pgeom->fluid_switch, def->geom.fluid_switch);
WriteAttrKey(elem, "fluidshape", fluid_map, 2, pgeom->fluid_ellipsoid, def->geom.fluid_ellipsoid);
WriteAttr(elem, "fluidcoef", 5, pgeom->fluid_coefs, def->geom.fluid_coefs);
WriteAttrKey(elem, "shellinertia", meshtype_map, 2, pgeom->typeinertia, def->geom.typeinertia);
if (mjuu_defined(pgeom->_mass)) {
WriteAttr(elem, "mass", 1, &pgeom->mass, &mass);
if (mjuu_defined(pgeom->mass)) {
WriteAttr(elem, "mass", 1, &pgeom->mass_, &mass);
} else {
WriteAttr(elem, "density", 1, &pgeom->density, &def->geom.density);
}
@@ -404,17 +404,17 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
// hfield and mesh attributes
if (pgeom->type==mjGEOM_HFIELD) {
WriteAttrTxt(elem, "hfield", pgeom->hfieldname);
WriteAttrTxt(elem, "hfield", pgeom->get_hfieldname());
}
if (pgeom->type==mjGEOM_MESH || pgeom->type==mjGEOM_SDF) {
WriteAttrTxt(elem, "mesh", pgeom->meshname);
WriteAttrTxt(elem, "mesh", pgeom->get_meshname());
}
// userdata
if (writingdefaults) {
WriteVector(elem, "user", pgeom->userdata);
WriteVector(elem, "user", pgeom->get_userdata());
} else {
WriteVector(elem, "user", pgeom->userdata, def->geom.userdata);
WriteVector(elem, "user", pgeom->get_userdata(), def->geom.get_userdata());
}
// write plugin
src/xml/xml_urdf.cc
+7 -7
View File
@@ -223,7 +223,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
std::string name, text;
XMLElement *elem, *temp, *temp1;
mjmBody *pbody, *world;
mjCGeom* pgeom;
mjmGeom* pgeom;
// get body name and pointer to mjmBody
ReadAttrTxt(body_elem, "name", name, true);
@@ -322,7 +322,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
mjXUtil::ReadAttrTxt(elem, "name", geom_name);
name = GetPrefixedName(name);
if (urGeomNames.find(geom_name) == urGeomNames.end()) {
pgeom->name = geom_name;
mjm_setString(pgeom->name, geom_name.c_str());
urGeomNames.insert(geom_name);
} else {
std::cerr << "WARNING: Geom with duplicate name '" << geom_name
@@ -345,7 +345,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
mjXUtil::ReadAttrTxt(elem, "name", geom_name);
geom_name = GetPrefixedName(geom_name);
if (urGeomNames.find(geom_name) == urGeomNames.end()) {
pgeom->name = geom_name;
mjm_setString(pgeom->name, geom_name.c_str());
urGeomNames.insert(geom_name);
} else {
std::cerr << "WARNING: Geom with duplicate name '" << geom_name
@@ -501,7 +501,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
// parse origin and geometry elements of visual or collision
mjCGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) {
mjmGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) {
XMLElement *elem, *temp;
std::string text, meshfile;
@@ -509,8 +509,8 @@ mjCGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) {
elem = FindSubElem(geom_elem, "geometry", true);
// add BOX geom, modify type later
mjCGeom* pgeom = (mjCGeom*)mjm_addGeom(pbody, 0);
pgeom->name = "";
mjmGeom* pgeom = mjm_addGeom(pbody, 0);
mjm_setString(pgeom->name, "");
pgeom->type = mjGEOM_BOX;
if (collision) {
pgeom->contype = 1;
@@ -589,7 +589,7 @@ mjCGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) {
// set fields
pmesh->set_file(meshfile);
pmesh->name = meshname;
pgeom->meshname = meshname;
mjm_setString(pgeom->meshname, meshname.c_str());
pmesh->set_scale(meshscale);
}
src/xml/xml_urdf.h
+1 -1
View File
@@ -52,7 +52,7 @@ class mjXURDF : public mjXBase {
void AddToTree(int n); // add body to mjCModel tree
void Body(tinyxml2::XMLElement* body_elem); // parse body
void Joint(tinyxml2::XMLElement* joint_elem); // parse joint
mjCGeom* Geom(tinyxml2::XMLElement* geom_elem,
mjmGeom* Geom(tinyxml2::XMLElement* geom_elem,
mjmBody* pbody, bool collision); // parse origin and geometry of geom
void Origin(tinyxml2::XMLElement* origin_elem, double* pos, double* quat); // parse origin element
test/user/user_mesh_test.cc
+1 -1
View File
@@ -549,7 +549,7 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNoMass) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, NotNull());
EXPECT_THAT(model, NotNull()) << error.data();
CheckTetrahedronWasRescaled(model);
mj_deleteModel(model);
}