diff --git a/CHANGELOG.md b/CHANGELOG.md index e40823ad18..4119db90f4 100644 --- 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 index ef3f02d124..8758a94c33 100644 --- 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 index 5a1b1a44e7..30a63430e5 100644 --- a/src/io/util.cpp +++ b/src/io/util.cpp @@ -8,7 +8,7 @@ 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; @@ -16,19 +16,22 @@ dart::dynamics::SkeletonPtr loadSkeletonFromURDF( = 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(skeleton->getRootJoint(0)); + if (!transform.matrix().isIdentity()) + { + auto freeJoint + = dynamic_cast(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 index c6779234f6..bb443e8594 100644 --- 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 new file mode 100644 index 0000000000..533bb3889e --- /dev/null +++ b/tests/io/test_loadSkeletonFromURDF.cpp @@ -0,0 +1,146 @@ +#include +#include +#include +#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(); + + 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()); + + auto boxShape = static_cast(shape.get()); + EXPECT_EIGEN_EQUAL( + boxShape->getSize(), Eigen::Vector3d(0.0254, 0.0254, 0.0254), EPS); + } + + auto joint = dynamic_cast(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(); + + 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()); + + auto boxShape = static_cast(shape.get()); + EXPECT_EIGEN_EQUAL( + boxShape->getSize(), Eigen::Vector3d(0.0254, 0.0254, 0.0254), EPS); + } + + auto joint = dynamic_cast(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(); + + 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()); + + auto boxShape = static_cast(shape.get()); + EXPECT_EIGEN_EQUAL( + boxShape->getSize(), Eigen::Vector3d(50.0, 50.0, 0.05), EPS); + } + + auto joint = dynamic_cast(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(); + + 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 new file mode 100644 index 0000000000..e37378b683 --- /dev/null +++ b/tests/resources/urdf/objects/block.urdf @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/tests/resources/urdf/objects/ground.urdf b/tests/resources/urdf/objects/ground.urdf new file mode 100644 index 0000000000..a0d0a2b506 --- /dev/null +++ b/tests/resources/urdf/objects/ground.urdf @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +