From b224367a9b5de430e868c0215a56034ce998b87e Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 12 Feb 2024 08:46:57 -0800 Subject: [PATCH 01/38] 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); } From 72cf304e2bc39c8eb77b10f1be0da30b003d81c6 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 12 Feb 2024 09:03:20 -0800 Subject: [PATCH 02/38] Improve documentation of mesh pre-processing. PiperOrigin-RevId: 606268723 Change-Id: I1d939cbb214036ab477e12ae3fd266a25ff89399 --- doc/XMLreference.rst | 39 ++++++++++++++++++++------------------- 1 file changed, 20 insertions(+), 19 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index eeb855d2..483c21ab 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1218,40 +1218,41 @@ construct simple shapes directly in the XML. For example, a pyramid can be creat .. code-block:: xml - + -Positioning and orienting is complicated by the fact that vertex data are often designed relative to coordinate frames -whose origin is not inside the mesh. In contrast, MuJoCo expects the origin of a geom's local frame to coincide with the -geometric center of the shape. We resolve this discrepancy by pre-processing the mesh in the compiler, so that it is -centered around (0,0,0) and its principal axes of inertia are the coordinate axes. We also save the translation and -rotation offsets needed to achieve such alignment in :ref:`mjModel.mesh_pos` and -:ref:`mjModel.mesh_quat`. These offsets are then applied to the referencing geom's position and orientation; see -also :at:`mesh` attribute of :ref:`geom ` below. Fortunately most meshes used in robot models are designed in -a coordinate frame centered at the joint. This makes the corresponding MJCF model intuitive: we set the body frame at the -joint, so that the joint position is (0,0,0) in the body frame, and simply reference the mesh. Below is an MJCF model -fragment of a forearm, containing all the information needed to put the mesh where one would expect it to be. The body -position is specified relative to the parent body, namely the upper arm (not shown). It is offset by 35 cm which is the -typical length of the human upper arm. If the mesh vertex data were not designed in the above convention, we would have -to use the geom position and orientation (or the new refpos, refquat mechanism) to compensate, but in practice this is +Positioning and orienting is complicated by the fact that vertex data in the source asset are often relative to +coordinate frames whose origin is not inside the mesh. In contrast, MuJoCo expects the origin of a geom's local frame to +coincide with the geometric center of the shape. We resolve this discrepancy by pre-processing the mesh in the compiler, +so that it is centered around (0,0,0) and its principal axes of inertia are the coordinate axes. We save the translation +and rotation offsets applied to the source asset in :ref:`mjModel.mesh_pos` and +:ref:`mjModel.mesh_quat`; these are required if one reads vertex data from the source and needs to re-apply the +transform. These offsets are then composed with the referencing geom's position and orientation; see also the :at:`mesh` +attribute of :ref:`geom ` below. Fortunately most meshes used in robot models are designed in a coordinate +frame centered at the joint. This makes the corresponding MJCF model intuitive: we set the body frame at the joint, so +that the joint position is (0,0,0) in the body frame, and simply reference the mesh. Below is an MJCF model fragment of +a forearm, containing all the information needed to put the mesh where one would expect it to be. The body position is +specified relative to the parent body, namely the upper arm (not shown). It is offset by 35 cm which is the typical +length of the human upper arm. If the mesh vertex data were not designed in the above convention, we would have to use +the geom position and orientation (or the :at:`refpos`, :at:`refquat`` mechanism) to compensate, but in practice this is rarely needed. .. code-block:: xml - + - - + + The inertial computation mentioned above is part of an algorithm used not only to center and align the mesh, but also to infer the mass and inertia of the body to which it is attached. This is done by computing the centroid of the triangle faces, connecting each face with the centroid to form a triangular pyramid, computing the mass and signed inertia of all -pyramids (considered solid or hollow if :at:`shellinertia` is true) and accumulating them. The sign ensures that -pyramids on the outside of the surfaces are subtracted, as it can occur with concave geometries. This algorithm can be +pyramids (considered solid, or hollow if :at:`shellinertia` is true) and accumulating them. The sign ensures that +pyramids on the outside of the surfaces are subtracted, as can occur with concave geometries. This algorithm can be found in section 1.3.8 of Computational Geometry in C (Second Edition) by Joseph O'Rourke. The full list of processing steps applied by the compiler to each mesh is as follows: From 13d6eeacdd258d17a9d671a055f257003133e48b Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 12 Feb 2024 09:24:39 -0800 Subject: [PATCH 03/38] Add mjmPair to C API. PiperOrigin-RevId: 606274243 Change-Id: I15e2a38096a61bc1377a666c96113ae20ce3ed59 --- src/user/user_api.cc | 12 ++++++- src/user/user_api.h | 25 +++++++++++++++ src/user/user_init.cc | 14 +++++++++ src/user/user_objects.cc | 61 ++++++++++++++++++++++++------------ src/user/user_objects.h | 28 ++++++++++------- src/xml/xml_native_reader.cc | 29 +++++++++++------ src/xml/xml_native_reader.h | 2 +- src/xml/xml_native_writer.cc | 18 +++++------ 8 files changed, 137 insertions(+), 52 deletions(-) diff --git a/src/user/user_api.cc b/src/user/user_api.cc index f29a7429..2aeea3d6 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -137,7 +137,7 @@ mjmFrame* mjm_addFrame(mjmBody* bodyspec, mjmFrame* parentframe) { -// Add material to model. +// add material to model mjmMaterial* mjm_addMaterial(void* model, void* defspec) { mjCModel* modelC = static_cast(model); mjCDef* def = static_cast(defspec); @@ -147,6 +147,16 @@ mjmMaterial* mjm_addMaterial(void* model, void* defspec) { +// add pair to model +mjmPair* mjm_addPair(void* model, void* defspec) { + mjCModel* modelC = static_cast(model); + mjCDef* def = static_cast(defspec); + mjCPair* pair = modelC->AddPair(def); + return &pair->spec; +} + + + // add equality to model mjmEquality* mjm_addEquality(void* model, void* defspec) { mjCModel* modelC = static_cast(model); diff --git a/src/user/user_api.h b/src/user/user_api.h index d904f0d4..c2327f5a 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -325,6 +325,25 @@ typedef struct _mjmMaterial { // material specification } mjmMaterial; +typedef struct _mjmPair { + mjElement element; // internal, do not modify + mjString name; // name + mjString classname; // class name + mjString geomname1; // name of geom 1 + mjString geomname2; // name of geom 2 + + // optional parameters: computed from geoms if not set by user + int condim; // contact dimensionality + mjtNum solref[mjNREF]; // solver reference, normal direction + mjtNum solreffriction[mjNREF]; // solver reference, frictional directions + mjtNum solimp[mjNIMP]; // solver impedance + double margin; // margin for contact detection + double gap; // include in solver if distpair.CopyFromSpec(); *this = _def->pair; } // set model, def model = _model; def = (_def ? _def : (_model ? _model->defaults[0] : 0)); + + // point to local (needs to be after defaults) + PointToLocal(); + + // in case this camera is not compiled + CopyFromSpec(); +} + + + +void mjCPair::PointToLocal() { + spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.classname = (mjString)&classname; + spec.geomname1 = (mjString)&spec_geomname1_; + spec.geomname2 = (mjString)&spec_geomname2_; + spec.info = (mjString)&info; +} + + + +void mjCPair::CopyFromSpec() { + *static_cast(this) = spec; + geomname1_ = spec_geomname1_; + geomname2_ = spec_geomname2_; + geomname1 = (mjString)&geomname1_; + geomname2 = (mjString)&geomname2_; } // compiler void mjCPair::Compile(void) { + CopyFromSpec(); + // check condim if (condim!=1 && condim!=3 && condim!=4 && condim!=6) { throw mjCError(this, "invalid condim in collision %d", "", id); } // find geom 1 - geom1 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname1); + geom1 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname1_); if (!geom1) { - throw mjCError(this, "geom '%s' not found in collision %d", geomname1.c_str(), id); + throw mjCError(this, "geom '%s' not found in collision %d", geomname1_.c_str(), id); } // find geom 2 - geom2 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname2); + geom2 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname2_); if (!geom2) { - throw mjCError(this, "geom '%s' not found in collision %d", geomname2.c_str(), id); + throw mjCError(this, "geom '%s' not found in collision %d", geomname2_.c_str(), id); } // mark geoms as not visual @@ -3398,9 +3419,9 @@ void mjCPair::Compile(void) { // swap if body1 > body2 if (geom1->body->id > geom2->body->id) { - string nametmp = geomname1; - geomname1 = geomname2; - geomname2 = nametmp; + string nametmp = geomname1_; + geomname1_ = geomname2_; + geomname2_ = nametmp; mjCGeom* geomtmp = geom1; geom1 = geom2; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 5265842e..47f4398d 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -966,24 +966,23 @@ class mjCMaterial : public mjCBase, private mjmMaterial { //------------------------- class mjCPair ---------------------------------------------------------- // Predefined geom pair for collision detection -class mjCPair : public mjCBase { +class mjCPair : public mjCBase, private mjmPair { friend class mjCDef; friend class mjCBody; friend class mjCModel; + friend class mjXWriter; public: - // parameters set by user - std::string geomname1; // name of geom 1 - std::string geomname2; // name of geom 2 + mjmPair spec; + using mjCBase::name; + using mjCBase::classname; + using mjCBase::info; - // optional parameters: computed from geoms if not set by user - int condim; // contact dimensionality - mjtNum solref[mjNREF]; // solver reference, normal direction - mjtNum solreffriction[mjNREF]; // solver reference, frictional directions - mjtNum solimp[mjNIMP]; // solver impedance - double margin; // margin for contact detection - double gap; // include in solver if distclassname); - ReadAttrTxt(elem, "geom1", ppair->geomname1, true); - ReadAttrTxt(elem, "geom2", ppair->geomname2, true); + if (ReadAttrTxt(elem, "class", classname)) { + mjm_setString(ppair->classname, classname.c_str()); + } + if (ReadAttrTxt(elem, "geom1", geomname1)) { + mjm_setString(ppair->geomname1, geomname1.c_str()); + } + if (ReadAttrTxt(elem, "geom2", geomname2)) { + mjm_setString(ppair->geomname2, geomname2.c_str()); + } } // read other parameters - ReadAttrTxt(elem, "name", ppair->name); + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(ppair->name, name.c_str()); + } ReadAttrInt(elem, "condim", &ppair->condim); ReadAttr(elem, "solref", mjNREF, ppair->solref, text, false, false); ReadAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, text, false, false); @@ -1771,7 +1779,9 @@ void mjXReader::OnePair(XMLElement* elem, mjCPair* ppair) { ReadAttr(elem, "gap", 1, &ppair->gap, text); ReadAttr(elem, "friction", 5, ppair->friction, text, false, false); - GetXMLPos(elem, ppair); + // write error info + mjm_setString(ppair->info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); } @@ -2626,7 +2636,7 @@ void mjXReader::Default(XMLElement* section, int parentid) { else if (name=="light") OneLight(elem, &def->light.spec); // read pair - else if (name=="pair") OnePair(elem, &def->pair); + else if (name=="pair") OnePair(elem, &def->pair.spec); // read equality else if (name=="equality") OneEquality(elem, &def->equality.spec); @@ -2658,6 +2668,7 @@ void mjXReader::Default(XMLElement* section, int parentid) { mjm_finalize(def->equality.spec.element); mjm_finalize(def->tendon.spec.element); mjm_finalize(def->flex.spec.element); + mjm_finalize(def->pair.spec.element); // advance elem = NextSiblingElement(elem); @@ -3338,7 +3349,7 @@ void mjXReader::Contact(XMLElement* section) { // geom pair to include if (name=="pair") { // create pair and parse - mjCPair* ppair = model->AddPair(def); + mjmPair* ppair = mjm_addPair(model, def); OnePair(elem, ppair); } diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index c3b85317..459d0a06 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -62,7 +62,7 @@ class mjXReader : public mjXBase { void OneSite(tinyxml2::XMLElement* elem, mjmSite& site); void OneCamera(tinyxml2::XMLElement* elem, mjmCamera* pcamera); void OneLight(tinyxml2::XMLElement* elem, mjmLight* plight); - void OnePair(tinyxml2::XMLElement* elem, mjCPair* ppair); + void OnePair(tinyxml2::XMLElement* elem, mjmPair* ppair); void OneEquality(tinyxml2::XMLElement* elem, mjmEquality* pequality); void OneTendon(tinyxml2::XMLElement* elem, mjmTendon* ptendon); void OneActuator(tinyxml2::XMLElement* elem, mjmActuator* pactuator); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 03f56732..5fe4965f 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -523,19 +523,19 @@ void mjXWriter::OnePair(XMLElement* elem, mjCPair* ppair, mjCDef* def) { // regular if (!writingdefaults) { WriteAttrTxt(elem, "class", ppair->classname); - WriteAttrTxt(elem, "geom1", ppair->geomname1); - WriteAttrTxt(elem, "geom2", ppair->geomname2); + WriteAttrTxt(elem, "geom1", ppair->get_geomname1()); + WriteAttrTxt(elem, "geom2", ppair->get_geomname2()); } // defaults and regular WriteAttrTxt(elem, "name", ppair->name); - WriteAttrInt(elem, "condim", ppair->condim, def->pair.condim); - WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.margin); - WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.gap); - WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.solref); - WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.solreffriction); - WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.solimp); - WriteAttr(elem, "friction", 5, ppair->friction, def->pair.friction); + WriteAttrInt(elem, "condim", ppair->condim, def->pair.spec.condim); + WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.spec.margin); + WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.spec.gap); + WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.spec.solref); + WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.spec.solreffriction); + WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.spec.solimp); + WriteAttr(elem, "friction", 5, ppair->friction, def->pair.spec.friction); } From b6873ec8a2e60f173422029906f9b47d4b48afa9 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 12 Feb 2024 09:36:50 -0800 Subject: [PATCH 04/38] Move `mjModel.mesh_{pos,quat}` before `mesh_pathadr` so all attributes related to original asset data are next to each other. PiperOrigin-RevId: 606277473 Change-Id: Ia4f6df552192c8a2847126975b6a1a31df63cf65 --- doc/includes/references.h | 4 ++-- include/mujoco/mjmodel.h | 4 ++-- introspect/structs.py | 28 ++++++++++++++-------------- unity/Runtime/Bindings/MjBindings.cs | 4 ++-- 4 files changed, 20 insertions(+), 20 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index b94d5d71..913d2465 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1154,8 +1154,6 @@ struct mjModel_ { int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1) int* mesh_texcoordnum; // number of texcoord (nmesh x 1) int* mesh_graphadr; // graph data address; -1: no graph (nmesh x 1) - mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3) - mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4) float* mesh_vert; // vertex positions for all meshes (nmeshvert x 3) float* mesh_normal; // normals for all meshes (nmeshnormal x 3) float* mesh_texcoord; // vertex texcoords for all meshes (nmeshtexcoord x 2) @@ -1163,6 +1161,8 @@ struct mjModel_ { int* mesh_facenormal; // normal face data (nmeshface x 3) int* mesh_facetexcoord; // texture face data (nmeshface x 3) int* mesh_graph; // convex graph data (nmeshgraph x 1) + mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3) + mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4) int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1) // skins diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index b91ee594..a5bdca11 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -868,8 +868,6 @@ struct mjModel_ { int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1) int* mesh_texcoordnum; // number of texcoord (nmesh x 1) int* mesh_graphadr; // graph data address; -1: no graph (nmesh x 1) - mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3) - mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4) float* mesh_vert; // vertex positions for all meshes (nmeshvert x 3) float* mesh_normal; // normals for all meshes (nmeshnormal x 3) float* mesh_texcoord; // vertex texcoords for all meshes (nmeshtexcoord x 2) @@ -877,6 +875,8 @@ struct mjModel_ { int* mesh_facenormal; // normal face data (nmeshface x 3) int* mesh_facetexcoord; // texture face data (nmeshface x 3) int* mesh_graph; // convex graph data (nmeshgraph x 1) + mjtNum* mesh_pos; // translation applied to asset vertices (nmesh x 3) + mjtNum* mesh_quat; // rotation applied to asset vertices (nmesh x 4) int* mesh_pathadr; // address of asset path for mesh; -1: none (nmesh x 1) // skins diff --git a/introspect/structs.py b/introspect/structs.py index 8f44927d..fee5933b 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -2591,20 +2591,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='graph data address; -1: no graph (nmesh x 1)', ), - StructFieldDecl( - name='mesh_pos', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - doc='translation applied to asset vertices (nmesh x 3)', - ), - StructFieldDecl( - name='mesh_quat', - type=PointerType( - inner_type=ValueType(name='mjtNum'), - ), - doc='rotation applied to asset vertices (nmesh x 4)', - ), StructFieldDecl( name='mesh_vert', type=PointerType( @@ -2654,6 +2640,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='convex graph data (nmeshgraph x 1)', # pylint: disable=line-too-long ), + StructFieldDecl( + name='mesh_pos', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='translation applied to asset vertices (nmesh x 3)', + ), + StructFieldDecl( + name='mesh_quat', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='rotation applied to asset vertices (nmesh x 4)', + ), StructFieldDecl( name='mesh_pathadr', type=PointerType( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 48816d2d..a5a1219e 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5365,8 +5365,6 @@ public unsafe struct mjModel_ { public int* mesh_texcoordadr; public int* mesh_texcoordnum; public int* mesh_graphadr; - public double* mesh_pos; - public double* mesh_quat; public float* mesh_vert; public float* mesh_normal; public float* mesh_texcoord; @@ -5374,6 +5372,8 @@ public unsafe struct mjModel_ { public int* mesh_facenormal; public int* mesh_facetexcoord; public int* mesh_graph; + public double* mesh_pos; + public double* mesh_quat; public int* mesh_pathadr; public int* skin_matid; public int* skin_group; From eabd4c06f9b10fd49fb9a6262ca1fb16f5c652ac Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 12 Feb 2024 09:48:44 -0800 Subject: [PATCH 05/38] Move schema definition validation from runtime to a test. PiperOrigin-RevId: 606280564 Change-Id: I9fec9eb3538f63f84e1982f21e1a93f16342d540 --- src/xml/xml_native_reader.cc | 9 +-- src/xml/xml_native_reader.h | 15 +++- src/xml/xml_util.cc | 75 +++---------------- src/xml/xml_util.h | 2 +- test/xml/xml_schema_test.cc | 135 +++++++++++++++++++++++++++++++++++ 5 files changed, 157 insertions(+), 79 deletions(-) create mode 100644 test/xml/xml_schema_test.cc diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 36b39f99..c307dc1e 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -81,8 +81,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) { //---------------------------------- MJCF schema --------------------------------------------------- -static const int nMJCF = 227; -static const char* MJCF[nMJCF][mjXATTRNUM] = { +const char* MJCF[nMJCF][mjXATTRNUM] = { {"mujoco", "!", "1", "model"}, {"<"}, {"compiler", "*", "20", "autolimits", "boundmass", "boundinertia", "settotalmass", @@ -774,12 +773,6 @@ const mjMap flexself_map[5] = { // constructor mjXReader::mjXReader() : schema(MJCF, nMJCF) { - // check for schema construction error - if (!schema.GetError().empty()) { - throw mjXError(0, "Schema construction error: %s", - schema.GetError().c_str()); - } - readingdefaults = false; } diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 459d0a06..43b0caab 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -17,10 +17,15 @@ #include -#include "user/user_model.h" -#include "xml/xml_base.h" #include "tinyxml2.h" +#include +#include "user/user_api.h" +#include "user/user_model.h" +#include "user/user_objects.h" +#include "xml/xml_base.h" +#include "xml/xml_util.h" + class mjXReader : public mjXBase { public: mjXReader(); // constructor @@ -74,7 +79,11 @@ class mjXReader : public mjXBase { mjCDef* GetClass(tinyxml2::XMLElement* section); // get default class name static void GetXMLPos(tinyxml2::XMLElement* elem, mjCBase* obj); // get xml position - bool readingdefaults; // true while reading defaults + bool readingdefaults; // true while reading defaults }; +// MJCF schema +#define nMJCF 227 +extern const char* MJCF[nMJCF][mjXATTRNUM]; + #endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_ diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc index a58487d6..df80e533 100644 --- a/src/xml/xml_util.cc +++ b/src/xml/xml_util.cc @@ -157,56 +157,7 @@ XMLElement* NextSiblingElement(XMLElement* e, const char* name) { } // constructor -mjXSchema::mjXSchema(const char* schema[][mjXATTRNUM], unsigned nrow, bool checkptr) { - if (schema[0][0][0] == '<' || schema[0][0][0] == '>') { - throw "expected element, found bracket"; - } - - // check entire schema for null pointers - if (checkptr) { - char msg[100]; - - for (int i=0; i mjXATTRNUM-3) { - mju::sprintf_arr(msg, - "invalid number of attributes in row %d, element %s", i, schema[i][0]); - throw msg; - } - - // attribute pointers - for (int j=0; j1) { - // check for bracketed block - if (schema[1][0][0]!='<' || schema[nrow-1][0][0]!='>') { - throw "expected brackets after complex element"; - } - + if (nrow > 1) { // parse block into simple and complex elements, create children int start = 2; while (start < nrow-1) { int end = start; // look for bracketed block at start+1 - if (schema[start+1][0][0]=='<') { + if (schema[start+1][0][0] == '<') { // look for corresponding closing bracket int cnt = 0; while (end <= nrow-1) { - if (schema[end][0][0]=='<') { + if (schema[end][0][0] == '<') { cnt++; - } else if (schema[end][0][0]=='>') { + } else if (schema[end][0][0] == '>') { cnt--; - if (cnt==0) { + if (cnt == 0) { break; } } end++; } - - // closing bracket not found - if (end > nrow-1) { - throw "matching closing bracket not found"; - } } - // add element, check for error - subschema_.emplace_back(schema+start, end-start+1, false); + // add child element + subschema_.emplace_back(schema+start, end-start+1); // proceed with next subelement start = end+1; diff --git a/src/xml/xml_util.h b/src/xml/xml_util.h index 43f71290..875eb5a4 100644 --- a/src/xml/xml_util.h +++ b/src/xml/xml_util.h @@ -56,7 +56,7 @@ class [[nodiscard]] mjXError { // Custom XML file validation class mjXSchema { public: - mjXSchema(const char* schema[][mjXATTRNUM], unsigned nrow, bool checkptr = true); + mjXSchema(const char* schema[][mjXATTRNUM], unsigned nrow); std::string GetError(); // return error void Print(std::stringstream& str, int level) const; // print schema diff --git a/test/xml/xml_schema_test.cc b/test/xml/xml_schema_test.cc new file mode 100644 index 00000000..6b92747e --- /dev/null +++ b/test/xml/xml_schema_test.cc @@ -0,0 +1,135 @@ +// Copyright 2024 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include "src/xml/xml_native_reader.h" +#include "src/xml/xml_util.h" +#include "test/fixture.h" +#include +#include + +namespace mujoco { +namespace { + + +using XMLSchemaTest = MujocoTest; + +static std::optional IsValidSchemaFormat( + const char* schema[][mjXATTRNUM], unsigned nrow) { + if (schema[0][0][0] == '<' || schema[0][0][0] == '>') { + return "expected element, found bracket"; + } + + // check entire schema for null pointers + for (int i = 0; i < nrow; i++) { + // base pointers + if (!schema[i][0]) { + return "null pointer found in row " + std::to_string(i); + } + + // detect element + if (schema[i][0][0] != '<' && schema[i][0][0] != '>') { + // first 3 pointers required + if (!schema[i][1] || !schema[i][2]) { + return absl::StrFormat("expected element, found null pointers" + "in row %d, element %s", i, schema[i][0]); + } + + // check type + if (schema[i][1][0] != '!' && schema[i][1][0] != '?' && + schema[i][1][0] != '*' && schema[i][1][0] != 'R') { + return absl::StrFormat("invalid type in row %d, element %s", + i, schema[i][0]); + } + + // number of attributes + int nattr = 0; + if (!absl::SimpleAtoi(schema[i][2], &nattr)) { + return absl::StrFormat("unparseable number of attributes in" + " row %d, element %s", i, schema[i][0]); + } else if (nattr < 0 || nattr > mjXATTRNUM - 3) { + return absl::StrFormat("invalid number of attributes in" + " row %d, element %s", i, schema[i][0]); + } + + // attribute pointers + for (int j = 0; j < nattr; j++) { + if (!schema[i][3 + j]) { + return absl::StrFormat("null attribute %d in" + " row %d, element %s", j, i, schema[i][0]); + } + } + } + } + + // process sub-elements of complex element + if (nrow > 1) { + // check for bracketed block + if (schema[1][0][0] != '<' || schema[nrow - 1][0][0] != '>') { + return "expected brackets after complex element"; + } + + // parse block into simple and complex elements + int start = 2; + while (start < nrow - 1) { + int end = start; + + // look for bracketed block at start + 1 + if (schema[start + 1][0][0] == '<') { + // look for corresponding closing bracket + int cnt = 0; + while (end <= nrow - 1) { + if (schema[end][0][0] == '<') { + cnt++; + } else if (schema[end][0][0] == '>') { + cnt--; + if (cnt == 0) { + break; + } + } + end++; + } + + // closing bracket not found + if (end > nrow - 1) { + return "matching closing bracket not found"; + } + } + + // recursively check for error + auto error = IsValidSchemaFormat(schema + start, end - start + 1); + if (error.has_value()) { + return error; + } + + // proceed with next subelement + start = end + 1; + } + } + return std::nullopt; +} + +TEST_F(XMLSchemaTest, MjcfSchemaTest) { + auto error = IsValidSchemaFormat(MJCF, nMJCF); + auto error_msg = error.value_or(""); + + EXPECT_EQ(error_msg, ""); + ASSERT_FALSE(error.has_value()); +} + +} // namespace +} // namespace mujoco From 805d862d5feffe5a4449afc0605b990baf7aab07 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Mon, 12 Feb 2024 14:31:16 -0800 Subject: [PATCH 06/38] Fix manifold point function. PiperOrigin-RevId: 606366838 Change-Id: I8cf031f6f0fd0173777e0a9bb6d11c0c77c617ec --- mjx/mujoco/mjx/_src/collision_convex.py | 3 +- mjx/mujoco/mjx/_src/collision_driver_test.py | 32 ++++++++++++++++++-- 2 files changed, 32 insertions(+), 3 deletions(-) diff --git a/mjx/mujoco/mjx/_src/collision_convex.py b/mjx/mujoco/mjx/_src/collision_convex.py index 62d357f9..21197c15 100644 --- a/mjx/mujoco/mjx/_src/collision_convex.py +++ b/mjx/mujoco/mjx/_src/collision_convex.py @@ -173,7 +173,7 @@ def _manifold_points( bp = b - poly dist_bp = jp.abs(bp.dot(bc)) + dist_mask dist_ap = jp.abs(ap.dot(ac)) + dist_mask - d_idx = jp.concatenate([dist_bp, dist_ap]).argmax() % poly.shape[0] + d_idx = (dist_bp + dist_ap).argmax() % poly.shape[0] return jp.array([a_idx, b_idx, c_idx, d_idx]) @@ -514,6 +514,7 @@ def plane_convex(plane: GeomInfo, convex: GeomInfo) -> Contact: frame = jp.stack([math.make_frame(n)] * 4, axis=0) unique = jp.tril(idx == idx[:, None]).sum(axis=1) == 1 dist = jp.where(unique, -support[idx], 1) + pos = pos - 0.5 * dist[:, None] * n return dist, pos, frame diff --git a/mjx/mujoco/mjx/_src/collision_driver_test.py b/mjx/mujoco/mjx/_src/collision_driver_test.py index 5b58f7f8..f0bca5e9 100644 --- a/mjx/mujoco/mjx/_src/collision_driver_test.py +++ b/mjx/mujoco/mjx/_src/collision_driver_test.py @@ -265,7 +265,7 @@ class ConvexTest(absltest.TestCase): - + @@ -281,7 +281,35 @@ class ConvexTest(absltest.TestCase): # extract the contact points with penetration c = jax.tree_map(lambda x: jp.take(x, jp.array([0, 1]), axis=0), dx.contact) for field in dataclasses.fields(Contact): - _assert_attr_eq(c, d.contact, field.name, 'box_plane', 1e-2) + _assert_attr_eq(c, d.contact, field.name, 'box_plane', 1e-5) + + _FLAT_BOX_PLANE = """ + + + + + + + + + + """ + + def test_flat_box_plane(self): + """Tests box collision with a plane.""" + with jax.disable_jit(): + d, dx = _collide(self._FLAT_BOX_PLANE) + + np.testing.assert_array_less(dx.contact.dist, 0) + + # sort positions for comparison + idx = np.lexsort((dx.contact.pos[:, 0], dx.contact.pos[:, 1])) + dx = dx.tree_replace({'contact.pos': dx.contact.pos[idx]}) + idx = np.lexsort((d.contact.pos[:, 0], d.contact.pos[:, 1])) + d.contact.pos[:] = d.contact.pos[idx] + + for field in dataclasses.fields(Contact): + _assert_attr_eq(dx.contact, d.contact, field.name, 'flat_box_plane', 1e-5) _BOX_BOX = """ From 47bb4a828e55d97763abc248b6e47384cb8912a0 Mon Sep 17 00:00:00 2001 From: Erik Frey Date: Mon, 12 Feb 2024 15:45:25 -0800 Subject: [PATCH 07/38] Fixes to the MJX Euler integrator and Newton solver, and test improvements to catch future bugs like this. PiperOrigin-RevId: 606389094 Change-Id: I50d0143f338fe29a823c930dba82995ce8a64ce2 --- doc/changelog.rst | 2 + mjx/mujoco/mjx/_src/forward.py | 6 ++- mjx/mujoco/mjx/_src/forward_test.py | 47 ++++++++++++----- mjx/mujoco/mjx/_src/solver.py | 12 ++--- mjx/mujoco/mjx/_src/solver_test.py | 66 +++++++++++++++--------- mjx/mujoco/mjx/test_data/constraints.xml | 2 +- 6 files changed, 89 insertions(+), 46 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 55cf4d2d..14608cee 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -16,6 +16,8 @@ MJX - Avoid calling ``mjx.ncon`` in ``mjx.get_data_into`` when ``nc`` can be derived from ``mjx.Data``. 2. Fixed a bug in ``mjx-viewer`` that prevented it from running. Updated ``mjx-viewer`` to use newer ``mjx.get_data_into`` function call. +3. Fixed a bug in ``mjx.euler`` that applied incorrect damping when using dense mass matrices. +4. Fixed a bug in ``mjx.solve`` that was causing slow convergence when using ``mjSOL_NEWTON`` in :ref:`mjtSolver`. Version 3.1.2 (February 05, 2024) ----------------------------------- diff --git a/mjx/mujoco/mjx/_src/forward.py b/mjx/mujoco/mjx/_src/forward.py index 682cc88e..c6bbdbc1 100644 --- a/mjx/mujoco/mjx/_src/forward.py +++ b/mjx/mujoco/mjx/_src/forward.py @@ -287,8 +287,10 @@ def euler(m: Model, d: Data) -> Data: # integrate damping implicitly qacc = d.qacc if not m.opt.disableflags & DisableBit.EULERDAMP: - # TODO(robotics-simulation): can this be done with a smaller perf hit - dh = d.replace(qM=d.qM.at[m.dof_Madr].add(m.opt.timestep * m.dof_damping)) + if support.is_sparse(m): + dh = d.replace(qM=d.qM.at[m.dof_Madr].add(m.opt.timestep * m.dof_damping)) + else: + dh = d.replace(qM=d.qM + jp.diag(m.opt.timestep * m.dof_damping)) dh = smooth.factor_m(m, dh) qfrc = d.qfrc_smooth + d.qfrc_constraint qacc = smooth.solve_m(m, dh, qfrc) diff --git a/mjx/mujoco/mjx/_src/forward_test.py b/mjx/mujoco/mjx/_src/forward_test.py index cc767ae8..0a777797 100644 --- a/mjx/mujoco/mjx/_src/forward_test.py +++ b/mjx/mujoco/mjx/_src/forward_test.py @@ -27,14 +27,14 @@ import numpy as np _TOLERANCE = 1e-5 -def _assert_eq(a, b, name): - tol = _TOLERANCE * 10 # avoid test noise +def _assert_eq(a, b, name, tol=_TOLERANCE): + tol = tol * 10 # avoid test noise err_msg = f'mismatch: {name}' np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) -def _assert_attr_eq(a, b, attr): - _assert_eq(getattr(a, attr), getattr(b, attr), attr) +def _assert_attr_eq(a, b, attr, tol=_TOLERANCE): + _assert_eq(getattr(a, attr), getattr(b, attr), attr, tol=tol) class ForwardTest(absltest.TestCase): @@ -46,7 +46,7 @@ class ForwardTest(absltest.TestCase): d.ctrl = np.array([-18, 0.59, 0.47]) d.xfrc_applied[0, 2] = 0.1 # torque d.xfrc_applied[1, 4] = 0.3 # linear force - mujoco.mj_step(m, d, 100) # get some dynamics going + mujoco.mj_step(m, d, 20) # get some dynamics going mujoco.mj_forward(m, d) mx = mjx.put_model(m) @@ -75,14 +75,13 @@ class ForwardTest(absltest.TestCase): d.ctrl = np.array([-18, 0.59, 0.47]) d.xfrc_applied[0, 2] = 0.1 # torque d.xfrc_applied[1, 4] = 0.3 # linear force - mujoco.mj_step(m, d, 100) # get some dynamics going + mujoco.mj_step(m, d, 20) # get some dynamics going - mx = mjx.put_model(m) - dx = jax.jit(mjx.step)(mx, mjx.put_data(m, d)) + dx = jax.jit(mjx.step)(mjx.put_model(m), mjx.put_data(m, d)) mujoco.mj_step(m, d) _assert_attr_eq(d, dx, 'act') _assert_attr_eq(d, dx, 'time') - _assert_attr_eq(d, dx, 'qvel') + _assert_attr_eq(d, dx, 'qvel', tol=5e-4) _assert_attr_eq(d, dx, 'qpos') def test_rk4(self): @@ -111,8 +110,7 @@ class ForwardTest(absltest.TestCase): mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero mujoco.mj_forward(m, d) - mx = mjx.put_model(m) - dx = jax.jit(mjx.rungekutta4)(mx, mjx.put_data(m, d)) + dx = jax.jit(mjx.rungekutta4)(mjx.put_model(m), mjx.put_data(m, d)) mujoco.mj_RungeKutta(m, d, 4) _assert_attr_eq(d, dx, 'qvel') @@ -120,6 +118,30 @@ class ForwardTest(absltest.TestCase): _assert_attr_eq(d, dx, 'act') _assert_attr_eq(d, dx, 'time') + def test_eulerdamp(self): + m = test_util.load_test_file('pendula.xml') + self.assertTrue((m.dof_damping > 0).any()) + + d = mujoco.MjData(m) + d.qvel[:] = 1.0 + d.qacc[:] = 1.0 + mujoco.mj_forward(m, d) + dx = jax.jit(mjx.euler)(mjx.put_model(m), mjx.put_data(m, d)) + mujoco.mj_Euler(m, d) + + _assert_attr_eq(d, dx, 'qpos') + + # also test sparse + m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE + d = mujoco.MjData(m) + d.qvel[:] = 1.0 + d.qacc[:] = 1.0 + mujoco.mj_forward(m, d) + dx = jax.jit(mjx.euler)(mjx.put_model(m), mjx.put_data(m, d)) + mujoco.mj_Euler(m, d) + + _assert_attr_eq(d, dx, 'qpos') + def test_disable_eulerdamp(self): m = test_util.load_test_file('pendula.xml') self.assertTrue((m.dof_damping > 0).any()) @@ -128,8 +150,7 @@ class ForwardTest(absltest.TestCase): d = mujoco.MjData(m) d.qvel[:] = 1.0 d.qacc[:] = 1.0 - mx = mjx.put_model(m) - dx = jax.jit(mjx.euler)(mx, mjx.put_data(m, d)) + dx = jax.jit(mjx.euler)(mjx.put_model(m), mjx.put_data(m, d)) np.testing.assert_allclose(dx.qvel, 1 + m.opt.timestep) diff --git a/mjx/mujoco/mjx/_src/solver.py b/mjx/mujoco/mjx/_src/solver.py index cd0cc63c..dae5a82b 100644 --- a/mjx/mujoco/mjx/_src/solver.py +++ b/mjx/mujoco/mjx/_src/solver.py @@ -218,13 +218,13 @@ def _update_gradient(m: Model, d: Data, ctx: _Context) -> _Context: mgrad = smooth.solve_m(m, d, grad) elif m.opt.solver == SolverType.NEWTON: ne, nf, *_ = constraint.count_constraints(m) - active = (ctx.Jaref < 0).at[:ne + nf].set(True) - h = d.qM + support.make_m(m, d.efc_J.T * d.efc_D * active, d.efc_J.T) - dh = d.replace(qM=h) - dh = smooth.factor_m(m, dh) - mgrad = smooth.solve_m(m, dh, grad) + active = (ctx.Jaref < 0).at[: ne + nf].set(True) + h = (d.efc_J.T * d.efc_D * active) @ d.efc_J + h = support.full_m(m, d) + h + h_ = jax.scipy.linalg.cho_factor(h) + mgrad = jax.scipy.linalg.cho_solve(h_, grad) else: - raise NotImplementedError(f"unsupported solver type: {m.opt.solver}") + raise NotImplementedError(f'unsupported solver type: {m.opt.solver}') ctx = ctx.replace(grad=grad, Mgrad=mgrad) diff --git a/mjx/mujoco/mjx/_src/solver_test.py b/mjx/mujoco/mjx/_src/solver_test.py index 692f6619..f4fdf732 100644 --- a/mjx/mujoco/mjx/_src/solver_test.py +++ b/mjx/mujoco/mjx/_src/solver_test.py @@ -33,8 +33,8 @@ def _assert_eq(a, b, name, tol=_TOLERANCE): np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) -def _assert_attr_eq(a, b, attr): - _assert_eq(getattr(a, attr), getattr(b, attr), attr) +def _assert_attr_eq(a, b, attr, tol=_TOLERANCE): + _assert_eq(getattr(a, attr), getattr(b, attr), attr, tol=tol) class SolverTest(absltest.TestCase): @@ -42,13 +42,20 @@ class SolverTest(absltest.TestCase): def test_newton(self): """Test newton solver.""" m = test_util.load_test_file('constraints.xml') + # it's critical that mgrad is optimally calculated, so lower iterations + # to be sure that MJX is converging as quickly as MuJoCo + m.opt.iterations = 1 d = mujoco.MjData(m) - mujoco.mj_step(m, d, 100) # at 100 steps mix of active/inactive constraints - mujoco.mj_forward(m, d) - mx = mjx.put_model(m) - dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d)) + mujoco.mj_step(m, d, 20) # significant constraint forces at 20 steps + + # mj_forward overwrites qacc_warmstart, so let's restore it to what it was + # at the beginning of the step so that MJX does not have a trivial solution + warmstart = d.qacc_warmstart.copy() + mujoco.mj_forward(m, d) + d.qacc_warmstart = warmstart + + dx = jax.jit(mjx.solve)(mjx.put_model(m), mjx.put_data(m, d)) - _assert_attr_eq(d, dx, 'qacc_warmstart') _assert_attr_eq(d, dx, 'qacc') _assert_attr_eq(d, dx, 'qfrc_constraint') nnz = dx.efc_J.any(axis=1) @@ -58,23 +65,30 @@ class SolverTest(absltest.TestCase): """Test CG solver.""" m = test_util.load_test_file('constraints.xml') d = mujoco.MjData(m) - mujoco.mj_step(m, d, 100) # at 100 steps mix of active/inactive constraints - m.opt.solver = mujoco.mjtSolver.mjSOL_CG - mujoco.mj_forward(m, d) - mx = mjx.put_model(m) - dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d)) + mujoco.mj_step(m, d, 20) # significant constraint forces at 20 steps + + # CG does not converge as quickly as Newton but is cheaper to calculate + m.opt.solver = mujoco.mjtSolver.mjSOL_CG + m.opt.iterations = 8 + + # mj_forward overwrites qacc_warmstart, so let's restore it to what it was + # at the beginning of the step so that MJX does not have a trivial solution + warmstart = d.qacc_warmstart.copy() + mujoco.mj_forward(m, d) + d.qacc_warmstart = warmstart + + dx = jax.jit(mjx.solve)(mjx.put_model(m), mjx.put_data(m, d)) - _assert_attr_eq(d, dx, 'qacc_warmstart') _assert_attr_eq(d, dx, 'qacc') - _assert_attr_eq(d, dx, 'qfrc_constraint') + _assert_attr_eq(d, dx, 'qfrc_constraint', tol=8e-4) nnz = dx.efc_J.any(axis=1) - _assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force') + _assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force', tol=5e-4) def test_no_warmstart(self): """Test no warmstart.""" m = test_util.load_test_file('constraints.xml') d = mujoco.MjData(m) - mujoco.mj_step(m, d, 100) # at 100 steps mix of active/inactive constraints + mujoco.mj_step(m, d, 20) # significant constraint forces at 20 steps m.opt.disableflags |= mujoco.mjtDisableBit.mjDSBL_WARMSTART mujoco.mj_forward(m, d) mx = mjx.put_model(m) @@ -83,17 +97,21 @@ class SolverTest(absltest.TestCase): # without warmstart, the solution is not as close _assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force', tol=2e-2) - def test_dense(self): - """Test solver works with dense mass matrices.""" + def test_sparse(self): + """Test solver works with sparse mass matrices.""" m = test_util.load_test_file('constraints.xml') + m.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE d = mujoco.MjData(m) - mujoco.mj_step(m, d, 100) # at 100 steps mix of active/inactive constraints - mujoco.mj_forward(m, d) - m.opt.jacobian = mujoco.mjtJacobian.mjJAC_DENSE - mx = mjx.put_model(m) - dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d)) + mujoco.mj_step(m, d, 20) # significant constraint forces at 20 steps + + # mj_forward overwrites qacc_warmstart, so let's restore it to what it was + # at the beginning of the step so that MJX does not have a trivial solution + warmstart = d.qacc_warmstart.copy() + mujoco.mj_forward(m, d) + d.qacc_warmstart = warmstart + + dx = jax.jit(mjx.solve)(mjx.put_model(m), mjx.put_data(m, d)) - _assert_attr_eq(d, dx, 'qacc_warmstart') _assert_attr_eq(d, dx, 'qacc') _assert_attr_eq(d, dx, 'qfrc_constraint') nnz = dx.efc_J.any(axis=1) diff --git a/mjx/mujoco/mjx/test_data/constraints.xml b/mjx/mujoco/mjx/test_data/constraints.xml index 52eb95d8..c7a9cd04 100644 --- a/mjx/mujoco/mjx/test_data/constraints.xml +++ b/mjx/mujoco/mjx/test_data/constraints.xml @@ -5,7 +5,7 @@ * solref, solimp --> -