Minor changes to aikido::io::loadSkeletonFromURDF. (#401)

* Take DART Uri in loadSkeletonFromURDF. Only try to convert to a FreeJoint if the provided transform is not Identity. * Update CHANGELOG.md. * Add tests for loadSkeletonFromURDF.
personalrobotics · May 9, 2018 · abd33a1 · abd33a1
1 parent 564983b
commit abd33a1
Show file tree

Hide file tree

Showing 7 changed files with 217 additions and 9 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -41,6 +41,10 @@
 
   * Added Robot, Manipulator, Hand interfaces, and ConcreteRobot, ConcreteManipulator classes: [#325](https://github.com/personalrobotics/aikido/pull/325), [#392](https://github.com/personalrobotics/aikido/pull/392)
 
+* IO
+
+  * Added loadSkeletonFromURDF convenience function: [#388](https://github.com/personalrobotics/aikido/pull/388), [#401](https://github.com/personalrobotics/aikido/pull/401)
+
 * Build & Testing & ETC
 
   * Fixed Eigen memory alignment issues on 32bit Ubuntu: [#368](https://github.com/personalrobotics/aikido/pull/368)

diff --git a/include/aikido/io/util.hpp b/include/aikido/io/util.hpp
@@ -14,7 +14,7 @@ namespace io {
 /// \return the created Skeleton
 dart::dynamics::SkeletonPtr loadSkeletonFromURDF(
     const dart::common::ResourceRetrieverPtr& retriever,
-    const std::string& uri,
+    const dart::common::Uri& uri,
     const Eigen::Isometry3d& transform = Eigen::Isometry3d::Identity());
 
 } // namespace io

diff --git a/src/io/util.cpp b/src/io/util.cpp
@@ -8,27 +8,30 @@ namespace io {
 //==============================================================================
 dart::dynamics::SkeletonPtr loadSkeletonFromURDF(
     const dart::common::ResourceRetrieverPtr& retriever,
-    const std::string& uri,
+    const dart::common::Uri& uri,
     const Eigen::Isometry3d& transform)
 {
   dart::utils::DartLoader urdfLoader;
   const dart::dynamics::SkeletonPtr skeleton
       = urdfLoader.parseSkeleton(uri, retriever);
 
   if (!skeleton)
-    throw std::runtime_error("Unable to load '" + uri + "'");
+    throw std::runtime_error("Unable to load '" + uri.toString() + "'");
 
   if (skeleton->getNumJoints() == 0)
     throw std::runtime_error("Skeleton is empty.");
 
-  auto freeJoint
-      = dynamic_cast<dart::dynamics::FreeJoint*>(skeleton->getRootJoint(0));
+  if (!transform.matrix().isIdentity())
+  {
+    auto freeJoint
+        = dynamic_cast<dart::dynamics::FreeJoint*>(skeleton->getRootJoint());
 
-  if (!freeJoint)
-    throw std::runtime_error(
-        "Unable to cast Skeleton's root joint to FreeJoint.");
+    if (!freeJoint)
+      throw std::runtime_error(
+          "Unable to cast Skeleton's root joint to FreeJoint.");
 
-  freeJoint->setTransform(transform);
+    freeJoint->setTransform(transform);
+  }
 
   return skeleton;
 }

diff --git a/tests/io/CMakeLists.txt b/tests/io/CMakeLists.txt
@@ -36,5 +36,10 @@ target_link_libraries(test_KinBodyParser "${PROJECT_NAME}_io")
 target_compile_definitions(test_KinBodyParser
   PRIVATE "-DAIKIDO_TEST_RESOURCES_PATH=${PROJECT_SOURCE_DIR}/tests/resources")
 
+aikido_add_test(test_loadSkeletonFromURDF test_loadSkeletonFromURDF.cpp)
+target_link_libraries(test_loadSkeletonFromURDF "${PROJECT_NAME}_io")
+target_compile_definitions(test_loadSkeletonFromURDF
+  PRIVATE "-DAIKIDO_TEST_RESOURCES_PATH=${PROJECT_SOURCE_DIR}/tests/resources")
+
 aikido_add_test(test_yaml_extension test_yaml_extension.cpp)
 target_link_libraries(test_yaml_extension "${PROJECT_NAME}_io")
diff --git a/tests/io/test_loadSkeletonFromURDF.cpp b/tests/io/test_loadSkeletonFromURDF.cpp
@@ -0,0 +1,146 @@
+#include <dart/utils/utils.hpp>
+#include <gtest/gtest.h>
+#include <aikido/io/util.hpp>
+#include "eigen_tests.hpp"
+
+#define STR_EXPAND(tok) #tok
+#define STR(tok) STR_EXPAND(tok)
+
+using namespace aikido::io;
+
+static constexpr double EPS = 1e-6;
+
+static std::string TEST_RESOURCES_PATH = STR(AIKIDO_TEST_RESOURCES_PATH);
+
+//==============================================================================
+TEST(LoadURDF, LoadBox)
+{
+  auto uri = std::string("file://") + TEST_RESOURCES_PATH
+             + std::string("/urdf/objects/block.urdf");
+  auto retriever = std::make_shared<dart::common::LocalResourceRetriever>();
+
+  EXPECT_TRUE(retriever->exists(uri));
+
+  auto skel = loadSkeletonFromURDF(retriever, uri);
+  EXPECT_TRUE(skel != nullptr);
+  EXPECT_EQ(1u, skel->getNumBodyNodes());
+  EXPECT_EQ(1u, skel->getNumJoints());
+
+  auto bodyNode = skel->getBodyNode(0);
+  EXPECT_TRUE(bodyNode != nullptr);
+
+  // Two ShapeNodes: visual and collision
+  auto shapeNodes = bodyNode->getShapeNodes();
+  EXPECT_EQ(2u, shapeNodes.size());
+
+  for (std::size_t i = 0; i < bodyNode->getNumShapeNodes(); ++i)
+  {
+    auto shape = bodyNode->getShapeNode(i)->getShape();
+
+    EXPECT_TRUE(shape->is<dart::dynamics::BoxShape>());
+
+    auto boxShape = static_cast<dart::dynamics::BoxShape*>(shape.get());
+    EXPECT_EIGEN_EQUAL(
+        boxShape->getSize(), Eigen::Vector3d(0.0254, 0.0254, 0.0254), EPS);
+  }
+
+  auto joint = dynamic_cast<dart::dynamics::FreeJoint*>(skel->getRootJoint());
+  EXPECT_TRUE(joint != nullptr);
+}
+
+//==============================================================================
+TEST(LoadURDF, LoadBoxWithTransform)
+{
+  auto uri = std::string("file://") + TEST_RESOURCES_PATH
+             + std::string("/urdf/objects/block.urdf");
+  auto retriever = std::make_shared<dart::common::LocalResourceRetriever>();
+
+  EXPECT_TRUE(retriever->exists(uri));
+
+  auto tf = Eigen::Isometry3d::Identity();
+  tf.translation()[0] = 1.0;
+  tf.translation()[1] = 1.0;
+
+  auto skel = loadSkeletonFromURDF(retriever, uri, tf);
+  EXPECT_TRUE(skel != nullptr);
+  EXPECT_EQ(1u, skel->getNumBodyNodes());
+  EXPECT_EQ(1u, skel->getNumJoints());
+
+  auto bodyNode = skel->getBodyNode(0);
+  EXPECT_TRUE(bodyNode != nullptr);
+
+  // Two ShapeNodes: visual and collision
+  auto shapeNodes = bodyNode->getShapeNodes();
+  EXPECT_EQ(2u, shapeNodes.size());
+
+  for (std::size_t i = 0; i < bodyNode->getNumShapeNodes(); ++i)
+  {
+    auto shape = bodyNode->getShapeNode(i)->getShape();
+
+    EXPECT_TRUE(shape->is<dart::dynamics::BoxShape>());
+
+    auto boxShape = static_cast<dart::dynamics::BoxShape*>(shape.get());
+    EXPECT_EIGEN_EQUAL(
+        boxShape->getSize(), Eigen::Vector3d(0.0254, 0.0254, 0.0254), EPS);
+  }
+
+  auto joint = dynamic_cast<dart::dynamics::FreeJoint*>(skel->getRootJoint());
+  EXPECT_TRUE(joint != nullptr);
+
+  EXPECT_EIGEN_EQUAL(
+      tf.matrix(),
+      joint->getChildBodyNode()->getWorldTransform().matrix(),
+      EPS);
+}
+
+//==============================================================================
+TEST(LoadURDF, LoadGround)
+{
+  auto uri = std::string("file://") + TEST_RESOURCES_PATH
+             + std::string("/urdf/objects/ground.urdf");
+  auto retriever = std::make_shared<dart::common::LocalResourceRetriever>();
+
+  EXPECT_TRUE(retriever->exists(uri));
+
+  auto skel = loadSkeletonFromURDF(retriever, uri);
+  EXPECT_TRUE(skel != nullptr);
+  EXPECT_EQ(1u, skel->getNumBodyNodes());
+  EXPECT_EQ(1u, skel->getNumJoints());
+
+  auto bodyNode = skel->getBodyNode(0);
+  EXPECT_TRUE(bodyNode != nullptr);
+
+  // Two ShapeNodes: visual and collision
+  auto shapeNodes = bodyNode->getShapeNodes();
+  EXPECT_EQ(2u, shapeNodes.size());
+
+  for (std::size_t i = 0; i < bodyNode->getNumShapeNodes(); ++i)
+  {
+    auto shape = bodyNode->getShapeNode(i)->getShape();
+
+    EXPECT_TRUE(shape->is<dart::dynamics::BoxShape>());
+
+    auto boxShape = static_cast<dart::dynamics::BoxShape*>(shape.get());
+    EXPECT_EIGEN_EQUAL(
+        boxShape->getSize(), Eigen::Vector3d(50.0, 50.0, 0.05), EPS);
+  }
+
+  auto joint = dynamic_cast<dart::dynamics::WeldJoint*>(skel->getRootJoint());
+  EXPECT_TRUE(joint != nullptr);
+}
+
+//==============================================================================
+TEST(LoadURDF, LoadGroundWithTransform)
+{
+  auto uri = std::string("file://") + TEST_RESOURCES_PATH
+             + std::string("/urdf/objects/ground.urdf");
+  auto retriever = std::make_shared<dart::common::LocalResourceRetriever>();
+
+  EXPECT_TRUE(retriever->exists(uri));
+
+  auto tf = Eigen::Isometry3d::Identity();
+  tf.translation()[0] = 1.0;
+  tf.translation()[1] = 1.0;
+
+  EXPECT_THROW(loadSkeletonFromURDF(retriever, uri, tf), std::runtime_error);
+}
diff --git a/tests/resources/urdf/objects/block.urdf b/tests/resources/urdf/objects/block.urdf
@@ -0,0 +1,19 @@
+<robot name="blue_wood_block_1inx1in">
+  <link name="blue_wood_block_1inx1in">
+    <collision>
+      <origin xyz="0 0 0.0127" />
+      <geometry>
+        <box size="0.0254 0.0254 0.0254" />
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0 0 0.0127" />
+      <material name="c">
+        <color rgba="0 0 1.0 1.0" />
+      </material>
+      <geometry>
+        <box size="0.0254 0.0254 0.0254" />
+      </geometry>
+    </visual>
+  </link>
+</robot>
diff --git a/tests/resources/urdf/objects/ground.urdf b/tests/resources/urdf/objects/ground.urdf
@@ -0,0 +1,31 @@
+<robot name="ground">
+  <link name="world"/>
+  <link name="ground_link">
+    <inertial>
+      <mass value="100.0"/>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 -0.025" rpy="0 0 0"/>
+      <material name="color">
+        <color rgba="0.8 0.8 0.8 1"/>
+      </material>
+      <geometry>
+        <box size="50.0 50.0 0.05"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.025" rpy="0 0 0"/>
+      <geometry>
+        <box size="50.0 50.0 0.05"/>
+      </geometry>
+      <contact_coefficients mu="1.0" kp="1.0" kd="1.0"/>
+    </collision>
+  </link>
+  <joint name="ground_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="world"/>
+    <child link="ground_link"/>
+  </joint>
+</robot>