From b873800ab390677b9861f97d004604b811365033 Mon Sep 17 00:00:00 2001 From: DeepMind Date: Mon, 12 Jun 2023 09:27:33 -0700 Subject: [PATCH] Initial Implementation of MuJoCo Physics Device 1) Modifications to MJCModel and URDF parsing to enable loading multiple URDF's 2) Device Skeleton for MuJoCo Physics Device 3) Scene Loading and Modifications 4) State Generation PiperOrigin-RevId: 539676439 Change-Id: Ic65f812b660319443e74f68acd851dba1435ddca --- src/xml/xml.cc | 6 +++++ src/xml/xml_urdf.cc | 54 ++++++++++++++++++++++++++++++++++++--------- src/xml/xml_urdf.h | 5 +++++ 3 files changed, 55 insertions(+), 10 deletions(-) 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_