diff --git a/include/aikido/io.hpp b/include/aikido/io.hpp index 8ea5e69b86..eb4d576706 100644 --- a/include/aikido/io.hpp +++ b/include/aikido/io.hpp @@ -1,3 +1,4 @@ #include "io/CatkinResourceRetriever.hpp" #include "io/KinBodyParser.hpp" +#include "io/util.hpp" #include "io/yaml.hpp" diff --git a/include/aikido/io/util.hpp b/include/aikido/io/util.hpp new file mode 100644 index 0000000000..ef3f02d124 --- /dev/null +++ b/include/aikido/io/util.hpp @@ -0,0 +1,23 @@ +#ifndef AIKIDO_IO_UTIL_HPP_ +#define AIKIDO_IO_UTIL_HPP_ + +#include + +namespace aikido { +namespace io { + +/// Load a DART Skeleton from a URDF and set its pose. +/// +/// \param[in] retriever DART retriever to resolve the URI +/// \param[in] uri URI to the object URDF +/// \param[in] transform Initial transform for the Skeleton +/// \return the created Skeleton +dart::dynamics::SkeletonPtr loadSkeletonFromURDF( + const dart::common::ResourceRetrieverPtr& retriever, + const std::string& uri, + const Eigen::Isometry3d& transform = Eigen::Isometry3d::Identity()); + +} // namespace io +} // namespace aikido + +#endif // AIKIDO_IO_UTIL_HPP_ diff --git a/src/io/CMakeLists.txt b/src/io/CMakeLists.txt index 90b64ffba3..2d8b7f975b 100644 --- a/src/io/CMakeLists.txt +++ b/src/io/CMakeLists.txt @@ -29,6 +29,7 @@ set(sources CatkinResourceRetriever.cpp KinBodyParser.cpp yaml.cpp + util.cpp ) add_library("${PROJECT_NAME}_io" SHARED ${sources}) diff --git a/src/io/util.cpp b/src/io/util.cpp new file mode 100644 index 0000000000..5a1b1a44e7 --- /dev/null +++ b/src/io/util.cpp @@ -0,0 +1,37 @@ +#include "aikido/io/util.hpp" +#include +#include "aikido/io/CatkinResourceRetriever.hpp" + +namespace aikido { +namespace io { + +//============================================================================== +dart::dynamics::SkeletonPtr loadSkeletonFromURDF( + const dart::common::ResourceRetrieverPtr& retriever, + const std::string& 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 + "'"); + + if (skeleton->getNumJoints() == 0) + throw std::runtime_error("Skeleton is empty."); + + auto freeJoint + = dynamic_cast(skeleton->getRootJoint(0)); + + if (!freeJoint) + throw std::runtime_error( + "Unable to cast Skeleton's root joint to FreeJoint."); + + freeJoint->setTransform(transform); + + return skeleton; +} + +} // namespace io +} // namespace aikido