From 8f3c488de8343d03b456d3ef2be6e58cb15eccf9 Mon Sep 17 00:00:00 2001
From: Google DeepMind <noreply@deepmind.com>
Date: Wed, 12 Jul 2023 19:33:47 -0700
Subject: [PATCH] allow specifying static bodies when parsing URDF

PiperOrigin-RevId: 547664406
Change-Id: I4b555431d1877fb7547c6b550387cd4230c2b0c4
---
 src/xml/xml_urdf.cc | 8 +++++---
 src/xml/xml_urdf.h  | 7 ++++++-
 2 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc
index c58fdcee..9f83b91c 100644
--- a/src/xml/xml_urdf.cc
+++ b/src/xml/xml_urdf.cc
@@ -79,7 +79,9 @@ std::string mjXURDF::GetPrefixedName(const std::string& name) {
 }
 
 // actual parser
-void mjXURDF::Parse(XMLElement* root, const std::string& prefix, double* pos, double* quat) {
+void mjXURDF::Parse(
+    XMLElement* root, const std::string& prefix, double* pos, double* quat,
+    const bool static_body) {
   std::string name, text;
   XMLElement *elem, *temp;
   int id_parent, id_child;
@@ -208,7 +210,7 @@ void mjXURDF::Parse(XMLElement* root, const std::string& prefix, double* pos, do
 
       // add a free joint to allow motion of the body
       // if the mass is 0, assume the object is static
-      if (pbody->mass > 0) {
+      if (!static_body && pbody->mass > 0) {
         auto pjoint = pbody->AddJoint();
         pjoint->name = urName[i] + "_free_joint";
         pjoint->type = mjJNT_FREE;
@@ -350,7 +352,7 @@ void mjXURDF::Parse(XMLElement* root) {
   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(root, /*prefix=*/"", pos, quat, true);
 }
 
 // parse joint
diff --git a/src/xml/xml_urdf.h b/src/xml/xml_urdf.h
index 63079f8e..fdca1dfd 100644
--- a/src/xml/xml_urdf.h
+++ b/src/xml/xml_urdf.h
@@ -35,7 +35,12 @@ class mjXURDF : public mjXBase {
   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,
+      const std::string& prefix,
+      double* pos,
+      double* quat,
+      bool static_body);
   void Parse(tinyxml2::XMLElement* root);             // main parser
 
  private: