From 9ecd7009235db298815094b316f3654fc80943d4 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 13 Feb 2024 09:08:55 -0800 Subject: [PATCH] Add mjmMesh to C API. PiperOrigin-RevId: 606642150 Change-Id: Iae69ce8426729b1e2394d9da9f349a2f0f7ccf98 --- src/user/user_api.cc | 10 ++ src/user/user_api.h | 27 ++++++ src/user/user_flexcomp.cc | 6 +- src/user/user_init.cc | 11 +++ src/user/user_mesh.cc | 181 +++++++++++++---------------------- src/user/user_objects.cc | 2 +- src/user/user_objects.h | 74 +++++++------- src/xml/xml_native_reader.cc | 64 +++++++++---- src/xml/xml_native_reader.h | 2 +- src/xml/xml_native_writer.cc | 29 +++--- src/xml/xml_urdf.cc | 28 ++++-- 11 files changed, 234 insertions(+), 200 deletions(-) diff --git a/src/user/user_api.cc b/src/user/user_api.cc index ca43c95a..ed4b2ef7 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -137,6 +137,16 @@ mjmFrame* mjm_addFrame(mjmBody* bodyspec, mjmFrame* parentframe) { +// add mesh to model +mjmMesh* mjm_addMesh(void* model, void* defspec) { + mjCDef* def = defspec ? reinterpret_cast(defspec) : 0; + mjCModel* modelC = static_cast(model); + mjCMesh* mesh = modelC->AddMesh(def); + return &mesh->spec; +} + + + // add texture to model mjmTexture* mjm_addTexture(void* model) { mjCModel* modelC = static_cast(model); diff --git a/src/user/user_api.h b/src/user/user_api.h index 0de97b41..7b5cafe1 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -325,6 +325,27 @@ typedef struct _mjmFlex { } mjmFlex; +typedef struct _mjmMesh { // mesh specification + mjElement element; // internal, do not modify + mjString name; // name + mjString classname; // class name + mjString content_type; // content type of file + mjString file; // mesh file + double refpos[3]; // reference position + double refquat[4]; // reference orientation + double scale[3]; // rescale mesh + mjtByte smoothnormal; // do not exclude large-angle faces from normals + mjFloatVec uservert; // user vertex data + mjFloatVec usernormal; // user normal data + mjFloatVec usertexcoord; // user texcoord data + mjIntVec userface; // user vertex indices + mjIntVec userfacenormal; // user normal indices + mjIntVec userfacetexcoord; // user texcoord indices + mjmPlugin plugin; // sdf plugin + mjString info; // message appended to compiler errors +} mjmMesh; + + typedef struct _mjmTexture { // texture specification mjElement element; // internal, do not modify mjString name; // name @@ -558,6 +579,9 @@ MJAPI mjmFrame* mjm_addFrame(mjmBody* body, mjmFrame* parentframe); // Add flex to model. MJAPI mjmFlex* mjm_addFlex(void* model); +// Add mesh to model. +MJAPI mjmMesh* mjm_addMesh(void* model, void* defspec); + // Add texture to model. MJAPI mjmTexture* mjm_addTexture(void* model); @@ -672,6 +696,9 @@ MJAPI void mjm_defaultLight(mjmLight& light); // Default flex attributes. MJAPI void mjm_defaultFlex(mjmFlex& flex); +// Default mesh attributes. +MJAPI void mjm_defaultMesh(mjmMesh& mesh); + // Default texture attributes. MJAPI void mjm_defaultTexture(mjmTexture& texture); diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index a9987df9..1f585a88 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -857,13 +857,13 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) { // LoadOBJ uses userXXX, extra processing needed if (isobj) { // check sizes - if (mesh.uservert().empty() || mesh.userface().empty()) { + if (mesh.get_uservert().empty() || mesh.get_userface().empty()) { return comperr(error, "Vertex and face data required", error_sz); } - if (mesh.uservert().size()%3) { + if (mesh.get_uservert().size()%3) { return comperr(error, "Vertex data must be multiple of 3", error_sz); } - if (mesh.userface().size()%3) { + if (mesh.get_userface().size()%3) { return comperr(error, "Face data must be multiple of 3", error_sz); } diff --git a/src/user/user_init.cc b/src/user/user_init.cc index dba2367c..b900d089 100644 --- a/src/user/user_init.cc +++ b/src/user/user_init.cc @@ -192,6 +192,17 @@ void mjm_defaultFlex(mjmFlex& flex) { +// default mesh attributes +void mjm_defaultMesh(mjmMesh& mesh) { + memset(&mesh, 0, sizeof(mjmMesh)); + mjuu_setvec(mesh.refpos, 0, 0, 0); + mjuu_setvec(mesh.refquat, 1, 0, 0, 0); + mjuu_setvec(mesh.scale, 1, 1, 1); + mesh.smoothnormal = false; +} + + + // default texture attributes void mjm_defaultTexture(mjmTexture& texture) { memset(&texture, 0, sizeof(mjmTexture)); diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 8fb4849b..0d7f6b93 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -129,11 +129,7 @@ static void ReadFromBuffer(T* dst, const char* src) { //------------------ class mjCMesh implementation -------------------------------------------------- mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) { - // set defaults - mjuu_setvec(refpos_, 0, 0, 0); - mjuu_setvec(refquat_, 1, 0, 0, 0); - mjuu_setvec(scale_, 1, 1, 1); - smoothnormal_ = false; + mjm_defaultMesh(spec); // clear internal variables mjuu_setvec(pos_surface_, 0, 0, 0); @@ -179,9 +175,50 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) { model = _model; def = (_def ? _def : (_model ? _model->defaults[0] : 0)); + // in case this body is not compiled + CopyFromSpec(); + // point to local (needs to be after defaults) - plugin.name = (mjString)&plugin_name; - plugin.instance_name = (mjString)&plugin_instance_name; + PointToLocal(); +} + + + +void mjCMesh::PointToLocal() { + spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.classname = (mjString)&classname; + spec.file = (mjString)&spec_file_; + spec.content_type = (mjString)&spec_content_type_; + spec.uservert = (mjFloatVec)&spec_uservert_; + spec.usernormal = (mjFloatVec)&spec_usernormal_; + spec.userface = (mjIntVec)&spec_userface_; + spec.usertexcoord = (mjFloatVec)&spec_usertexcoord_; + spec.plugin.name = (mjString)&plugin_name; + spec.plugin.instance_name = (mjString)&plugin_instance_name; + spec.info = (mjString)&info; +} + + + +void mjCMesh::CopyFromSpec() { + *static_cast(this) = spec; + file_ = spec_file_; + content_type_ = spec_content_type_; + uservert_ = spec_uservert_; + usernormal_ = spec_usernormal_; + userface_ = spec_userface_; + usertexcoord_ = spec_usertexcoord_; + file = (mjString)&file_; + content_type = (mjString)&content_type_; + uservert = (mjFloatVec)&uservert_; + usernormal = (mjFloatVec)&usernormal_; + userface = (mjIntVec)&userface_; + usertexcoord = (mjFloatVec)&usertexcoord_; + plugin.active = spec.plugin.active; + plugin.instance = spec.plugin.instance; + plugin.name = spec.plugin.name; + plugin.instance_name = spec.plugin.instance_name; } @@ -199,96 +236,6 @@ mjCMesh::~mjCMesh() { -void mjCMesh::set_content_type(std::optional&& content_type) { - if (content_type.has_value()) { - content_type_ = std::move(content_type.value()); - } -} - - - -void mjCMesh::set_file(std::optional&& file) { - if (file.has_value()) { - file_ = std::move(file.value()); - } -} - - - -void mjCMesh::set_refpos(std::optional> refpos) { - if (refpos.has_value()) { - std::copy(refpos.value().begin(), refpos.value().end(), refpos_); - } -} - - - -void mjCMesh::set_refquat(std::optional> refquat) { - if (refquat.has_value()) { - std::copy(refquat.value().begin(), refquat.value().end(), refquat_); - } -} - - - -void mjCMesh::set_scale(std::optional> scale) { - if (scale.has_value()) { - set_scale(scale.value()); - } -} - - - -void mjCMesh::set_uservert(std::optional>&& uservert) { - if (uservert.has_value()) { - uservert_ = std::move(uservert.value()); - } -} - - - -void mjCMesh::set_usernormal(std::optional>&& usernormal) { - if (usernormal.has_value()) { - usernormal_ = std::move(usernormal.value()); - } -} - - - -void mjCMesh::set_usertexcoord(std::optional>&& usertexcoord) { - if (usertexcoord.has_value()) { - usertexcoord_ = std::move(usertexcoord.value()); - } -} - - - -void mjCMesh::set_userface(std::optional>&& userface) { - if (userface.has_value()) { - userface_ = std::move(userface.value()); - } -} - - - -void mjCMesh::set_file(const std::string& file) { - file_ = file; -} - - - -void mjCMesh::set_scale(std::array scale) { - std::copy(scale.begin(), scale.end(), scale_); -} - - - -void mjCMesh::set_smoothnormal(bool smoothnormal) { - smoothnormal_ = smoothnormal; -} - - - void mjCMesh::set_needhull(bool needhull) { needhull_ = needhull; } @@ -303,7 +250,7 @@ void mjCMesh::LoadSDF() { name.c_str(), id); } - if (scale_[0] != 1 || scale_[1] != 1 || scale_[2] != 1) { + if (scale[0] != 1 || scale[1] != 1 || scale[2] != 1) { throw mjCError(this, "attribute scale is not compatible with SDFs in mesh '%s', (id = %d)", name.c_str(), id); } @@ -371,9 +318,9 @@ void mjCMesh::LoadSDF() { userface.push_back(mesh.indices.at(i)); } - set_uservert(uservert); - set_usernormal(usernormal); - set_userface(userface); + uservert_ = std::move(uservert); + usernormal_ = std::move(usernormal); + userface_ = std::move(userface); delete[] field; } @@ -381,6 +328,8 @@ void mjCMesh::LoadSDF() { // compiler void mjCMesh::Compile(const mjVFS* vfs) { + CopyFromSpec(); + // load file if (!file_.empty()) { // remove path from file if necessary @@ -957,7 +906,7 @@ void mjCMesh::LoadOBJ(mjResource* resource) { if (!objReader.GetShapes().empty()) { const auto& mesh = objReader.GetShapes()[0].mesh; - bool righthand = (scale_[0]*scale_[1]*scale_[2] > 0); + bool righthand = (scale[0]*scale[1]*scale[2] > 0); // iterate over mesh faces std::vector face_indices; @@ -1005,7 +954,7 @@ void mjCMesh::LoadOBJ(mjResource* resource) { // load STL binary mesh void mjCMesh::LoadSTL(mjResource* resource) { - bool righthand = (scale_[0]*scale_[1]*scale_[2]>0); + bool righthand = (scale[0]*scale[1]*scale[2]>0); // get file data in buffer char* buffer = 0; @@ -1085,7 +1034,7 @@ void mjCMesh::LoadSTL(mjResource* resource) { // load MSH binary mesh void mjCMesh::LoadMSH(mjResource* resource) { - bool righthand = (scale_[0]*scale_[1]*scale_[2]>0); + bool righthand = (scale[0]*scale[1]*scale[2]>0); // get file data in buffer char* buffer = 0; @@ -1193,9 +1142,9 @@ void mjCMesh::ComputeVolume(double CoM[3], mjtGeomInertia type, // apply transformations void mjCMesh::ApplyTransformations() { // translate - if (refpos_[0]!=0 || refpos_[1]!=0 || refpos_[2]!=0) { + if (refpos[0]!=0 || refpos[1]!=0 || refpos[2]!=0) { // prepare translation - float rp[3] = {(float)refpos_[0], (float)refpos_[1], (float)refpos_[2]}; + float rp[3] = {(float)refpos[0], (float)refpos[1], (float)refpos[2]}; // process vertices for (int i=0; i(this) = spec; userdata_ = spec_userdata_; userdata = (mjDoubleVec)&userdata_; - spec.info = (mjString)&info; mju_copy4(alt_.axisangle, alt.axisangle); mju_copy(alt_.xyaxes, alt.xyaxes, 6); mju_copy3(alt_.zaxis, alt.zaxis); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index c8a5a742..affcef6e 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -615,26 +615,36 @@ class mjCFlex: public mjCBase, private mjmFlex { //------------------------- class mjCMesh ---------------------------------------------------------- // Describes a mesh -class mjCMesh: public mjCBase { +class mjCMesh: public mjCBase, private mjmMesh { friend class mjCFlexcomp; + friend class mjXWriter; public: mjCMesh(mjCModel* = 0, mjCDef* = 0); ~mjCMesh(); - // public getters - const std::string& content_type() const { return content_type_; } - const std::string& file() const { return file_; } + mjmMesh spec; + using mjCBase::name; + using mjCBase::classname; + using mjCBase::plugin; + using mjCBase::info; + + void CopyFromSpec(void); + void PointToLocal(void); + + // public getters and setters + const std::string& get_content_type() const { return content_type_; } const std::string& get_file() const { return file_; } - const double* refpos() const { return refpos_; } - const double* refquat() const { return refquat_; } - const double* scale() const { return scale_; } - bool smoothnormal() const { return smoothnormal_; } + const double* get_refpos() const { return refpos; } + const double* get_refquat() const { return refquat; } + const double* get_scale() const { return scale; } + bool get_smoothnormal() const { return smoothnormal; } + void set_needhull(bool needhull); // public getters for user data - const std::vector& uservert() const { return uservert_; } - const std::vector& usernormal() const { return usernormal_; } - const std::vector& usertexcoord() const { return usertexcoord_; } - const std::vector& userface() const { return userface_; } + const std::vector& get_uservert() const { return uservert_; } + const std::vector& get_usernormal() const { return usernormal_; } + const std::vector& get_usertexcoord() const { return usertexcoord_; } + const std::vector& get_userface() const { return userface_; } // mesh properties computed by Compile const double* aamm() const { return aamm_; } @@ -651,24 +661,6 @@ class mjCMesh: public mjCBase { // bounding volume hierarchy tree const mjCBoundingVolumeHierarchy& tree() { return tree_; } - // general setters - void set_file(const std::string& file); - void set_scale(std::array scale); - void set_smoothnormal(bool smoothnormal); - void set_needhull(bool needhull); - - // setters used in reading XML attributes (no-op if empty optional) - void set_content_type(std::optional&& content_type); - void set_file(std::optional&& file); - void set_refpos(std::optional> refpos); - void set_refquat(std::optional> refquat); - void set_scale(std::optional> scale); - - void set_uservert(std::optional>&& uservert); - void set_usernormal(std::optional>&& usernormal); - void set_usertexcoord(std::optional>&& usertexcoord); - void set_userface(std::optional>&& userface); - void Compile(const mjVFS* vfs); // compiler double* GetPosPtr(mjtGeomInertia type); // get position double* GetQuatPtr(mjtGeomInertia type); // get orientation @@ -694,19 +686,25 @@ class mjCMesh: public mjCBase { void SetBoundingVolume(int faceid); private: - bool visual_; // true: the mesh is only visual - std::string content_type_; // content type of file - std::string file_; // mesh file - double refpos_[3]; // reference position (translate) - double refquat_[4]; // reference orientation (rotate) - double scale_[3]; // rescale mesh - bool smoothnormal_; // do not exclude large-angle faces from normals - + // variable size attributes + std::string content_type_; // content type of file + std::string file_; // mesh file std::vector uservert_; // user vertex data std::vector usernormal_; // user normal data std::vector usertexcoord_; // user texcoord data std::vector userface_; // user vertex indices std::vector userfacenormal_; // user normal indices + + std::string spec_content_type_; + std::string spec_file_; + std::vector spec_uservert_; + std::vector spec_usernormal_; + std::vector spec_usertexcoord_; + std::vector spec_userface_; + std::vector spec_userfacenormal_; + + // used by the compiler + bool visual_; // true: the mesh is only visual std::vector userfacetexcoord_; // user texcoord indices std::vector< std::pair > useredge_; // user half-edge data diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 6cba45f9..07ad8dbc 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1346,18 +1346,26 @@ void mjXReader::OneFlex(XMLElement* elem, mjmFlex* pflex) { // mesh element parser -void mjXReader::OneMesh(XMLElement* elem, mjCMesh* pmesh) { +void mjXReader::OneMesh(XMLElement* elem, mjmMesh* pmesh) { int n; - string text; + string text, name, classname, content_type, file; // read attributes - ReadAttrTxt(elem, "name", pmesh->name); - ReadAttrTxt(elem, "class", pmesh->classname); - pmesh->set_content_type(ReadAttrStr(elem, "content_type")); - pmesh->set_file(ReadAttrStr(elem, "file")); - pmesh->set_refpos(ReadAttrArr(elem, "refpos")); - pmesh->set_refquat(ReadAttrArr(elem, "refquat")); - pmesh->set_scale(ReadAttrArr(elem, "scale")); + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(pmesh->name, name.c_str()); + } + if (ReadAttrTxt(elem, "class", classname)) { + mjm_setString(pmesh->classname, classname.c_str()); + } + if (ReadAttrTxt(elem, "content_type", content_type)) { + mjm_setString(pmesh->content_type, content_type.c_str()); + } + if (ReadAttrTxt(elem, "file", file)) { + mjm_setString(pmesh->file, file.c_str()); + } + ReadAttr(elem, "refpos", 3, pmesh->refpos, text); + ReadAttr(elem, "refpos", 4, pmesh->refquat, text); + ReadAttr(elem, "scale", 3, pmesh->scale, text); XMLElement* eplugin = FirstChildElement(elem, "plugin"); if (eplugin) { @@ -1365,22 +1373,44 @@ void mjXReader::OneMesh(XMLElement* elem, mjCMesh* pmesh) { } if (MapValue(elem, "smoothnormal", &n, bool_map, 2)) { - pmesh->set_smoothnormal((n==1)); + pmesh->smoothnormal = (n==1); } // read user vertex data - pmesh->set_uservert(ReadAttrVec(elem, "vertex")); + if (ReadAttrTxt(elem, "vertex", text)) { + auto uservert = ReadAttrVec(elem, "vertex"); + if (uservert.has_value()) { + mjm_setFloat(pmesh->uservert, uservert->data(), uservert->size()); + } + } // read user normal data - pmesh->set_usernormal(ReadAttrVec(elem, "normal")); + if (ReadAttrTxt(elem, "normal", text)) { + auto usernormal = ReadAttrVec(elem, "normal"); + if (usernormal.has_value()) { + mjm_setFloat(pmesh->usernormal, usernormal->data(), usernormal->size()); + } + } // read user texcoord data - pmesh->set_usertexcoord(ReadAttrVec(elem, "texcoord")); + if (ReadAttrTxt(elem, "texcoord", text)) { + auto usertexcoord = ReadAttrVec(elem, "texcoord"); + if (usertexcoord.has_value()) { + mjm_setFloat(pmesh->usertexcoord, usertexcoord->data(), usertexcoord->size()); + } + } // read user face data - pmesh->set_userface(ReadAttrVec(elem, "face")); + if (ReadAttrTxt(elem, "face", text)) { + auto userface = ReadAttrVec(elem, "face"); + if (userface.has_value()) { + mjm_setInt(pmesh->userface, userface->data(), userface->size()); + } + } - GetXMLPos(elem, pmesh); + // write error info + mjm_setString(pmesh->info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); } @@ -2608,7 +2638,7 @@ void mjXReader::Default(XMLElement* section, int parentid) { name = elem->Value(); // read mesh - if (name=="mesh") OneMesh(elem, &def->mesh); + if (name=="mesh") OneMesh(elem, &def->mesh.spec); // read material else if (name=="material") OneMaterial(elem, &def->material.spec); @@ -3073,7 +3103,7 @@ void mjXReader::Asset(XMLElement* section) { // mesh sub-element else if (name=="mesh") { // create mesh and parse - mjCMesh* pmesh = model->AddMesh(def); + mjmMesh* pmesh = mjm_addMesh(model, def); OneMesh(elem, pmesh); } diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 43b0caab..69682a35 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -59,7 +59,7 @@ class mjXReader : public mjXBase { // single element parsers, used in defaults and main body void OneFlex(tinyxml2::XMLElement* elem, mjmFlex* pflex); - void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh); + void OneMesh(tinyxml2::XMLElement* elem, mjmMesh* pmesh); void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin); void OneMaterial(tinyxml2::XMLElement* elem, mjmMaterial* pmaterial); void OneJoint(tinyxml2::XMLElement* elem, mjmJoint* pjoint); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 36fd29fc..763ed1e4 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -159,39 +159,40 @@ void mjXWriter::OneMesh(XMLElement* elem, mjCMesh* pmesh, mjCDef* def) { if (!writingdefaults) { WriteAttrTxt(elem, "name", pmesh->name); WriteAttrTxt(elem, "class", pmesh->classname); - WriteAttrTxt(elem, "content_type", pmesh->content_type()); - WriteAttrTxt(elem, "file", pmesh->file()); + WriteAttrTxt(elem, "content_type", pmesh->get_content_type()); + WriteAttrTxt(elem, "file", pmesh->get_file()); // write vertex data - if (!pmesh->uservert().empty()) { - Vector2String(text, pmesh->uservert()); + if (!pmesh->get_uservert().empty()) { + Vector2String(text, pmesh->get_uservert()); WriteAttrTxt(elem, "vertex", text); } // write normal data - if (!pmesh->usernormal().empty()) { - Vector2String(text, pmesh->usernormal()); + if (!pmesh->get_usernormal().empty()) { + Vector2String(text, pmesh->get_usernormal()); WriteAttrTxt(elem, "normal", text); } // write texcoord data - if (!pmesh->usertexcoord().empty()) { - Vector2String(text, pmesh->usertexcoord()); + if (!pmesh->get_usertexcoord().empty()) { + Vector2String(text, pmesh->get_usertexcoord()); WriteAttrTxt(elem, "texcoord", text); } // write face data - if (!pmesh->userface().empty()) { - Vector2String(text, pmesh->userface()); + if (!pmesh->get_userface().empty()) { + Vector2String(text, pmesh->get_userface()); WriteAttrTxt(elem, "face", text); } } // defaults and regular - WriteAttr(elem, "refpos", 3, pmesh->refpos(), def->mesh.refpos()); - WriteAttr(elem, "refquat", 4, pmesh->refquat(), def->mesh.refquat()); - WriteAttr(elem, "scale", 3, pmesh->scale(), def->mesh.scale()); - WriteAttrKey(elem, "smoothnormal", bool_map, 2, pmesh->smoothnormal(), def->mesh.smoothnormal()); + WriteAttr(elem, "refpos", 3, pmesh->refpos, def->mesh.refpos); + WriteAttr(elem, "refquat", 4, pmesh->refquat, def->mesh.refquat); + WriteAttr(elem, "scale", 3, pmesh->scale, def->mesh.scale); + WriteAttrKey(elem, "smoothnormal", bool_map, 2, pmesh->get_smoothnormal(), + def->mesh.get_smoothnormal()); } diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 1f76bce4..168d705b 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -571,26 +571,34 @@ mjmGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) { meshname = mjuu_stripext(meshname); // look for existing mesh - mjCMesh* pmesh = (mjCMesh*)model->FindObject(mjOBJ_MESH, meshname); + mjCMesh* mesh = (mjCMesh*)model->FindObject(mjOBJ_MESH, meshname); + mjmMesh* pmesh = 0; // does not exist: create - if (!pmesh) { - pmesh = model->AddMesh(); + if (!mesh) { + pmesh = mjm_addMesh(model, 0); } // exists with different scale: append name with '1', create - else if (pmesh->scale()[0]!=meshscale[0] || - pmesh->scale()[1]!=meshscale[1] || - pmesh->scale()[2]!=meshscale[2]) { - pmesh = model->AddMesh(); + else if (mesh->spec.scale[0]!=meshscale[0] || + mesh->spec.scale[1]!=meshscale[1] || + mesh->spec.scale[2]!=meshscale[2]) { + pmesh = mjm_addMesh(model, 0); meshname = meshname + "1"; } + // point to already existing spec + else { + pmesh = &mesh->spec; + } + // set fields - pmesh->set_file(meshfile); - pmesh->name = meshname; + mjm_setString(pmesh->file, meshfile.c_str()); + mjm_setString(pmesh->name, meshname.c_str()); mjm_setString(pgeom->meshname, meshname.c_str()); - pmesh->set_scale(meshscale); + pmesh->scale[0] = meshscale[0]; + pmesh->scale[1] = meshscale[1]; + pmesh->scale[2] = meshscale[2]; } else {