diff --git a/src/user/user_api.cc b/src/user/user_api.cc index edfda080..a4435e42 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -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(defspec); mjCBody* body = reinterpret_cast(bodyspec->element); mjCGeom* geom = body->AddGeom(def); - return geom; + return &geom->spec; } diff --git a/src/user/user_api.h b/src/user/user_api.h index 9190ef19..3cda342b 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -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 distdef = (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); } diff --git a/src/user/user_init.cc b/src/user/user_init.cc index 22bc46e0..3481976b 100644 --- a/src/user/user_init.cc +++ b/src/user/user_init.cc @@ -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)); diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index e50cf1f0..35e56cbd 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -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; igeom.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_; } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index e627426a..10d018cf 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -565,6 +565,7 @@ mjCDef* mjCModel::AddDef(string name, int parentid) { if (parentid>=0 && parentidchildid.push_back(thisid); + def->MakePointerLocal(); } def->parentid = parentid; def->name = name; @@ -654,8 +655,8 @@ void mjCModel::MakeLists(mjCBody* body) { template void mjCModel::DeleteMaterial(std::vector& 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; imesh && 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; iuserdata.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); } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 4eb31b63..6c02e16d 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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; imass; - 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; iinferinertia = 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(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(); } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 98a323a9..fdf79714 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -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 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& 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 userdata_; + std::string spec_hfieldname_; + std::string spec_meshname_; + std::string spec_material_; + std::vector 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& 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>&& 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 diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 67c947f2..8bbe389c 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -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 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 diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 583ebace..20a71e2d 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -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); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index c30ff289..e329cfc0 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -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 diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 00240b2f..0a8c8bbc 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -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); } diff --git a/src/xml/xml_urdf.h b/src/xml/xml_urdf.h index 4fa24159..c47aedcd 100644 --- a/src/xml/xml_urdf.h +++ b/src/xml/xml_urdf.h @@ -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 diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index a4bd0052..845dbba6 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -549,7 +549,7 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNoMass) { )"; std::array error; mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, NotNull()); + EXPECT_THAT(model, NotNull()) << error.data(); CheckTetrahedronWasRescaled(model); mj_deleteModel(model); }