Articulated object skinning

data/test_assets/urdf/skinned_prism.ao_config.json

@@ -0,0 +1,4 @@
+{
+    "render_asset": "../objects/skinned_prism.glb",
+    "debug_render_primitives": false
+}

data/test_assets/urdf/skinned_prism.urdf

@@ -0,0 +1,107 @@
+<?xml version="1.0" ?>
+<robot name="skinned_prism">
+  <material name="mat">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+
+  <link name="A">
+    <visual>
+      <material name="mat"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.05"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.05"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="B">
+    <visual>
+      <material name="mat"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.05"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.05"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="C">
+    <visual>
+      <material name="mat"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.05"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.05"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="D">
+    <visual>
+      <material name="mat"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.05"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.05"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="E">
+    <visual>
+      <material name="mat"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.05"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.05"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="B" type="spherical">
+    <parent link="A"/>
+    <child link="B"/>
+    <origin xyz="0 0 1.5" rpy="0 0 0"/>
+  </joint>
+  <joint name="C" type="spherical">
+    <parent link="B"/>
+    <child link="C"/>
+    <origin xyz="0 0 3" rpy="0 0 0"/>
+  </joint>
+  <joint name="D" type="spherical">
+    <parent link="C"/>
+    <child link="D"/>
+    <origin xyz="0 0 4.5" rpy="0 0 0"/>
+  </joint>
+  <joint name="E" type="spherical">
+    <parent link="D"/>
+    <child link="E"/>
+    <origin xyz="0 0 6" rpy="0 0 0"/>
+  </joint>
+</robot>

src/esp/assets/MeshMetaData.h

@@ -45,6 +45,9 @@ struct MeshTransformNode {
 
   /** @brief Default constructor. */
   MeshTransformNode() = default;
+
+  /** @brief Node name in the original file. */
+  std::string name{};
 };
 
 /**
@@ -74,6 +77,10 @@ struct MeshMetaData {
   std::pair<start, end> textureIndex =
       std::make_pair(ID_UNDEFINED, ID_UNDEFINED);
 
+  /** @brief Index range (inclusive) of skin data for the asset in the global
+   * asset datastructure. */
+  std::pair<start, end> skinIndex = std::make_pair(ID_UNDEFINED, ID_UNDEFINED);
+
   /** @brief The root of the mesh component transformation hierarchy tree which
    * stores the relationship between components of the asset.*/
   MeshTransformNode root;
@@ -116,6 +123,19 @@ struct MeshMetaData {
     textureIndex.second = textureEnd;
   }
 
