Add skinned articulated object test.

facebookresearch · Apr 20, 2023 · ef1fb4e · ef1fb4e
1 parent e96d590
commit ef1fb4e
Show file tree

Hide file tree

Showing 7 changed files with 183 additions and 2 deletions.
diff --git a/data/test_assets/objects/skinned_prism.glb b/data/test_assets/objects/skinned_prism.glb
diff --git a/data/test_assets/screenshots/SimTestSkinnedAOInitialPose.png b/data/test_assets/screenshots/SimTestSkinnedAOInitialPose.png
diff --git a/data/test_assets/screenshots/SimTestSkinnedAOPose.png b/data/test_assets/screenshots/SimTestSkinnedAOPose.png
diff --git a/data/test_assets/urdf/skinned_prism.ao_config.json b/data/test_assets/urdf/skinned_prism.ao_config.json
@@ -0,0 +1,3 @@
+{
+    "render_asset": "data/test_assets/objects/skinned_prism.glb"
+}
diff --git a/data/test_assets/urdf/skinned_prism.urdf b/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>
diff --git a/src/esp/physics/objectWrappers/ManagedArticulatedObject.h b/src/esp/physics/objectWrappers/ManagedArticulatedObject.h
@@ -66,6 +66,13 @@ class ManagedArticulatedObject
     return {};
   }
 
+  std::vector<int> getLinkIdsWithBase() const {
+    if (auto sp = getObjectReference()) {
+      return sp->getLinkIdsWithBase();
+    }
+    return {};
+  }
+
   std::unordered_map<int, int> getLinkObjectIds() const {
     if (auto sp = getObjectReference()) {
       return sp->getLinkObjectIds();

diff --git a/src/tests/SimTest.cpp b/src/tests/SimTest.cpp
@@ -13,10 +13,12 @@
 #include <Magnum/Magnum.h>
 #include <Magnum/PixelFormat.h>
 #include <string>
+#include <vector>
 
 #include "esp/assets/ResourceManager.h"
 #include "esp/metadata/MetadataMediator.h"
 #include "esp/physics/RigidObject.h"
+#include "esp/physics/objectManagers/ArticulatedObjectManager.h"
 #include "esp/physics/objectManagers/RigidObjectManager.h"
 #include "esp/sensor/CameraSensor.h"
 #include "esp/sim/Simulator.h"
@@ -135,9 +137,9 @@ struct SimTest : Cr::TestSuite::Tester {
   void buildingPrimAssetObjectTemplates();
   void addObjectByHandle();
   void addObjectInvertedScale();
-
   void addSensorToObject();
   void createMagnumRenderingOff();
+  void testArticulatedObjectSkinned();
 
   esp::logging::LoggingContext loggingContext_;
   // TODO: remove outlier pixels from image and lower maxThreshold
@@ -182,6 +184,9 @@ SimTest::SimTest() {
             &SimTest::addObjectInvertedScale,
             &SimTest::addSensorToObject}, Cr::Containers::arraySize(SimulatorBuilder) );
   addTests({&SimTest::createMagnumRenderingOff});
+#ifdef ESP_BUILD_WITH_BULLET
+  addTests({&SimTest::testArticulatedObjectSkinned});
+#endif
   // clang-format on
 }
 void SimTest::basic() {
@@ -661,7 +666,7 @@ void SimTest::buildingPrimAssetObjectTemplates() {
 
     CORRADE_COMPARE_AS(newHandle, origCylinderHandle,
                        Cr::TestSuite::Compare::NotEqual);
-    // set bogus file directory, to validate that copy is reggistered
+    // set bogus file directory, to validate that copy is registered
     primAttr->setFileDirectory("test0");
     // register new attributes
     int idx = assetAttribsMgr->registerObject(primAttr);
@@ -1013,4 +1018,63 @@ void SimTest::createMagnumRenderingOff() {
 
 }  // namespace
 
+void SimTest::testArticulatedObjectSkinned() {
+  ESP_DEBUG() << "Starting Test : testArticulatedObjectSkinned";
+
+  const std::string urdfFile = "data/test_assets/urdf/skinned_prism.urdf";
+
+  // create a simulator
+  SimulatorConfiguration simConfig{};
+  simConfig.activeSceneName = "";
+  simConfig.enablePhysics = true;
+  simConfig.physicsConfigFile = physicsConfigFile;
+  simConfig.createRenderer = true;
+  auto simulator = Simulator::create_unique(simConfig);
+  auto aoManager = simulator->getArticulatedObjectManager();
+
+  CORRADE_COMPARE(aoManager->getNumObjects(), 0);
+  auto ao = aoManager->addArticulatedObjectFromURDF(urdfFile);
+  CORRADE_COMPARE(aoManager->getNumObjects(), 1);
+
+  CORRADE_VERIFY(ao);
+  CORRADE_COMPARE(ao->getNumLinks(), 4);
+
+  const auto linkIds = ao->getLinkIdsWithBase();
+
+  auto linkA = ao->getLink(linkIds[0]);
+  CORRADE_VERIFY(linkA->linkName == "A");
+  auto linkB = ao->getLink(linkIds[1]);
+  CORRADE_VERIFY(linkB->linkName == "B");
+  auto linkC = ao->getLink(linkIds[2]);
+  CORRADE_VERIFY(linkC->linkName == "C");
+  auto linkD = ao->getLink(linkIds[3]);
+  CORRADE_VERIFY(linkD->linkName == "D");
+  auto linkE = ao->getLink(linkIds[4]);
+  CORRADE_VERIFY(linkE->linkName == "E");
+
+  ao->setTranslation({1.f, -3.f, -6.f});
+
+  checkPinholeCameraRGBAObservation(
+      *simulator, "SimTestSkinnedAOInitialPose.png", maxThreshold, 0.71f);
+
+  const auto rot = Mn::Quaternion::rotation(
+      Mn::Deg(25.f), Mn::Vector3(0.f, 1.f, 0.f).normalized());
+  std::vector<float> jointPos{};
+  for (int i = 0; i < 4; ++i) {
+    const auto rotData = rot.data();
+    jointPos.push_back(rotData[0]);  // x
+    jointPos.push_back(rotData[1]);  // y
+    jointPos.push_back(rotData[2]);  // z
+    jointPos.push_back(rotData[3]);  // w
+  }
+  ao->setJointPositions(jointPos);
+
+  checkPinholeCameraRGBAObservation(*simulator, "SimTestSkinnedAOPose.png",
+                                    maxThreshold, 0.71f);
+
+  aoManager->removeAllObjects();
+  CORRADE_COMPARE(aoManager->getNumObjects(), 0);
+
+}  // SimTest::testArticulatedObjectSkinned
+
 CORRADE_TEST_MAIN(SimTest)