Add flag to render articulated object primitives while having a skinn…

…ed mesh for debugging.
facebookresearch · Apr 19, 2023 · e96d590 · e96d590
1 parent c13ea2d
commit e96d590
Show file tree

Hide file tree

Showing 3 changed files with 55 additions and 21 deletions.
diff --git a/src/esp/io/URDFParser.cpp b/src/esp/io/URDFParser.cpp
@@ -126,22 +126,34 @@ bool Model::loadJsonAttributes(const std::string& filename) {
   const io::JsonGenericValue jsonConfig = docConfig->GetObject();
 
   // check for render asset
-  const std::string renderAssetAttributeName = "render_asset";
-  const char* ra_cstr = renderAssetAttributeName.c_str();
+  const char* attrRenderAsset = "render_asset";
+  const char* attrDebugRenderPrimitives = "debug_render_primitives";
 
   bool hasRenderAsset = false;
-  if (jsonConfig.HasMember(ra_cstr)) {
-    if (!jsonConfig[ra_cstr].IsString()) {
+  if (jsonConfig.HasMember(attrRenderAsset)) {
+    if (!jsonConfig[attrRenderAsset].IsString()) {
       ESP_WARNING() << "<Model> : Json Config file specifies a render_asset "
                        "attribute but "
                        "it is not a string. Skipping render_asset config load.";
       return false;
     } else {
-      const std::string renderAssetPath{jsonConfig[ra_cstr].GetString()};
+      const std::string renderAssetPath{
+          jsonConfig[attrRenderAsset].GetString()};
       m_renderAsset = renderAssetPath;
       hasRenderAsset = true;
     }
   }
+  if (jsonConfig.HasMember(attrDebugRenderPrimitives)) {
+    if (!jsonConfig[attrDebugRenderPrimitives].IsBool()) {
+      ESP_WARNING()
+          << "<Model> : Json Config file specifies debug_render_primitives "
+             "attribute but it is not a bool. Skipping debug_render_primitives "
+             "config load.";
+      return false;
+    } else {
+      m_debugRenderPrimitives = jsonConfig[attrDebugRenderPrimitives].GetBool();
+    }
+  }
 
   // check for user defined attributes and verify it is an object
   const std::string subGroupName = "user_defined";
@@ -174,7 +186,7 @@ bool Model::loadJsonAttributes(const std::string& filename) {
 
   if (!hasRenderAsset) {
     ESP_WARNING() << "<Model> : Json Config file exists but \"" << subGroupName
-                  << "\" and \"" << renderAssetAttributeName
+                  << "\" and \"" << attrRenderAsset
                   << "\" tags not found within file.";
   }
   return false;

diff --git a/src/esp/io/URDFParser.h b/src/esp/io/URDFParser.h
@@ -376,6 +376,20 @@ class Model {
     return m_renderAsset;
   }
 
+  /**
+   * @brief Set hint to render articulated object primitives even if a render
+   * asset is present.
+   */
+  void setDebugRenderPrimitives(bool debugRenderPrimitives) {
+    m_debugRenderPrimitives = debugRenderPrimitives;
+  }
+
+  /**
+   * @brief Get hint to render articulated object primitives even if a render
+   * asset is present.
+   */
+  bool getDebugRenderPrimitives() const { return m_debugRenderPrimitives; }
+
   /**
    * @brief This function conditionally loads configuration data from a Json
    * file for this Articulated Model, should an appropriate file exist. This
@@ -424,6 +438,9 @@ class Model {
   //! Path to a render asset associated with this articulated object.
   Cr::Containers::Optional<std::string> m_renderAsset = Cr::Containers::NullOpt;
 
+  //! Forces link primitives to be rendered even if a render asset is present.
+  bool m_debugRenderPrimitives = false;
+
   //! Scale the transformation and parameters of a Shape
   void scaleShape(Shape& shape, float scale);
 };  // class model

diff --git a/src/esp/physics/bullet/BulletPhysicsManager.cpp b/src/esp/physics/bullet/BulletPhysicsManager.cpp
@@ -163,9 +163,10 @@ int BulletPhysicsManager::addArticulatedObjectFromURDF(
   u2b->initURDF2BulletCache();
 
   articulatedObject->initializeFromURDF(*urdfImporter_, {}, physicsNode_);
+  auto model = u2b->getModel();
 
   // if the URDF model specifies a render asset, load and link it
-  auto renderAssetPath = u2b->getModel()->getRenderAsset();
+  auto renderAssetPath = model->getRenderAsset();
   if (renderAssetPath) {
     // load associated skinned mesh
     assets::AssetInfo assetInfo = assets::AssetInfo::fromPath(*renderAssetPath);
@@ -187,20 +188,24 @@ int BulletPhysicsManager::addArticulatedObjectFromURDF(
         articulatedObject->btMultiBody_->getLinkCollider(linkIx), linkObjectId);
   }
 
-  // attach link visual shapes
-  for (size_t urdfLinkIx = 0; urdfLinkIx < u2b->getModel()->m_links.size();
-       ++urdfLinkIx) {
-    auto urdfLink = u2b->getModel()->getLink(urdfLinkIx);
-    if (!urdfLink->m_visualArray.empty()) {
-      int bulletLinkIx =
-          u2b->cache->m_urdfLinkIndices2BulletLinkIndices[urdfLinkIx];
-      ArticulatedLink& linkObject = articulatedObject->getLink(bulletLinkIx);
-      ESP_CHECK(
-          attachLinkGeometry(&linkObject, urdfLink, drawables, lightSetup),
-          "BulletPhysicsManager::addArticulatedObjectFromURDF(): Failed to "
-          "instance render asset (attachGeometry) for link"
-              << urdfLinkIx << ".");
-      linkObject.node().computeCumulativeBB();
+  bool renderVisualShapes =
+      !renderAssetPath || model->getDebugRenderPrimitives();
+  if (renderVisualShapes) {
+    // attach link visual shapes
+    for (size_t urdfLinkIx = 0; urdfLinkIx < model->m_links.size();
+         ++urdfLinkIx) {
+      auto urdfLink = model->getLink(urdfLinkIx);
+      if (!urdfLink->m_visualArray.empty()) {
+        int bulletLinkIx =
+            u2b->cache->m_urdfLinkIndices2BulletLinkIndices[urdfLinkIx];
+        ArticulatedLink& linkObject = articulatedObject->getLink(bulletLinkIx);
+        ESP_CHECK(
+            attachLinkGeometry(&linkObject, urdfLink, drawables, lightSetup),
+            "BulletPhysicsManager::addArticulatedObjectFromURDF(): Failed to "
+            "instance render asset (attachGeometry) for link"
+                << urdfLinkIx << ".");
+        linkObject.node().computeCumulativeBB();
+      }
     }
   }