From b224367a9b5de430e868c0215a56034ce998b87e Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 12 Feb 2024 08:46:57 -0800 Subject: [PATCH] Add mjmFlex to C API. PiperOrigin-RevId: 606264589 Change-Id: Ifd94f5241a3eaaf23653b136cece81c054f43202 --- src/user/user_api.cc | 50 +++++++++++- src/user/user_api.h | 84 +++++++++++++++++--- src/user/user_flexcomp.cc | 61 ++++++++------- src/user/user_init.cc | 26 ++++++- src/user/user_mesh.cc | 143 +++++++++++++++++++---------------- src/user/user_model.cc | 10 +-- src/user/user_objects.cc | 32 ++++---- src/user/user_objects.h | 62 +++++++-------- src/xml/xml_native_reader.cc | 85 +++++++++++++-------- src/xml/xml_native_reader.h | 2 +- src/xml/xml_native_writer.cc | 16 ++-- 11 files changed, 368 insertions(+), 203 deletions(-) diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 84c11eeb..f29a7429 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -19,6 +19,7 @@ #include #include "user/user_model.h" #include "user/user_objects.h" +#include "xml/xml_util.h" @@ -114,6 +115,15 @@ mjmLight* mjm_addLight(mjmBody* bodyspec, void* defspec) { +// add flex to model +mjmFlex* mjm_addFlex(void* model) { + mjCModel* modelC = static_cast(model); + mjCFlex* flex = modelC->AddFlex(); + return &flex->spec; +} + + + // add frame to body mjmFrame* mjm_addFrame(mjmBody* bodyspec, mjmFrame* parentframe) { mjCFrame* parentframeC = 0; @@ -292,8 +302,44 @@ void mjm_setString(mjString dest, const char* text) { +// split text and copy into string array +void mjm_setStringVec(mjStringVec dest, const char* text) { + std::vector* v = reinterpret_cast*>(dest); + mjXUtil::String2Vector(text, *v); +} + + +// add text entry to destination string vector +void mjm_addToStringVec(mjStringVec dest, const char* text) { + std::vector* v = reinterpret_cast*>(dest); + v->push_back(std::string(text)); +} + + +// set int array +void mjm_setInt(mjIntVec dest, const int* array, int size) { + std::vector* v = reinterpret_cast*>(dest); + v->assign(size, 0.0); + for (int i = 0; i < size; ++i) { + (*v)[i] = array[i]; + } +} + + + +// set float array +void mjm_setFloat(mjFloatVec dest, const float* array, int size) { + std::vector* v = reinterpret_cast*>(dest); + v->assign(size, 0.0); + for (int i = 0; i < size; ++i) { + (*v)[i] = array[i]; + } +} + + + // set double array -void mjm_setDouble(mjDouble dest, const double* array, int size) { +void mjm_setDouble(mjDoubleVec dest, const double* array, int size) { std::vector* v = reinterpret_cast*>(dest); v->assign(size, 0.0); for (int i = 0; i < size; ++i) { @@ -312,7 +358,7 @@ const char* mjm_getString(const mjString source) { // get double array -const double* mjm_getDouble(const mjDouble source, int* size) { +const double* mjm_getDouble(const mjDoubleVec source, int* size) { std::vector* v = reinterpret_cast*>(source); if (size) { *size = v->size(); diff --git a/src/user/user_api.h b/src/user/user_api.h index 7148be46..d904f0d4 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -30,7 +30,10 @@ extern "C" { typedef struct _mjElement* mjElement; typedef struct _mjString* mjString; -typedef struct _mjDouble* mjDouble; +typedef struct _mjStringVec* mjStringVec; +typedef struct _mjIntVec* mjIntVec; +typedef struct _mjDoubleVec* mjDoubleVec; +typedef struct _mjFloatVec* mjFloatVec; //---------------------------------- enum types (mjt) ---------------------------------------------- @@ -80,7 +83,7 @@ typedef struct _mjmBody { // body specification // other mjtByte mocap; // is this a mocap body double gravcomp; // gravity compensation - mjDouble userdata; // user data + mjDoubleVec userdata; // user data mjtByte explicitinertial; // whether to save the body with explicit inertial clause mjmPlugin plugin; // passive force plugin mjString info; // message appended to compiler errors @@ -131,7 +134,7 @@ typedef struct _mjmJoint { // joint specification // other int group; // group double urdfeffort; // effort (urdf) - mjDouble userdata; // user data + mjDoubleVec userdata; // user data mjString info; // message appended to compiler errors } mjmJoint; @@ -179,7 +182,7 @@ typedef struct _mjmGeom { // geom specification mjString hfieldname; // heightfield attached to geom mjString meshname; // mesh attached to geom double fitscale; // scale mesh uniformly - mjDouble userdata; // user data + mjDoubleVec userdata; // user data mjmPlugin plugin; // sdf plugin mjString info; // message appended to compiler errors } mjmGeom; @@ -204,7 +207,7 @@ typedef struct _mjmSite { // site specification float rgba[4]; // rgba when material is omitted // other - mjDouble userdata; // user data + mjDoubleVec userdata; // user data mjString info; // message appended to compiler errors } mjmSite; @@ -233,7 +236,7 @@ typedef struct _mjmCamera { // camera specification float principal_pixel[2]; // principal point (pixel) // other - mjDouble userdata; // user data + mjDoubleVec userdata; // user data mjString info; // message appended to compiler errors } mjmCamera; @@ -265,6 +268,47 @@ typedef struct _mjmLight { // light specification } mjmLight; +typedef struct _mjmFlex { + mjElement element; // internal, do not modify + mjString name; // name + mjString classname; // class name + + // contact properties + int contype; // contact type + int conaffinity; // contact affinity + int condim; // contact dimensionality + int priority; // contact priority + 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 dist3) { return comperr(error, "Invalid dim, must be between 1 and 3", error_sz); } @@ -164,7 +166,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz // force flatskin shading for box, cylinder and 3D grid if (type==mjFCOMPTYPE_BOX || type==mjFCOMPTYPE_CYLINDER || (type==mjFCOMPTYPE_GRID && dim==3)) { - def.flex.flatskin = true; + def.flex.spec.flatskin = true; } // check pin sizes @@ -368,21 +370,22 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz } // create flex, copy parameters - mjCFlex* pf = model->AddFlex(); - int id = pf->id; - *pf = def.flex; - pf->model = model; - pf->id = id; - pf->name = name; - pf->elem = element; - if (!centered) { - pf->vert = point; - } - pf->texcoord = texcoord; + mjCFlex* flex = model->AddFlex(); + mjmFlex* pf = &flex->spec; + int id = flex->id; + + *flex = def.flex; + flex->PointToLocal(); + + flex->model = model; + flex->id = id; + mjm_setString(pf->name, name.c_str()); + mjm_setInt(pf->elem, element.data(), element.size()); + mjm_setFloat(pf->texcoord, texcoord.data(), texcoord.size()); // rigid: set parent name, nothing else to do if (rigid) { - pf->vertbody.push_back(std::string(mjm_getString(body->name)).c_str()); + mjm_addToStringVec(pf->vertbody, mjm_getString(body->name)); return true; } @@ -399,7 +402,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz // pinned: parent body if (pinned[i]) { - pf->vertbody.push_back(std::string(mjm_getString(body->name)).c_str()); + mjm_addToStringVec(pf->vertbody, mjm_getString(body->name)); // add plugin if (plugin_instance) { @@ -456,13 +459,13 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz char txt[100]; mju::sprintf_arr(txt, "%s_%d", name.c_str(), i); mjm_setString(pb->name, txt); - pf->vertbody.push_back(std::string(mjm_getString(pb->name)).c_str()); + mjm_addToStringVec(pf->vertbody, mjm_getString(pb->name)); // clear flex vertex coordinates if allocated if (!centered) { - pf->vert[3*i] = 0; - pf->vert[3*i+1] = 0; - pf->vert[3*i+2] = 0; + point[3*i] = 0; + point[3*i+1] = 0; + point[3*i+2] = 0; } // add plugin @@ -476,6 +479,10 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz } } + if (!centered) { + mjm_setDouble(pf->vert, point.data(), point.size()); + } + // create edge equality constraint if (equality) { mjmEquality* pe = mjm_addEquality(model, &def); @@ -502,7 +509,7 @@ int mjCFlexcomp::GridID(int ix, int iy, int iz) { // make grid bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { - int dim = def.flex.dim; + int dim = def.flex.spec.dim; bool hastex = texcoord.empty(); // 1D @@ -683,7 +690,7 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) { double pos[3]; // set 3D - def.flex.dim = 3; + def.flex.spec.dim = 3; // add center point point.push_back(0); @@ -815,7 +822,7 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) { } // check dim - if (def.flex.dim!=2) { + if (def.flex.spec.dim!=2) { return comperr(error, "Flex dim must be 2 in for mesh", error_sz); } @@ -1027,7 +1034,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) { if (entityDim<1 || entityDim>3) { throw mjCError(NULL, "Entity must be 1D, 2D or 3D"); } - def.flex.dim = entityDim; + def.flex.spec.dim = entityDim; // read and discard node tags; require range from minNodeTag to maxNodeTag for (size_t i=0; i3) { throw mjCError(NULL, "Entity must be 1D, 2D or 3D"); } - def.flex.dim = entityDim; + def.flex.spec.dim = entityDim; // check section byte size if (nodeend-nodebegin < 52+numNodes*4*8) { @@ -1127,7 +1134,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) { } // dimensionality must be same as nodes - if (entityDim!=def.flex.dim) { + if (entityDim!=def.flex.spec.dim) { throw mjCError(NULL, "Inconsistent dimensionality in Elements"); } @@ -1176,7 +1183,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) { } // dimensionality must be same as nodes - if (entityDim!=def.flex.dim) { + if (entityDim!=def.flex.spec.dim) { throw mjCError(NULL, "Inconsistent dimensionality in Elements"); } diff --git a/src/user/user_init.cc b/src/user/user_init.cc index ed12d2f9..3251e023 100644 --- a/src/user/user_init.cc +++ b/src/user/user_init.cc @@ -167,7 +167,31 @@ void mjm_defaultLight(mjmLight& light) { -// Default material attributes. +// default flex attributes +void mjm_defaultFlex(mjmFlex& flex) { + memset(&flex, 0, sizeof(mjmFlex)); + + // set contact defaults + flex.contype = 1; + flex.conaffinity = 1; + flex.condim = 3; + mjuu_setvec(flex.friction, 1, 0.005, 0.0001); + flex.solmix = 1.0; + mj_defaultSolRefImp(flex.solref, flex.solimp); + + // set other defaults + flex.dim = 2; + flex.radius = 0.005; + flex.internal = true; + flex.selfcollide = mjFLEXSELF_AUTO; + flex.activelayers = 1; + flex.rgba[0] = flex.rgba[1] = flex.rgba[2] = 0.5f; + flex.rgba[3] = 1.0f; +} + + + +// default material attributes void mjm_defaultMaterial(mjmMaterial& material) { memset(&material, 0, sizeof(mjmMaterial)); material.texuniform = false; diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 35e56cbd..8fb4849b 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -2222,34 +2222,11 @@ constexpr int eledge[3][6][2] = {{{ 0, 1}, {-1, -1}, {-1, -1}, // constructor mjCFlex::mjCFlex(mjCModel* _model) { + mjm_defaultFlex(spec); + // set model model = _model; - // set contact defaults - contype = 1; - conaffinity = 1; - condim = 3; - priority = 0; - mjuu_setvec(friction, 1, 0.005, 0.0001); - solmix = 1.0; - mj_defaultSolRefImp(solref, solimp); - margin = 0; - gap = 0; - - // set other defaults - dim = 2; - radius = 0.005; - internal = true; - flatskin = false; - selfcollide = mjFLEXSELF_AUTO; - activelayers = 1; - group = 0; - edgestiffness = 0; - edgedamping = 0; - material_.clear(); - rgba[0] = rgba[1] = rgba[2] = 0.5f; - rgba[3] = 1.0f; - // clear internal variables nvert = 0; nedge = 0; @@ -2257,50 +2234,84 @@ mjCFlex::mjCFlex(mjCModel* _model) { matid = -1; rigid = false; centered = false; + + PointToLocal(); + CopyFromSpec(); +} + + +void mjCFlex::PointToLocal() { + spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.classname = (mjString)&classname; + spec.material = (mjString)&spec_material_; + spec.vertbody = (mjStringVec)&spec_vertbody_; + spec.vert = (mjDoubleVec)&spec_vert_; + spec.texcoord = (mjFloatVec)&spec_texcoord_; + spec.elem = (mjIntVec)&spec_elem_; + spec.info = (mjString)&info; +} + + +void mjCFlex::CopyFromSpec() { + *static_cast(this) = spec; + spec.info = (mjString)&info; + material_ = spec_material_; + vertbody_ = spec_vertbody_; + vert_ = spec_vert_; + texcoord_ = spec_texcoord_; + elem_ = spec_elem_; + material = (mjString)&material_; + vertbody = (mjStringVec)&vertbody_; + vert = (mjDoubleVec)&vert_; + texcoord = (mjFloatVec)&texcoord_; + elem = (mjIntVec)&elem_; } bool mjCFlex::HasTexcoord() const { - return !texcoord.empty(); + return !texcoord_.empty(); } void mjCFlex::DelTexcoord() { - texcoord.clear(); + texcoord_.clear(); } // compiler void mjCFlex::Compile(const mjVFS* vfs) { + CopyFromSpec(); + // set nelem; check sizes if (dim<1 || dim>3) { throw mjCError(this, "dim must be 1, 2 or 3"); } - if (elem.empty()) { + if (elem_.empty()) { throw mjCError(this, "elem is empty"); } - if (elem.size() % (dim+1)) { + if (elem_.size() % (dim+1)) { throw mjCError(this, "elem size must be multiple of (dim+1)"); } - if (vertbody.empty()) { + if (vertbody_.empty()) { throw mjCError(this, "vertbody is empty"); } - if (vert.size() % 3) { + if (vert_.size() % 3) { throw mjCError(this, "vert size must be a multiple of 3"); } if (edgestiffness>0 && dim>1) { throw mjCError(this, "edge stiffness only available for dim=1, please use elasticity plugins"); } - nelem = (int)elem.size()/(dim+1); + nelem = (int)elem_.size()/(dim+1); // set nvert, rigid, centered; check size - if (vert.empty()) { + if (vert_.empty()) { centered = true; - nvert = (int)vertbody.size(); + nvert = (int)vertbody_.size(); } else { - nvert = (int)vert.size()/3; - if (vertbody.size()==1) { + nvert = (int)vert_.size()/3; + if (vertbody_.size()==1) { rigid = true; } } @@ -2309,14 +2320,14 @@ void mjCFlex::Compile(const mjVFS* vfs) { } // check elem vertex ids - for (int i=0; i<(int)elem.size(); i++) { - if (elem[i]<0 || elem[i]>=nvert) { + for (int i=0; i<(int)elem_.size(); i++) { + if (elem_[i]<0 || elem_[i]>=nvert) { throw mjCError(this, "elem vertex id out of range"); } } // check texcoord - if (!texcoord.empty() && texcoord.size()!=2*nvert) { + if (!texcoord_.empty() && texcoord_.size()!=2*nvert) { throw mjCError(this, "two texture coordinates per vertex expected"); } @@ -2329,20 +2340,20 @@ void mjCFlex::Compile(const mjVFS* vfs) { } // resolve body ids - for (int i=0; i<(int)vertbody.size(); i++) { - mjCBase* pbody = model->FindObject(mjOBJ_BODY, vertbody[i]); + for (int i=0; i<(int)vertbody_.size(); i++) { + mjCBase* pbody = model->FindObject(mjOBJ_BODY, vertbody_[i]); if (pbody) { vertbodyid.push_back(pbody->id); } else { - throw mjCError(this, "unkown body '%s' in flex", vertbody[i].c_str()); + throw mjCError(this, "unkown body '%s' in flex", vertbody_[i].c_str()); } } // process elements - for (int e=0; e<(int)elem.size()/(dim+1); e++) { + for (int e=0; e<(int)elem_.size()/(dim+1); e++) { // make sorted copy of element vector el; - el.assign(elem.begin()+e*(dim+1), elem.begin()+(e+1)*(dim+1)); + el.assign(elem_.begin()+e*(dim+1), elem_.begin()+(e+1)*(dim+1)); std::sort(el.begin(), el.end()); // check for repeated vertices @@ -2367,8 +2378,8 @@ void mjCFlex::Compile(const mjVFS* vfs) { // determine centered if not already set if (!centered) { centered = true; - for (int i=0; i<(int)vert.size(); i++) { - if (vert[i]!=0) { + for (int i=0; i<(int)vert_.size(); i++) { + if (vert_[i]!=0) { centered = false; break; } @@ -2385,7 +2396,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { // add vertex offset within body if not centered if (!centered) { mjtNum offset[3]; - mju_rotVecQuat(offset, vert.data()+3*i, model->bodies[b]->xquat0); + mju_rotVecQuat(offset, vert_.data()+3*i, model->bodies[b]->xquat0); mju_addTo3(vertxpos.data()+3*i, offset); } } @@ -2394,7 +2405,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { // faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2) if (dim==3) { for (int e=0; e0) { // flip orientation - int tmp = elem[e*(dim+1)+1]; - elem[e*(dim+1)+1] = elem[e*(dim+1)+2]; - elem[e*(dim+1)+2] = tmp; + int tmp = elem_[e*(dim+1)+1]; + elem_[e*(dim+1)+1] = elem_[e*(dim+1)+2]; + elem_[e*(dim+1)+2] = tmp; } } } // create edges - std::vector edgeidx(elem.size()*kNumEdges[dim-1]); + std::vector edgeidx(elem_.size()*kNumEdges[dim-1]); // map from edge vertices to their index in `edges` vector std::unordered_map, int, PairHash> edge_indices; // insert local edges into global vector - for (int f = 0; f < (int)elem.size()/(dim+1); f++) { - int* v = elem.data() + f*(dim+1); + for (int f = 0; f < (int)elem_.size()/(dim+1); f++) { + int* v = elem_.data() + f*(dim+1); for (int e = 0; e < kNumEdges[dim-1]; e++) { auto pair = std::pair( std::min(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]), @@ -2447,7 +2458,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { // add plugins std::string userface, useredge; - mjXUtil::Vector2String(userface, elem); + mjXUtil::Vector2String(userface, elem_); mjXUtil::Vector2String(useredge, edgeidx); for (int i=0; i<(int)vertbodyid.size(); i++) { @@ -2472,12 +2483,12 @@ void mjCFlex::CreateBVH(void) { int nbvh = 0; // allocate element bounding boxes - elemaabb.resize(6*nelem); + elemaabb_.resize(6*nelem); tree.AllocateBoundingVolumes(nelem); // construct element bounding boxes, add to hierarchy for (int e=0; e=activelayers) { @@ -2496,12 +2507,12 @@ void mjCFlex::CreateBVH(void) { } // compute aabb (center, size) - elemaabb[6*e+0] = 0.5*(xmax[0]+xmin[0]); - elemaabb[6*e+1] = 0.5*(xmax[1]+xmin[1]); - elemaabb[6*e+2] = 0.5*(xmax[2]+xmin[2]); - elemaabb[6*e+3] = 0.5*(xmax[0]-xmin[0]) + radius; - elemaabb[6*e+4] = 0.5*(xmax[1]-xmin[1]) + radius; - elemaabb[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius; + elemaabb_[6*e+0] = 0.5*(xmax[0]+xmin[0]); + elemaabb_[6*e+1] = 0.5*(xmax[1]+xmin[1]); + elemaabb_[6*e+2] = 0.5*(xmax[2]+xmin[2]); + elemaabb_[6*e+3] = 0.5*(xmax[0]-xmin[0]) + radius; + elemaabb_[6*e+4] = 0.5*(xmax[1]-xmin[1]) + radius; + elemaabb_[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius; // add bounding volume for this element mjCBoundingVolume* bv = tree.GetBoundingVolume(nbvh++); @@ -2509,7 +2520,7 @@ void mjCFlex::CreateBVH(void) { bv->conaffinity = conaffinity; bv->quat = NULL; bv->SetId(e); - bv->aabb = elemaabb.data() + 6*e; + bv->aabb = elemaabb_.data() + 6*e; bv->pos = bv->aabb; } @@ -2533,7 +2544,7 @@ void mjCFlex::CreateShellPair(void) { // element vertices in original (unsorted) order vector el; - el.assign(elem.begin()+n, elem.begin()+n+dim+1); + el.assign(elem_.begin()+n, elem_.begin()+n+dim+1); // line: 2 vertex fragments if (dim==1) { @@ -2702,7 +2713,7 @@ void mjCFlex::CreateShellPair(void) { // process both elements for (int ei=0; ei<2; ei++) { - const int* edata = elem.data() + connectspec[n][ei]*(dim+1); + const int* edata = elem_.data() + connectspec[n][ei]*(dim+1); // find element vertex that is not in the common fragment for (int i=0; i<=dim; i++) { diff --git a/src/user/user_model.cc b/src/user/user_model.cc index f85fc3b2..b9cbecaf 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1995,15 +1995,15 @@ void mjCModel::CopyObjects(mjModel* m) { m->flex_evpairnum[i] = (int)pfl->evpair.size()/2; memcpy(m->flex_evpair + 2*evpair_adr, pfl->evpair.data(), pfl->evpair.size()*sizeof(int)); } - if (pfl->texcoord.empty()) { + if (pfl->texcoord_.empty()) { m->flex_texcoordadr[i] = -1; } else { m->flex_texcoordadr[i] = texcoord_adr; memcpy(m->flex_texcoord + 2*texcoord_adr, - pfl->texcoord.data(), pfl->texcoord.size()*sizeof(float)); + pfl->texcoord_.data(), pfl->texcoord_.size()*sizeof(float)); } m->flex_elemnum[i] = pfl->nelem; - memcpy(m->flex_elem + elemdata_adr, pfl->elem.data(), pfl->elem.size()*sizeof(int)); + memcpy(m->flex_elem + elemdata_adr, pfl->elem_.data(), pfl->elem_.size()*sizeof(int)); memcpy(m->flex_elemlayer + elem_adr, pfl->elemlayer.data(), pfl->nelem*sizeof(int)); if (m->flex_shellnum[i]) { memcpy(m->flex_shell + shelldata_adr, pfl->shell.data(), pfl->shell.size()*sizeof(int)); @@ -2042,7 +2042,7 @@ void mjCModel::CopyObjects(mjModel* m) { mju_zero(m->flex_vert + 3*vert_adr, 3*pfl->nvert); } else { - memcpy(m->flex_vert + 3*vert_adr, pfl->vert.data(), 3*pfl->nvert*sizeof(mjtNum)); + memcpy(m->flex_vert + 3*vert_adr, pfl->vert_.data(), 3*pfl->nvert*sizeof(mjtNum)); } // copy or set vertbodyid @@ -2073,7 +2073,7 @@ void mjCModel::CopyObjects(mjModel* m) { elemdata_adr += (pfl->dim+1) * pfl->nelem; shelldata_adr += (int)pfl->shell.size(); evpair_adr += (int)pfl->evpair.size()/2; - texcoord_adr += (int)pfl->texcoord.size()/2; + texcoord_adr += (int)pfl->texcoord_.size()/2; bvh_adr += pfl->tree.nbvh; } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 942e7699..9f0e9d55 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -596,7 +596,7 @@ void mjCBody::PointToLocal() { spec.element = (mjElement)this; spec.name = (mjString)&name; spec.classname = (mjString)&classname; - spec.userdata = (mjDouble)&spec_userdata_; + spec.userdata = (mjDoubleVec)&spec_userdata_; spec.plugin.name = (mjString)&plugin_name; spec.plugin.instance_name = (mjString)&plugin_instance_name; } @@ -605,7 +605,7 @@ void mjCBody::PointToLocal() { void mjCBody::CopyFromSpec() { *static_cast(this) = spec; userdata_ = spec_userdata_; - userdata = (mjDouble)&userdata_; + userdata = (mjDoubleVec)&userdata_; spec.info = (mjString)&info; mju_copy4(alt_.axisangle, alt.axisangle); mju_copy(alt_.xyaxes, alt.xyaxes, 6); @@ -1262,7 +1262,7 @@ void mjCJoint::PointToLocal() { spec.element = (mjElement)this; spec.name = (mjString)&name; spec.classname = (mjString)&classname; - spec.userdata = (mjDouble)&spec_userdata_; + spec.userdata = (mjDoubleVec)&spec_userdata_; spec.info = (mjString)&info; } @@ -1271,7 +1271,7 @@ void mjCJoint::PointToLocal() { void mjCJoint::CopyFromSpec() { *static_cast(this) = spec; userdata_ = spec_userdata_; - userdata = (mjDouble)&spec_userdata_; + userdata = (mjDoubleVec)&spec_userdata_; } @@ -1460,7 +1460,7 @@ void mjCGeom::PointToLocal(void) { spec.name = (mjString)&name; spec.info = (mjString)&info; spec.classname = (mjString)&classname; - spec.userdata = (mjDouble)&spec_userdata_; + spec.userdata = (mjDoubleVec)&spec_userdata_; spec.material = (mjString)&spec_material_; spec.meshname = (mjString)&spec_meshname_; spec.hfieldname = (mjString)&spec_hfieldname_; @@ -1476,7 +1476,7 @@ void mjCGeom::CopyFromSpec() { hfieldname_ = spec_hfieldname_; meshname_ = spec_meshname_; material_ = spec_material_; - userdata = (mjDouble)&userdata_; + userdata = (mjDoubleVec)&userdata_; hfieldname = (mjString)&hfieldname_; meshname = (mjString)&meshname_; material = (mjString)&material_; @@ -2061,7 +2061,7 @@ void mjCSite::PointToLocal() { spec.info = (mjString)&info; spec.classname = (mjString)&classname; spec.material = (mjString)&spec_material_; - spec.userdata = (mjDouble)&spec_userdata_; + spec.userdata = (mjDoubleVec)&spec_userdata_; } @@ -2070,7 +2070,7 @@ void mjCSite::CopyFromSpec() { *static_cast(this) = spec; userdata_ = spec_userdata_; material_ = spec_material_; - userdata = (mjDouble)&userdata_; + userdata = (mjDoubleVec)&userdata_; material = (mjString)&material_; mju_copy4(alt_.axisangle, alt.axisangle); mju_copy(alt_.xyaxes, alt.xyaxes, 6); @@ -2201,7 +2201,7 @@ void mjCCamera::PointToLocal() { spec.element = (mjElement)this; spec.name = (mjString)&name; spec.classname = (mjString)&classname; - spec.userdata = (mjDouble)&spec_userdata_; + spec.userdata = (mjDoubleVec)&spec_userdata_; spec.targetbody = (mjString)&spec_targetbody_; spec.info = (mjString)&info; } @@ -2212,7 +2212,7 @@ void mjCCamera::CopyFromSpec() { *static_cast(this) = spec; userdata_ = spec_userdata_; targetbody_ = spec_targetbody_; - userdata = (mjDouble)&userdata_; + userdata = (mjDoubleVec)&userdata_; targetbody = (mjString)&targetbody_; mju_copy4(alt_.axisangle, alt.axisangle); mju_copy(alt_.xyaxes, alt.xyaxes, 6); @@ -3717,7 +3717,7 @@ void mjCTendon::PointToLocal() { spec.name = (mjString)&name; spec.classname = (mjString)&classname; spec.material = (mjString)&spec_material_; - spec.userdata = (mjDouble)&spec_userdata_; + spec.userdata = (mjDoubleVec)&spec_userdata_; spec.info = (mjString)&info; } @@ -3728,7 +3728,7 @@ void mjCTendon::CopyFromSpec() { material_ = spec_material_; userdata_ = spec_userdata_; material = (mjString)&material_; - userdata = (mjDouble)&userdata_; + userdata = (mjDoubleVec)&userdata_; } @@ -4097,7 +4097,7 @@ void mjCActuator::MakePointerLocal() { spec.element = (mjElement)this; spec.name = (mjString)&name; spec.classname = (mjString)&classname; - spec.userdata = (mjDouble)&spec_userdata_; + spec.userdata = (mjDoubleVec)&spec_userdata_; spec.target = (mjString)&spec_target_; spec.refsite = (mjString)&spec_refsite_; spec.slidersite = (mjString)&spec_slidersite_; @@ -4114,7 +4114,7 @@ void mjCActuator::CopyFromSpec() { target_ = spec_target_; refsite_ = spec_refsite_; slidersite_ = spec_slidersite_; - userdata = (mjDouble)&userdata_; + userdata = (mjDoubleVec)&userdata_; target = (mjString)&target_; refsite = (mjString)&refsite_; slidersite = (mjString)&slidersite_; @@ -4359,7 +4359,7 @@ void mjCSensor::MakePointerLocal() { spec.element = (mjElement)this; spec.name = (mjString)&name; spec.classname = (mjString)&classname; - spec.userdata = (mjDouble)&spec_userdata_; + spec.userdata = (mjDoubleVec)&spec_userdata_; spec.objname = (mjString)&spec_objname_; spec.refname = (mjString)&spec_refname_; spec.plugin.name = (mjString)&plugin_name; @@ -4374,7 +4374,7 @@ void mjCSensor::CopyFromSpec() { userdata_ = spec_userdata_; objname_ = spec_objname_; refname_ = spec_refname_; - userdata = (mjDouble)&userdata_; + userdata = (mjDoubleVec)&userdata_; objname = (mjString)&objname_; refname = (mjString)&refname_; plugin.active = spec.plugin.active; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index d6ca7b7a..5265842e 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -566,7 +566,7 @@ class mjCLight : public mjCBase, private mjmLight { //------------------------- class mjCFlex ---------------------------------------------------------- // Describes a flex -class mjCFlex: public mjCBase { +class mjCFlex: public mjCBase, private mjmFlex { friend class mjCDef; friend class mjCModel; friend class mjCFlexcomp; @@ -574,41 +574,24 @@ class mjCFlex: public mjCBase { friend class mjXWriter; public: - void set_material(std::string _material) { material_ = _material; } - std::string& get_material() { return material_; } + mjmFlex spec; + using mjCBase::name; + using mjCBase::classname; + using mjCBase::info; - // contact properties - int contype; // contact type - int conaffinity; // contact affinity - int condim; // contact dimensionality - int priority; // contact priority - 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& get_vertbody() { return vertbody_; } + const std::vector& get_vert() { return vert_; } + const std::vector& get_elemaabb() { return elemaabb_; } + const std::vector& get_elem() { return elem_; } + const std::vector& get_texcoord() { return texcoord_; } - std::vector vertbody; // vertex body names - std::vector vert; // vertex positions - std::vector elemaabb; // element bounding volume - std::vector elem; // element vertex ids - std::vector texcoord; // vertex texture coordinates - - bool HasTexcoord() const; // texcoord not null - void DelTexcoord(); // delete texcoord + bool HasTexcoord() const; // texcoord not null + void DelTexcoord(); // delete texcoord private: mjCFlex(mjCModel* = 0); @@ -629,7 +612,20 @@ class mjCFlex: public mjCBase { std::vector evpair; // element-vertex pairs std::vector vertxpos; // global vertex positions mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy + std::vector elemaabb_; // element bounding volume + + // variable-size data + std::vector vertbody_; // vertex body names + std::vector vert_; // vertex positions + std::vector elem_; // element vertex ids + std::vector texcoord_; // vertex texture coordinates std::string material_; // name of material used for rendering + + std::string spec_material_; + std::vector spec_vertbody_; + std::vector spec_vert_; + std::vector spec_elem_; + std::vector spec_texcoord_; }; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 7b5f7476..ab8e5231 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1275,14 +1275,22 @@ void mjXReader::Statistic(XMLElement* section) { //---------------------------------- one-element parsers ------------------------------------------- // flex element parser -void mjXReader::OneFlex(XMLElement* elem, mjCFlex* pflex) { - string text; +void mjXReader::OneFlex(XMLElement* elem, mjmFlex* pflex) { + string text, name, classname, material; int n; // read attributes - ReadAttrTxt(elem, "name", pflex->name); + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(pflex->name, name.c_str()); + } + if (ReadAttrTxt(elem, "classname", classname)) { + mjm_setString(pflex->classname, classname.c_str()); + } + if (ReadAttrTxt(elem, "material", material)) { + mjm_setString(pflex->material, material.c_str()); + } + ReadAttr(elem, "radius", 1, &pflex->radius, text); - ReadAttrTxt(elem, "material", pflex->get_material()); ReadAttr(elem, "rgba", 4, pflex->rgba, text); if (MapValue(elem, "flatskin", &n, bool_map, 2)) { pflex->flatskin = (n==1); @@ -1292,16 +1300,22 @@ void mjXReader::OneFlex(XMLElement* elem, mjCFlex* pflex) { // read data vectors if (ReadAttrTxt(elem, "body", text, true)) { - String2Vector(text, pflex->vertbody); + mjm_setStringVec(pflex->vertbody, text.c_str()); } if (ReadAttrTxt(elem, "vertex", text)) { - String2Vector(text, pflex->vert); + std::vector vert; + String2Vector(text, vert); + mjm_setDouble(pflex->vert, vert.data(), vert.size()); } if (ReadAttrTxt(elem, "element", text, true)) { - String2Vector(text, pflex->elem); + std::vector elem; + String2Vector(text, elem); + mjm_setInt(pflex->elem, elem.data(), elem.size()); } if (ReadAttrTxt(elem, "texcoord", text)) { - String2Vector(text, pflex->texcoord); + std::vector texcoord; + String2Vector(text, texcoord); + mjm_setFloat(pflex->texcoord, texcoord.data(), texcoord.size()); } // contact subelement @@ -1331,7 +1345,9 @@ void mjXReader::OneFlex(XMLElement* elem, mjCFlex* pflex) { ReadAttr(edge, "damping", 1, &pflex->edgedamping, text); } - GetXMLPos(elem, pflex); + // write error info + mjm_setString(pflex->info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); } @@ -2400,7 +2416,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjmBody* pbody, mjCDef* def) { // make flexcomp void mjXReader::OneFlexcomp(XMLElement* elem, mjmBody* pbody) { - string text; + string text, material; int n; // create out-of-DOM element @@ -2417,14 +2433,16 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjmBody* pbody) { ReadAttr(elem, "mass", 1, &fcomp.mass, text); ReadAttr(elem, "inertiabox", 1, &fcomp.inertiabox, text); ReadAttrTxt(elem, "file", fcomp.file); - ReadAttrTxt(elem, "material", fcomp.def.flex.get_material()); - ReadAttr(elem, "rgba", 4, fcomp.def.flex.rgba, text); - if (MapValue(elem, "flatskin", &n, bool_map, 2)) { - fcomp.def.flex.flatskin = (n==1); + if (ReadAttrTxt(elem, "material", material)) { + mjm_setString(fcomp.def.flex.spec.material, material.c_str()); } - ReadAttrInt(elem, "dim", &fcomp.def.flex.dim); - ReadAttr(elem, "radius", 1, &fcomp.def.flex.radius, text); - ReadAttrInt(elem, "group", &fcomp.def.flex.group); + ReadAttr(elem, "rgba", 4, fcomp.def.flex.spec.rgba, text); + if (MapValue(elem, "flatskin", &n, bool_map, 2)) { + fcomp.def.flex.spec.flatskin = (n==1); + } + ReadAttrInt(elem, "dim", &fcomp.def.flex.spec.dim); + ReadAttr(elem, "radius", 1, &fcomp.def.flex.spec.radius, text); + ReadAttrInt(elem, "group", &fcomp.def.flex.spec.group); // pose ReadAttr(elem, "pos", 3, fcomp.pos, text); @@ -2453,28 +2471,28 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjmBody* pbody) { } ReadAttr(edge, "solref", mjNREF, fcomp.def.equality.spec.solref, text, false, false); ReadAttr(edge, "solimp", mjNIMP, fcomp.def.equality.spec.solimp, text, false, false); - ReadAttr(edge, "stiffness", 1, &fcomp.def.flex.edgestiffness, text); - ReadAttr(edge, "damping", 1, &fcomp.def.flex.edgedamping, text); + ReadAttr(edge, "stiffness", 1, &fcomp.def.flex.spec.edgestiffness, text); + ReadAttr(edge, "damping", 1, &fcomp.def.flex.spec.edgedamping, text); } // contact XMLElement* cont = FirstChildElement(elem, "contact"); if (cont) { - ReadAttrInt(cont, "contype", &fcomp.def.flex.contype); - ReadAttrInt(cont, "conaffinity", &fcomp.def.flex.conaffinity); - ReadAttrInt(cont, "condim", &fcomp.def.flex.condim); - ReadAttrInt(cont, "priority", &fcomp.def.flex.priority); - ReadAttr(cont, "friction", 3, fcomp.def.flex.friction, text, false, false); - ReadAttr(cont, "solmix", 1, &fcomp.def.flex.solmix, text); - ReadAttr(cont, "solref", mjNREF, fcomp.def.flex.solref, text, false, false); - ReadAttr(cont, "solimp", mjNIMP, fcomp.def.flex.solimp, text, false, false); - ReadAttr(cont, "margin", 1, &fcomp.def.flex.margin, text); - ReadAttr(cont, "gap", 1, &fcomp.def.flex.gap, text); + ReadAttrInt(cont, "contype", &fcomp.def.flex.spec.contype); + ReadAttrInt(cont, "conaffinity", &fcomp.def.flex.spec.conaffinity); + ReadAttrInt(cont, "condim", &fcomp.def.flex.spec.condim); + ReadAttrInt(cont, "priority", &fcomp.def.flex.spec.priority); + ReadAttr(cont, "friction", 3, fcomp.def.flex.spec.friction, text, false, false); + ReadAttr(cont, "solmix", 1, &fcomp.def.flex.spec.solmix, text); + ReadAttr(cont, "solref", mjNREF, fcomp.def.flex.spec.solref, text, false, false); + ReadAttr(cont, "solimp", mjNIMP, fcomp.def.flex.spec.solimp, text, false, false); + ReadAttr(cont, "margin", 1, &fcomp.def.flex.spec.margin, text); + ReadAttr(cont, "gap", 1, &fcomp.def.flex.spec.gap, text); if (MapValue(cont, "internal", &n, bool_map, 2)) { - fcomp.def.flex.internal = (n==1); + fcomp.def.flex.spec.internal = (n==1); } - MapValue(cont, "selfcollide", &fcomp.def.flex.selfcollide, flexself_map, 5); - ReadAttrInt(cont, "activelayers", &fcomp.def.flex.activelayers); + MapValue(cont, "selfcollide", &fcomp.def.flex.spec.selfcollide, flexself_map, 5); + ReadAttrInt(cont, "activelayers", &fcomp.def.flex.spec.activelayers); } // pin @@ -2639,6 +2657,7 @@ void mjXReader::Default(XMLElement* section, int parentid) { mjm_finalize(def->material.spec.element); mjm_finalize(def->equality.spec.element); mjm_finalize(def->tendon.spec.element); + mjm_finalize(def->flex.spec.element); // advance elem = NextSiblingElement(elem); @@ -3385,7 +3404,7 @@ void mjXReader::Deformable(XMLElement* section) { // flex sub-element if (name=="flex") { // create flex and parse - mjCFlex* pflex = model->AddFlex(); + mjmFlex* pflex = mjm_addFlex(model); OneFlex(elem, pflex); } diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index c20564e7..c3b85317 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -53,7 +53,7 @@ class mjXReader : public mjXBase { void Keyframe(tinyxml2::XMLElement* section); // keyframe section // single element parsers, used in defaults and main body - void OneFlex(tinyxml2::XMLElement* elem, mjCFlex* pflex); + void OneFlex(tinyxml2::XMLElement* elem, mjmFlex* pflex); void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh); void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin); void OneMaterial(tinyxml2::XMLElement* elem, mjmMaterial* pmaterial); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index e5b6099b..03f56732 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -100,20 +100,20 @@ void mjXWriter::OneFlex(XMLElement* elem, mjCFlex* pflex) { WriteAttrInt(elem, "group", pflex->group, defflex.group); // data vectors - if (!pflex->vertbody.empty()) { - Vector2String(text, pflex->vertbody); + if (!pflex->get_vertbody().empty()) { + Vector2String(text, pflex->get_vertbody()); WriteAttrTxt(elem, "body", text); } - if (!pflex->vert.empty()) { - Vector2String(text, pflex->vert); + if (!pflex->get_vert().empty()) { + Vector2String(text, pflex->get_vert()); WriteAttrTxt(elem, "vertex", text); } - if (!pflex->elem.empty()) { - Vector2String(text, pflex->elem); + if (!pflex->get_elem().empty()) { + Vector2String(text, pflex->get_elem()); WriteAttrTxt(elem, "element", text); } - if (!pflex->texcoord.empty()) { - Vector2String(text, pflex->texcoord); + if (!pflex->get_texcoord().empty()) { + Vector2String(text, pflex->get_texcoord()); WriteAttrTxt(elem, "texcoord", text); }