diff --git a/src/xml/xml.cc b/src/xml/xml.cc index 7dbc7022..e87630bd 100644 --- a/src/xml/xml.cc +++ b/src/xml/xml.cc @@ -333,6 +333,12 @@ mjCModel* mjParseXML(const char* filename, int vfs_provider, char* error, int er else if (!strcasecmp(root->Value(), "robot")) { // parse URDF model mjXURDF parser; + + // set reasonable default for parsing a URDF + // this is separate from the Parser to allow multiple URDFs to be loaded. + model->strippath = true; + model->fusestatic = true; + parser.SetModel(model); parser.Parse(root); } diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 2cbe18a2..c58fdcee 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -68,18 +68,25 @@ void mjXURDF::Clear(void) { urGeomNames.clear(); } - +std::string mjXURDF::GetPrefixedName(const std::string& name) { + if (name.empty()) { + return name; + } + if (urPrefix.empty()) { + return name; + } + return urPrefix + "/" + name; +} // actual parser -void mjXURDF::Parse(XMLElement* root) { +void mjXURDF::Parse(XMLElement* root, const std::string& prefix, double* pos, double* quat) { std::string name, text; XMLElement *elem, *temp; int id_parent, id_child; + urPrefix = prefix; // set compiler defaults suitable for URDF - model->strippath = true; model->discardvisual = true; - model->fusestatic = true; // parse MuJoCo sections (not part of URDF) XMLElement* mjc = FindSubElem(root, "mujoco"); @@ -114,6 +121,7 @@ void mjXURDF::Parse(XMLElement* root) { name = elem->Value(); if (name=="link") { ReadAttrTxt(elem, "name", text, true); + text = GetPrefixedName(text); AddBody(text); } @@ -130,11 +138,13 @@ void mjXURDF::Parse(XMLElement* root) { // find parent, get name and id temp = FindSubElem(elem, "parent", true); ReadAttrTxt(temp, "link", text, true); + text = GetPrefixedName(text); id_parent = FindName(text, urName); // find child, get name and id temp = FindSubElem(elem, "child", true); ReadAttrTxt(temp, "link", text, true); + text = GetPrefixedName(text); id_child = FindName(text, urName); // make sure parent and child exist @@ -188,10 +198,25 @@ void mjXURDF::Parse(XMLElement* root) { // advance to next element elem = elem->NextSiblingElement(); } + + // override the pose for the base link and add a free joint + for (int i = 0; i < (int)urName.size(); i++) { + if (urParent[i] < 0) { + mjCBody* pbody = (mjCBody*)model->GetWorld()->FindObject(mjOBJ_BODY, urName[i]); + mjuu_copyvec(pbody->pos, pos, 3); + mjuu_copyvec(pbody->quat, quat, 4); + + // add a free joint to allow motion of the body + // if the mass is 0, assume the object is static + if (pbody->mass > 0) { + auto pjoint = pbody->AddJoint(); + pjoint->name = urName[i] + "_free_joint"; + pjoint->type = mjJNT_FREE; + } + } + } } - - // parse body/link void mjXURDF::Body(XMLElement* body_elem) { std::string name, text; @@ -201,11 +226,11 @@ void mjXURDF::Body(XMLElement* body_elem) { // get body name and pointer to mjCBody ReadAttrTxt(body_elem, "name", name, true); + name = GetPrefixedName(name); pbody = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, name); if (!pbody) { throw mjXError(body_elem, "URDF body not found"); // SHOULD NOT OCCUR } - // inertial element: copy into alternative body frame if ((elem = FindSubElem(body_elem, "inertial"))) { pbody->explicitinertial = true; @@ -264,13 +289,13 @@ void mjXURDF::Body(XMLElement* body_elem) { // otherwise use material table else { ReadAttrTxt(temp, "name", name, true); + name = GetPrefixedName(name); int imat = FindName(name, urMat); if (imat>=0) { std::memcpy(rgba, urRGBA[imat].val, 4*sizeof(float)); } } } - // create geom if not discarded if (!model->discardvisual) { pgeom = Geom(elem, pbody, false); @@ -282,6 +307,7 @@ void mjXURDF::Body(XMLElement* body_elem) { // save name if it doesn't already exist. mjXUtil::ReadAttrTxt(elem, "name", geom_name); + name = GetPrefixedName(name); if (urGeomNames.find(geom_name) == urGeomNames.end()) { pgeom->name = geom_name; urGeomNames.insert(geom_name); @@ -304,6 +330,7 @@ void mjXURDF::Body(XMLElement* body_elem) { // save name if it doesn't already exist. mjXUtil::ReadAttrTxt(elem, "name", geom_name); + geom_name = GetPrefixedName(geom_name); if (urGeomNames.find(geom_name) == urGeomNames.end()) { pgeom->name = geom_name; urGeomNames.insert(geom_name); @@ -313,13 +340,18 @@ void mjXURDF::Body(XMLElement* body_elem) { << std::endl; } } - // advance elem = elem->NextSiblingElement(); } } - +void mjXURDF::Parse(XMLElement* root) { + double pos[3] = {0}; + mjuu_setvec(pos, 0, 0, 0); + double quat[4] = {1, 0, 0, 0}; + mjuu_setvec(quat, 1, 0, 0, 0); + Parse(root, /*prefix=*/"", pos, quat); +} // parse joint void mjXURDF::Joint(XMLElement* joint_elem) { @@ -340,6 +372,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) { // get parent, check elem = FindSubElem(joint_elem, "parent", true); ReadAttrTxt(elem, "link", name, true); + name = GetPrefixedName(name); parent = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, name); if (!parent) { // SHOULD NOT OCCUR throw mjXError(elem, "invalid parent name in URDF joint definition"); @@ -348,6 +381,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) { // get child=this, check elem = FindSubElem(joint_elem, "child", true); ReadAttrTxt(elem, "link", name, true); + name = GetPrefixedName(name); pbody = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, name); if (!pbody) { // SHOULD NOT OCCUR throw mjXError(elem, "invalid child name in URDF joint definition"); diff --git a/src/xml/xml_urdf.h b/src/xml/xml_urdf.h index 040f2d36..63079f8e 100644 --- a/src/xml/xml_urdf.h +++ b/src/xml/xml_urdf.h @@ -34,9 +34,12 @@ class mjXURDF : public mjXBase { mjXURDF(); // constructor virtual ~mjXURDF(); // destructor + // parse and set frame of base link and append a prefix to the name + void Parse(tinyxml2::XMLElement* root, const std::string& prefix, double* pos, double* quat); // parse and set frame of base link void Parse(tinyxml2::XMLElement* root); // main parser private: + std::string GetPrefixedName(const std::string& name); // get prefix/name of element int FindName(std::string name, std::vector& list); // find name in list void AddName(std::string name, std::vector& list); // add name to list void AddBody(std::string name); // add body to local table @@ -57,6 +60,8 @@ class mjXURDF : public mjXBase { std::vector urMat; // material name std::vector urRGBA; // material RBG value std::unordered_set urGeomNames; // geom name + + std::string urPrefix; // prefix to apply to all names }; #endif // THIRD_PARTY_MUJOCO_SRC_XML_XML_URDF_