+  /**
+   * @brief Sets the skin indices for the asset. See @ref
+   * ResourceManager::skins_.
+   * @param skinStart First index for asset skin data in the global
+   * skin datastructure.
+   * @param skinEnd Final index for asset skin data in the global skin
+   * datastructure.
+   */
+  void setSkinIndices(int skinStart, int skinEnd) {
+    skinIndex.first = skinStart;
+    skinIndex.second = skinEnd;
+  }
+
   /**
    * @brief Set the root frame orientation based on passed frame
    * @param frame target frame in world space

src/esp/assets/RenderAssetInstanceCreationInfo.h

@@ -9,9 +9,14 @@
 #include <Corrade/Containers/Optional.h>
 #include <Magnum/Magnum.h>
 #include <Magnum/Math/Vector3.h>
+
+#include <memory>
 #include <string>
 
 namespace esp {
+namespace physics {
+class ArticulatedObject;
+}
 namespace assets {
 
 // parameters to control how a render asset instance is created
@@ -44,6 +49,7 @@ struct RenderAssetInstanceCreationInfo {
   Corrade::Containers::Optional<Magnum::Vector3> scale;
   Flags flags;
   std::string lightSetupKey;
+  std::shared_ptr<physics::ArticulatedObject> rig;
 };
 
 }  // namespace assets

src/esp/assets/ResourceManager.cpp

@@ -44,6 +44,7 @@
 #include <Magnum/Trade/PbrMetallicRoughnessMaterialData.h>
 #include <Magnum/Trade/PhongMaterialData.h>
 #include <Magnum/Trade/SceneData.h>
+#include <Magnum/Trade/SkinData.h>
 #include <Magnum/Trade/TextureData.h>
 #include <Magnum/VertexFormat.h>
 
@@ -59,6 +60,7 @@
 #include "esp/gfx/GenericDrawable.h"
 #include "esp/gfx/MaterialUtil.h"
 #include "esp/gfx/PbrDrawable.h"
+#include "esp/gfx/SkinData.h"
 #include "esp/gfx/replay/Recorder.h"
 #include "esp/io/Json.h"
 #include "esp/io/URDFParser.h"
@@ -1756,6 +1758,8 @@ bool ResourceManager::loadRenderAssetGeneral(const AssetInfo& info) {
     loadMaterials(*fileImporter_, loadedAssetData);
   }
   loadMeshes(*fileImporter_, loadedAssetData);
+  loadSkins(*fileImporter_, loadedAssetData);
+
   auto inserted = resourceDict_.emplace(filename, std::move(loadedAssetData));
   MeshMetaData& meshMetaData = inserted.first->second.meshMetaData;
 
@@ -1794,6 +1798,7 @@ bool ResourceManager::loadRenderAssetGeneral(const AssetInfo& info) {
        scene->parentsAsArray()) {
     nodes[parent.first()].emplace();
     nodes[parent.first()]->componentID = parent.first();
+    nodes[parent.first()]->name = fileImporter_->objectName(parent.first());
   }
 
   // Set transformations. Objects that are not part of the hierarchy are
@@ -1879,14 +1884,31 @@ scene::SceneNode* ResourceManager::createRenderAssetInstanceGeneralPrimitive(
                                       : scene::SceneNodeType::OBJECT;
   bool computeAbsoluteAABBs = creation.isStatic();
 
-  addComponent(loadedAssetData.meshMetaData,       // mesh metadata
-               newNode,                            // parent scene node
-               creation.lightSetupKey,             // lightSetup key
-               drawables,                          // drawable group
-               loadedAssetData.meshMetaData.root,  // mesh transform node
+  // If the object has a skin and a rig (articulated object), link them together
+  // such as the model bones are driven by the articulated object links.
+  std::shared_ptr<gfx::InstanceSkinData> instanceSkinData = nullptr;
+  const auto& meshMetaData = loadedAssetData.meshMetaData;
+  if (creation.rig && meshMetaData.skinIndex.first != ID_UNDEFINED) {
+    ESP_CHECK(
+        !skins_.empty(),
+        "Cannot instantiate skinned model because no skin data is imported.");
+    const auto& skinData = skins_[meshMetaData.skinIndex.first];
+    instanceSkinData = std::make_shared<gfx::InstanceSkinData>(skinData);
+    mapSkinnedModelToArticulatedObject(meshMetaData.root, creation.rig,
+                                       instanceSkinData);
+    ESP_CHECK(instanceSkinData->rootJointId != ID_UNDEFINED,
+              "Could not map skinned model to articulated object.");
+  }
+
+  addComponent(meshMetaData,            // mesh metadata
+               newNode,                 // parent scene node
+               creation.lightSetupKey,  // lightSetup key
+               drawables,               // drawable group
+               meshMetaData.root,       // mesh transform node
                visNodeCache,  // a vector of scene nodes, the visNodeCache
                computeAbsoluteAABBs,  // compute absolute AABBs
-               staticDrawableInfo);   // a vector of static drawable info
+               staticDrawableInfo,    // a vector of static drawable info
+               instanceSkinData);     // instance skinning data
 
   if (computeAbsoluteAABBs) {
     // now compute aabbs by constructed staticDrawableInfo
@@ -2444,6 +2466,34 @@ void ResourceManager::loadMeshes(Importer& importer,
   }
 }  // ResourceManager::loadMeshes
 
+void ResourceManager::loadSkins(Importer& importer,
+                                LoadedAssetData& loadedAssetData) {
+  if (importer.skin3DCount() == 0)
+    return;
+
+  const int skinStart = nextSkinID_;
+  const int skinEnd = skinStart + importer.skin3DCount() - 1;
+  nextSkinID_ = skinEnd + 1;
+  loadedAssetData.meshMetaData.setSkinIndices(skinStart, skinEnd);
+
+  for (int iSkin = 0; iSkin < importer.skin3DCount(); ++iSkin) {
+    auto skinData = std::make_shared<gfx::SkinData>();
+
+    Cr::Containers::Optional<Mn::Trade::SkinData3D> skin =
+        importer.skin3D(iSkin);
+    CORRADE_INTERNAL_ASSERT(skin);
+
+    // Cache bone names for later association with instance transforms
+    for (auto jointIt : skin->joints()) {
+      const auto gfxBoneName = fileImporter_->objectName(jointIt);
+      skinData->boneNameJointIdMap[gfxBoneName] = jointIt;
+    }
+
+    skinData->skin = std::make_shared<Mn::Trade::SkinData3D>(std::move(*skin));
+    skins_.emplace(skinStart + iSkin, std::move(skinData));
+  }
+}  // ResourceManager::loadSkins
+
 Mn::Image2D ResourceManager::convertRGBToSemanticId(
     const Mn::ImageView2D& srcImage,
     Cr::Containers::Array<Mn::UnsignedShort>& clrToSemanticId) {
@@ -2785,7 +2835,8 @@ void ResourceManager::addComponent(
     const MeshTransformNode& meshTransformNode,
     std::vector<scene::SceneNode*>& visNodeCache,
     bool computeAbsoluteAABBs,
-    std::vector<StaticDrawableInfo>& staticDrawableInfo) {
+    std::vector<StaticDrawableInfo>& staticDrawableInfo,
+    const std::shared_ptr<gfx::InstanceSkinData>& skinData /* = nullptr */) {
   // Add the object to the scene and set its transformation
   scene::SceneNode& node = parent.createChild();
   visNodeCache.push_back(&node);
@@ -2830,7 +2881,8 @@ void ResourceManager::addComponent(
                    node,                // scene node
                    lightSetupKey,       // lightSetup Key
                    materialKey,         // material key
-                   drawables);          // drawable group
+                   drawables,           // drawable group
+                   skinData);           // instance skinning data
 
     // compute the bounding box for the mesh we are adding
     if (computeAbsoluteAABBs) {
@@ -2849,10 +2901,53 @@ void ResourceManager::addComponent(
                  child,          // mesh transform node
                  visNodeCache,   // a vector of scene nodes, the visNodeCache
                  computeAbsoluteAABBs,  // compute absolute aabbs
-                 staticDrawableInfo);   // a vector of static drawable info
+                 staticDrawableInfo,    // a vector of static drawable info
+                 skinData);             // instance skinning data
   }
 }  // addComponent
 
+void ResourceManager::mapSkinnedModelToArticulatedObject(
+    const MeshTransformNode& meshTransformNode,
+    const std::shared_ptr<physics::ArticulatedObject>& rig,
+    const std::shared_ptr<gfx::InstanceSkinData>& skinData) {
+  // Find skin joint ID that matches the node
+  const auto& gfxBoneName = meshTransformNode.name;
+  const auto& boneNameJointIdMap = skinData->skinData->boneNameJointIdMap;
+  const auto jointIt = boneNameJointIdMap.find(gfxBoneName);
+  if (jointIt != boneNameJointIdMap.end()) {
+    int jointId = jointIt->second;
+
+    // Find articulated object link ID that matches the node
+    const auto& linkIds = rig->getLinkIdsWithBase();
+    const auto linkId =
+        std::find_if(linkIds.begin(), linkIds.end(),
+                     [&](int i) { return gfxBoneName == rig->getLinkName(i); });
+
+    // Map the articulated object link associated with the skin joint
+    if (linkId != linkIds.end()) {
+      auto* articulatedObjectNode = &rig->getLink(*linkId.base()).node();
+      skinData->jointIdToArticulatedObjectNode[jointId] = articulatedObjectNode;
+
+      // This node will be used for rendering.
+      auto& transformNode = articulatedObjectNode->createChild();
+
+      // Mapping
+      skinData->jointIdToTransformNode[jointId] = &transformNode;
+      skinData->localTransforms[jointId] =
+          meshTransformNode.transformFromLocalToParent;
+
+      // First node found is the root
+      if (skinData->rootJointId == ID_UNDEFINED) {
+        skinData->rootJointId = jointId;
+      }
+    }
+  }
+
+  for (const auto& child : meshTransformNode.children) {
+    mapSkinnedModelToArticulatedObject(child, rig, skinData);
+  }
+}  // mapSkinnedModelToArticulatedObject
+
 void ResourceManager::addPrimitiveToDrawables(int primitiveID,
                                               scene::SceneNode& node,
                                               DrawableGroup* drawables) {
@@ -2877,12 +2972,14 @@ void ResourceManager::removePrimitiveMesh(int primitiveID) {
   primitive_meshes_.erase(primMeshIter);
 }
 
-void ResourceManager::createDrawable(Mn::GL::Mesh* mesh,
-                                     gfx::Drawable::Flags& meshAttributeFlags,
-                                     scene::SceneNode& node,
-                                     const Mn::ResourceKey& lightSetupKey,
-                                     const Mn::ResourceKey& materialKey,
-                                     DrawableGroup* group /* = nullptr */) {
+void ResourceManager::createDrawable(
+    Mn::GL::Mesh* mesh,
+    gfx::Drawable::Flags& meshAttributeFlags,
+    scene::SceneNode& node,
+    const Mn::ResourceKey& lightSetupKey,
+    const Mn::ResourceKey& materialKey,
+    DrawableGroup* group /* = nullptr */,
+    const std::shared_ptr<gfx::InstanceSkinData>& skinData /* = nullptr */) {
   const auto& materialDataType =
       shaderManager_.get<gfx::MaterialData>(materialKey)->type;
   switch (materialDataType) {
@@ -2896,7 +2993,8 @@ void ResourceManager::createDrawable(Mn::GL::Mesh* mesh,
           shaderManager_,      // shader manager
           lightSetupKey,       // lightSetup key
           materialKey,         // material key
-          group);              // drawable group
+          group,               // drawable group
+          skinData);           // instance skinning data
       break;
     case gfx::MaterialDataType::Pbr:
       node.addFeature<gfx::PbrDrawable>(
@@ -2908,6 +3006,7 @@ void ResourceManager::createDrawable(Mn::GL::Mesh* mesh,
           group,               // drawable group
           activePbrIbl_ >= 0 ? pbrImageBasedLightings_[activePbrIbl_].get()
                              : nullptr);  // pbr image based lighting
+      // TODO: Carry skinData to PbrDrawable.
       break;
   }