diff --git a/CMakeLists.txt b/CMakeLists.txt index 760a54d99..d820c3776 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -166,6 +166,7 @@ if(MAGNUM_BUILD_DEPRECATED) option(MAGNUM_WITH_TINYGLTFIMPORTER "Build TinyGltfImporter plugin" OFF) endif() option(MAGNUM_WITH_UFBXIMPORTER "Build UfbxImporter plugin" OFF) +option(MAGNUM_WITH_URDFIMPORTER "Build UrdfImporter plugin" OFF) option(MAGNUM_WITH_WEBPIMPORTER "Build WebPImporter plugin" OFF) option(MAGNUM_BUILD_TESTS "Build unit tests" OFF) diff --git a/src/MagnumPlugins/CMakeLists.txt b/src/MagnumPlugins/CMakeLists.txt index 869a6fb79..8cf94751c 100644 --- a/src/MagnumPlugins/CMakeLists.txt +++ b/src/MagnumPlugins/CMakeLists.txt @@ -213,6 +213,10 @@ if(MAGNUM_WITH_UFBXIMPORTER) add_subdirectory(UfbxImporter) endif() +if(MAGNUM_WITH_URDFIMPORTER) + add_subdirectory(UrdfImporter) +endif() + if(MAGNUM_WITH_WEBPIMPORTER) add_subdirectory(WebPImporter) endif() diff --git a/src/MagnumPlugins/UrdfImporter/CMakeLists.txt b/src/MagnumPlugins/UrdfImporter/CMakeLists.txt new file mode 100644 index 000000000..b9c54d3ac --- /dev/null +++ b/src/MagnumPlugins/UrdfImporter/CMakeLists.txt @@ -0,0 +1,68 @@ +# +# This file is part of Magnum. +# +# Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, +# 2020, 2021, 2022 Vladimír Vondruš +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the "Software"), +# to deal in the Software without restriction, including without limitation +# the rights to use, copy, modify, merge, publish, distribute, sublicense, +# and/or sell copies of the Software, and to permit persons to whom the +# Software is furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +# DEALINGS IN THE SOFTWARE. +# + +find_package(Magnum REQUIRED Trade) +find_package(pugixml CONFIG REQUIRED) + +if(MAGNUM_BUILD_PLUGINS_STATIC AND NOT DEFINED MAGNUM_URDFIMPORTER_BUILD_STATIC) + set(MAGNUM_URDFIMPORTER_BUILD_STATIC 1) +endif() + +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/configure.h.cmake + ${CMAKE_CURRENT_BINARY_DIR}/configure.h) + +# UrdfImporter plugin +add_plugin(UrdfImporter + importers + "${MAGNUM_PLUGINS_IMPORTER_DEBUG_BINARY_INSTALL_DIR};${MAGNUM_PLUGINS_IMPORTER_DEBUG_LIBRARY_INSTALL_DIR}" + "${MAGNUM_PLUGINS_IMPORTER_RELEASE_BINARY_INSTALL_DIR};${MAGNUM_PLUGINS_IMPORTER_RELEASE_LIBRARY_INSTALL_DIR}" + UrdfImporter.conf + UrdfImporter.cpp + UrdfImporter.h) +if(MAGNUM_URDFIMPORTER_BUILD_STATIC AND MAGNUM_BUILD_STATIC_PIC) + set_target_properties(UrdfImporter PROPERTIES POSITION_INDEPENDENT_CODE ON) +endif() +target_include_directories(UrdfImporter PUBLIC + ${PROJECT_SOURCE_DIR}/src + ${PROJECT_BINARY_DIR}/src) +target_link_libraries(UrdfImporter PUBLIC + Magnum::Trade + pugixml::pugixml) + +install(FILES UrdfImporter.h ${CMAKE_CURRENT_BINARY_DIR}/configure.h + DESTINATION ${MAGNUM_PLUGINS_INCLUDE_INSTALL_DIR}/UrdfImporter) + +# Automatic static plugin import +if(MAGNUM_URDFIMPORTER_BUILD_STATIC) + install(FILES importStaticPlugin.cpp DESTINATION ${MAGNUM_PLUGINS_INCLUDE_INSTALL_DIR}/UrdfImporter) + target_sources(UrdfImporter INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/importStaticPlugin.cpp) +endif() + +if(MAGNUM_BUILD_TESTS) + add_subdirectory(Test) +endif() + +# MagnumPlugins UrdfImporter target alias for superprojects +add_library(MagnumPlugins::UrdfImporter ALIAS UrdfImporter) diff --git a/src/MagnumPlugins/UrdfImporter/Test/CMakeLists.txt b/src/MagnumPlugins/UrdfImporter/Test/CMakeLists.txt new file mode 100644 index 000000000..fe194131f --- /dev/null +++ b/src/MagnumPlugins/UrdfImporter/Test/CMakeLists.txt @@ -0,0 +1,65 @@ +# +# This file is part of Magnum. +# +# Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, +# 2020, 2021, 2022 Vladimír Vondruš +# +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the "Software"), +# to deal in the Software without restriction, including without limitation +# the rights to use, copy, modify, merge, publish, distribute, sublicense, +# and/or sell copies of the Software, and to permit persons to whom the +# Software is furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +# DEALINGS IN THE SOFTWARE. +# + +# IDE folder in VS, Xcode etc. CMake 3.12+, older versions have only the FOLDER +# property that would have to be set on each target separately. +set(CMAKE_FOLDER "MagnumPlugins/UrdfImporter/Test") + +if(CORRADE_TARGET_EMSCRIPTEN OR CORRADE_TARGET_ANDROID) + set(URDFIMPORTER_TEST_DIR ".") +else() + set(URDFIMPORTER_TEST_DIR ${CMAKE_CURRENT_SOURCE_DIR}) +endif() + +# CMake before 3.8 has broken $ expressions for iOS (see +# https://gitlab.kitware.com/cmake/cmake/merge_requests/404) and since Corrade +# doesn't support dynamic plugins on iOS, this sorta works around that. Should +# be revisited when updating Travis to newer Xcode (xcode7.3 has CMake 3.6). +if(NOT MAGNUM_URDFIMPORTER_BUILD_STATIC) + set(URDFIMPORTER_PLUGIN_FILENAME $) +endif() + +# First replace ${} variables, then $<> generator expressions +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/configure.h.cmake + ${CMAKE_CURRENT_BINARY_DIR}/configure.h.in) +file(GENERATE OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/$/configure.h + INPUT ${CMAKE_CURRENT_BINARY_DIR}/configure.h.in) + +corrade_add_test(UrdfImporterTest UrdfImporterTest.cpp + LIBRARIES Magnum::Trade) +target_include_directories(UrdfImporterTest PRIVATE ${CMAKE_CURRENT_BINARY_DIR}/$) +if(MAGNUM_URDFIMPORTER_BUILD_STATIC) + target_link_libraries(UrdfImporterTest PRIVATE UrdfImporter) +else() + # So the plugins get properly built when building the test + add_dependencies(UrdfImporterTest UrdfImporter) +endif() +if(CORRADE_BUILD_STATIC AND NOT MAGNUM_URDFIMPORTER_BUILD_STATIC) + # CMake < 3.4 does this implicitly, but 3.4+ not anymore (see CMP0065). + # That's generally okay, *except if* the build is static, the executable + # uses a plugin manager and needs to share globals with the plugins (such + # as output redirection and so on). + set_target_properties(UrdfImporterTest PROPERTIES ENABLE_EXPORTS ON) +endif() diff --git a/src/MagnumPlugins/UrdfImporter/Test/UrdfImporterTest.cpp b/src/MagnumPlugins/UrdfImporter/Test/UrdfImporterTest.cpp new file mode 100644 index 000000000..6f7ea5e2c --- /dev/null +++ b/src/MagnumPlugins/UrdfImporter/Test/UrdfImporterTest.cpp @@ -0,0 +1,104 @@ +/* + This file is part of Magnum. + + Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, + 2020, 2021, 2022 Vladimír Vondruš + + Permission is hereby granted, free of charge, to any person obtaining a + copy of this software and associated documentation files (the "Software"), + to deal in the Software without restriction, including without limitation + the rights to use, copy, modify, merge, publish, distribute, sublicense, + and/or sell copies of the Software, and to permit persons to whom the + Software is furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included + in all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + DEALINGS IN THE SOFTWARE. +*/ + +#include +#include +#include +#include + +#include "configure.h" + +namespace Magnum { namespace Trade { namespace Test { namespace { + +struct UrdfImporterTest: TestSuite::Tester { + explicit UrdfImporterTest(); + + void openMemory(); + void openTwice(); + void importTwice(); + + /* Explicitly forbid system-wide plugin dependencies */ + PluginManager::Manager _manager{"nonexistent"}; +}; + +/* Shared among all plugins that implement data copying optimizations */ +const struct { + const char* name; + bool(*open)(AbstractImporter&, Containers::ArrayView); +} OpenMemoryData[]{ + {"data", [](AbstractImporter& importer, Containers::ArrayView data) { + /* Copy to ensure the original memory isn't referenced */ + Containers::Array copy{NoInit, data.size()}; + Utility::copy(Containers::arrayCast(data), copy); + return importer.openData(copy); + }}, + {"memory", [](AbstractImporter& importer, Containers::ArrayView data) { + return importer.openMemory(data); + }}, +}; + +UrdfImporterTest::UrdfImporterTest() { + addInstancedTests({&UrdfImporterTest::openMemory}, + Containers::arraySize(OpenMemoryData)); + + addTests({&UrdfImporterTest::openTwice, + &UrdfImporterTest::importTwice}); + + /* Load the plugin directly from the build tree. Otherwise it's static and + already loaded. */ + #ifdef URDFIMPORTER_PLUGIN_FILENAME + CORRADE_INTERNAL_ASSERT_OUTPUT(_manager.load(URDFIMPORTER_PLUGIN_FILENAME) & PluginManager::LoadState::Loaded); + #endif +} + +void UrdfImporterTest::openMemory() { + /* Same as (a subset of) binary() except that it uses openData() & + openMemory() instead of openFile() to test data copying on import */ + + auto&& data = OpenMemoryData[testCaseInstanceId()]; + setTestCaseDescription(data.name); + + Containers::Pointer importer = _manager.instantiate("UrdfImporter"); + // TODO +} + +void UrdfImporterTest::openTwice() { + Containers::Pointer importer = _manager.instantiate("UrdfImporter"); + + // TODO + + /* Shouldn't crash, leak or anything */ +} + +void UrdfImporterTest::importTwice() { + Containers::Pointer importer = _manager.instantiate("UrdfImporter"); + + /* Verify that everything is working the same way on second use */ + // TODO +} + +}}}} + +CORRADE_TEST_MAIN(Magnum::Trade::Test::UrdfImporterTest) diff --git a/src/MagnumPlugins/UrdfImporter/Test/configure.h.cmake b/src/MagnumPlugins/UrdfImporter/Test/configure.h.cmake new file mode 100644 index 000000000..134403324 --- /dev/null +++ b/src/MagnumPlugins/UrdfImporter/Test/configure.h.cmake @@ -0,0 +1,27 @@ +/* + This file is part of Magnum. + + Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, + 2020, 2021, 2022 Vladimír Vondruš + + Permission is hereby granted, free of charge, to any person obtaining a + copy of this software and associated documentation files (the "Software"), + to deal in the Software without restriction, including without limitation + the rights to use, copy, modify, merge, publish, distribute, sublicense, + and/or sell copies of the Software, and to permit persons to whom the + Software is furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included + in all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + DEALINGS IN THE SOFTWARE. +*/ + +#cmakedefine URDFIMPORTER_PLUGIN_FILENAME "${URDFIMPORTER_PLUGIN_FILENAME}" +#define URDFIMPORTER_TEST_DIR "${URDFIMPORTER_TEST_DIR}" diff --git a/src/MagnumPlugins/UrdfImporter/UrdfImporter.conf b/src/MagnumPlugins/UrdfImporter/UrdfImporter.conf new file mode 100644 index 000000000..e69de29bb diff --git a/src/MagnumPlugins/UrdfImporter/UrdfImporter.cpp b/src/MagnumPlugins/UrdfImporter/UrdfImporter.cpp new file mode 100644 index 000000000..df27a36d5 --- /dev/null +++ b/src/MagnumPlugins/UrdfImporter/UrdfImporter.cpp @@ -0,0 +1,536 @@ +/* + This file is part of Magnum. + + Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, + 2020, 2021, 2022 Vladimír Vondruš + + Permission is hereby granted, free of charge, to any person obtaining a + copy of this software and associated documentation files (the "Software"), + to deal in the Software without restriction, including without limitation + the rights to use, copy, modify, merge, publish, distribute, sublicense, + and/or sell copies of the Software, and to permit persons to whom the + Software is furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included + in all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + DEALINGS IN THE SOFTWARE. +*/ + +#include "UrdfImporter.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define PUGIXML_NO_STL // TODO this will probably cause ABI issues +#include "pugixml.hpp" + +/* std::hash specialization to be able to use StringView in unordered_map. + This needs to be some common header because GltfImporter has exactly the + same. */ +namespace std { + // TODO isn't this in Containers already? + template<> struct hash { + std::size_t operator()(const Corrade::Containers::StringView& key) const { + const Corrade::Utility::MurmurHash2 hash; + const Corrade::Utility::HashDigest digest = hash(key.data(), key.size()); + return *reinterpret_cast(digest.byteArray()); + } + }; +} + +namespace Magnum { namespace Trade { + +using namespace Containers::Literals; + +struct UrdfImporter::State { + pugi::xml_document doc; + Containers::Array nodeNames; + std::unordered_map nodesForName; +}; + +UrdfImporter::UrdfImporter() = default; + +UrdfImporter::UrdfImporter(PluginManager::AbstractManager& manager, const Containers::StringView& plugin): AbstractImporter{manager, plugin} {} + +UrdfImporter::~UrdfImporter() = default; + +ImporterFeatures UrdfImporter::doFeatures() const { return ImporterFeature::OpenData; } + +bool UrdfImporter::doIsOpened() const { return !!_in; } + +void UrdfImporter::doClose() { _in = nullptr; } + +void UrdfImporter::doOpenData(Containers::Array&& data, DataFlags) { + Containers::Pointer state{InPlaceInit}; + + const pugi::xml_parse_result result = state->doc.load_buffer(data, data.size()); // TODO options + if(!result) { + Error{} << "Trade::UrdfImporter::openData(): error opening file:" << result.description(); + // TODO offset, etc + return; + } + + /* Gather link and joint names. Use empty names for other nodes in the + hierarchy. */ + for(const pugi::xml_node node: state->doc.root().child("robot").children()) { + if(node.name() == "joint"_s) { + if(const pugi::xml_attribute name = node.attribute("name")) + arrayAppend(state->nodeNames, name.as_string()); + else + arrayAppend(state->nodeNames, ""_s); + + } else if(node.name() == "link"_s) { + if(const pugi::xml_attribute name = node.attribute("name")) + arrayAppend(state->nodeNames, name.as_string()); + else + arrayAppend(state->nodeNames, ""_s); + + if(const pugi::xml_node inertial = node.child("inertial")) { + arrayAppend(state->nodeNames, ""_s); + } + if(const pugi::xml_node visual = node.child("visual")) { + arrayAppend(state->nodeNames, ""_s); + } + if(const pugi::xml_node collision = node.child("collision")) { + arrayAppend(state->nodeNames, ""_s); + } + + } else continue; + } + + /* Build a name->id map from these */ + for(std::size_t i = 0; i != state->nodeNames.size(); ++i) + if(const Containers::StringView name = state->nodeNames[i]) + state->nodesForName.emplace(name, i); + + /* All good */ + _in = std::move(state); +} + +Int UrdfImporter::doDefaultScene() const { return 0; } + +UnsignedInt UrdfImporter::doSceneCount() const { return 1; } + +Containers::String UrdfImporter::doSceneName(UnsignedInt) { + return _in->doc.root().child("robot").attribute("name").as_string(); +} + +Int UrdfImporter::doSceneForName(Containers::StringView) { + return -1; // TODO +} +namespace { + +constexpr auto SceneFieldJointAxis = sceneFieldCustom(1); +constexpr auto SceneFieldJointLimitLower = sceneFieldCustom(2); +constexpr auto SceneFieldJointLimitUpper = sceneFieldCustom(3); +constexpr auto SceneFieldJointLimitEffort = sceneFieldCustom(4); +constexpr auto SceneFieldJointLimitVelocity = sceneFieldCustom(5); +constexpr auto SceneFieldJointDynamicsDamping = sceneFieldCustom(6); +constexpr auto SceneFieldJointDynamicsFriction = sceneFieldCustom(7); +constexpr auto SceneFieldLinkInertialMass = sceneFieldCustom(8); +constexpr auto SceneFieldLinkInertia = sceneFieldCustom(9); +constexpr auto SceneFieldCollisionMesh = sceneFieldCustom(10); + +} + +Containers::String UrdfImporter::doSceneFieldName(const UnsignedInt name) { + switch(name) { + // TODO lowercase names for consistency with custom material attrs etc + #define _c(name) \ + case sceneFieldCustom(SceneField ## name): return #name; + _c(JointAxis) + _c(JointLimitLower) + _c(JointLimitUpper) + _c(JointLimitEffort) + _c(JointLimitVelocity) + _c(JointDynamicsDamping) + _c(JointDynamicsFriction) + _c(LinkInertialMass) + _c(LinkInertia) + _c(CollisionMesh) + #undef _c + } + + return {}; +} + +SceneField UrdfImporter::doSceneFieldForName(Containers::StringView) { + return {}; // TODO +} + +Containers::Optional UrdfImporter::doScene(UnsignedInt) { + /* Gather count of joints and links */ + std::size_t parentCount = 0; + std::size_t transformationCount = 0; + + std::size_t jointAxesCount = 0; + std::size_t jointLimitCount = 0; + std::size_t jointDynamicsCount = 0; + + std::size_t linkInertialCount = 0; + std::size_t linkVisualCount = 0; + std::size_t linkCollisionCount = 0; + + bool hasMaterials = false; + for(const pugi::xml_node node: _in->doc.root().child("robot").children()) { + if(node.name() == "joint"_s) { + ++parentCount; + if(node.child("origin")) ++transformationCount; + + if(node.child("axis")) ++jointAxesCount; + if(node.child("dynamics")) ++jointDynamicsCount; + if(node.child("limit")) ++jointLimitCount; // TODO check if required + + } else if(node.name() == "link"_s) { + ++parentCount; + if(const pugi::xml_node inertial = node.child("inertial")) { + ++parentCount; + ++linkInertialCount; + if(inertial.child("origin")) ++transformationCount; + } + if(const pugi::xml_node visual = node.child("visual")) { + ++parentCount; + ++linkVisualCount; + if(visual.child("origin")) ++transformationCount; + if(visual.child("material")) hasMaterials = true; + } + if(const pugi::xml_node collision = node.child("collision")) { + ++parentCount; + ++linkCollisionCount; // TODO multiple? + if(collision.child("origin")) ++transformationCount; + } + + } else { + Warning{} << "Trade::UrdfImporter::scene(): ignoring unknown node" << node.name(); + } + } + +// !Debug{} << "joints" << jointCount << "axes" << jointAxesCount << "limits" << jointLimitCount << "dynamics" << jointDynamicsCount; +// !Debug{} << "links" << linkCount << "iner" << linkInertialCount << "vs" << linkVisualCount << "col" << linkCollisionCount; + + struct Transformation { + UnsignedInt mapping; + Vector3 translation; + Quaternion rotation; + }; + + struct Parent { + UnsignedInt mapping; + Int parent; + }; + + struct JointAxis { + UnsignedInt mapping; + Vector3 axis; + }; + + struct JointLimit { + UnsignedInt mapping; + Float lower; + Float upper; + Float effort; + Float velocity; + }; + + struct JointDynamics { + UnsignedInt mapping; + Float damping; + Float friction; + }; + + struct LinkInertial { + UnsignedInt mapping; + Float mass; + Matrix3x3 inertia; + }; + + Containers::StridedArrayView1D parents; + Containers::StridedArrayView1D transformations; + + Containers::StridedArrayView1D jointAxes; + Containers::StridedArrayView1D jointLimits; + Containers::StridedArrayView1D jointDynamics; + + Containers::StridedArrayView1D linkInertials; + Containers::ArrayView linkVisualMeshObjects; + Containers::ArrayView linkVisualMeshes; + Containers::ArrayView linkVisualMeshMaterials; + Containers::ArrayView linkCollisionMeshObjects; + Containers::ArrayView linkCollisionMeshes; + + Containers::ArrayTuple data{ + {NoInit, parentCount, parents}, + {NoInit, transformationCount, transformations}, + + {NoInit, jointAxesCount, jointAxes}, + {NoInit, jointLimitCount, jointLimits}, + {NoInit, jointDynamicsCount, jointDynamics}, + + {NoInit, linkInertialCount, linkInertials}, + {NoInit, linkVisualCount, linkVisualMeshObjects}, + {NoInit, linkVisualCount, linkVisualMeshes}, + {NoInit, hasMaterials ? linkVisualCount : 0, linkVisualMeshMaterials}, + {NoInit, linkCollisionCount, linkCollisionMeshObjects}, + {NoInit, linkCollisionCount, linkCollisionMeshes}, + }; + + // TODO fill the data + std::size_t parentOffset = 0; + std::size_t transformationOffset = 0; + std::size_t jointAxesOffset = 0; + std::size_t jointLimitOffset = 0; + std::size_t jointDynamicsOffset = 0; + std::size_t linkInertialOffset = 0; + std::size_t linkVisualOffset = 0; + std::size_t linkCollisionOffset = 0; + for(const pugi::xml_node node: _in->doc.root().child("robot").children()) { + const UnsignedInt id = parentOffset; + + if(node.name() == "joint"_s) { + parents[parentOffset].mapping = id; + parents[parentOffset].parent = -1; // TODO + ++parentOffset; + + if(const pugi::xml_node origin = node.child("origin")) { + transformations[transformationOffset].mapping = id; + + if(const pugi::xml_attribute rpy = origin.attribute("rpy")) { + // TODO hacky, doesn't have error handling + Vector3 data = Utility::ConfigurationValue::fromString(rpy.as_string(), {}); + // TODO is this alright?! + transformations[transformationOffset].rotation = + Quaternion::rotation(Rad(data.z()), Vector3::yAxis())* + Quaternion::rotation(Rad(data.y()), Vector3::zAxis())* + Quaternion::rotation(Rad(data.x()), Vector3::xAxis()); + } else transformations[transformationOffset].rotation = {}; + + if(const pugi::xml_attribute xyz = origin.attribute("xyz")) { + // TODO hacky, doesn't have error handling + transformations[transformationOffset].translation = Utility::ConfigurationValue::fromString(xyz.as_string(), {}); + // is Y up or how?? + } else transformations[transformationOffset].translation = {}; + + ++transformationOffset; + } + + if(node.child("axis")) { + jointAxes[jointAxesOffset].mapping = id; + // TODO + ++jointAxesOffset; + } + if(const pugi::xml_node dynamics = node.child("dynamics")) { + jointDynamics[jointDynamicsOffset].mapping = id; + if(const pugi::xml_attribute damping = dynamics.attribute("damping")) { + jointDynamics[jointDynamicsOffset].damping = damping.as_float(); + } // TODO else? default value? error? what if not float? + // TODO friction? + ++jointDynamicsOffset; + } + if(const pugi::xml_node limit = node.child("limit")) { + jointLimits[jointLimitOffset].mapping = id; + if(const pugi::xml_attribute effort = limit.attribute("effort")) { + jointLimits[jointLimitOffset].effort = effort.as_float(); + } // TODO else? default value? error? what if not float? + if(const pugi::xml_attribute lower = limit.attribute("lower")) { + jointLimits[jointLimitOffset].lower = lower.as_float(); + } // TODO else? default value? error? what if not float? + if(const pugi::xml_attribute upper = limit.attribute("upper")) { + jointLimits[jointLimitOffset].upper = upper.as_float(); + } // TODO else? default value? error? what if not float? + if(const pugi::xml_attribute velocity = limit.attribute("velocity")) { + jointLimits[jointLimitOffset].velocity = velocity.as_float(); + } // TODO else? default value? error? what if not float? + ++jointLimitOffset; + } + + } else if(node.name() == "link"_s) { + parents[parentOffset].mapping = id; + parents[parentOffset].parent = -1; // TODO + ++parentOffset; + + if(const pugi::xml_node inertial = node.child("inertial")) { + const UnsignedInt inertialId = parentOffset; + parents[parentOffset].mapping = inertialId; + parents[parentOffset].parent = id; + ++parentOffset; + + linkInertials[linkInertialOffset].mapping = inertialId; + // TODO + const pugi::xml_node mass = inertial.child("mass"); + pugi::xml_attribute massValue; + if(mass && (massValue = mass.attribute("value"))) { + linkInertials[linkInertialOffset].mass = massValue.as_float(); + } // TODO else default? error if value not present?? what if not float? + ++linkInertialOffset; + + if(const pugi::xml_node origin = inertial.child("origin")) { + transformations[transformationOffset].mapping = inertialId; + // TODO AHHHH DUPLICATES + + if(const pugi::xml_attribute rpy = origin.attribute("rpy")) { + // TODO hacky, doesn't have error handling + Vector3 data = Utility::ConfigurationValue::fromString(rpy.as_string(), {}); + // TODO is this alright?! + transformations[transformationOffset].rotation = + Quaternion::rotation(Rad(data.z()), Vector3::yAxis())* + Quaternion::rotation(Rad(data.y()), Vector3::zAxis())* + Quaternion::rotation(Rad(data.x()), Vector3::xAxis()); + } else transformations[transformationOffset].rotation = {}; + + if(const pugi::xml_attribute xyz = origin.attribute("xyz")) { + // TODO hacky, doesn't have error handling + transformations[transformationOffset].translation = Utility::ConfigurationValue::fromString(xyz.as_string(), {}); + // is Y up or how?? + } else transformations[transformationOffset].translation = {}; + + ++transformationOffset; + } + } + if(const pugi::xml_node visual = node.child("visual")) { + const UnsignedInt visualId = parentOffset; + parents[parentOffset].mapping = visualId; + parents[parentOffset].parent = id; + ++parentOffset; + + linkVisualMeshObjects[linkVisualOffset] = visualId; + // TODO + ++linkVisualOffset; + + if(const pugi::xml_node origin = visual.child("origin")) { + transformations[transformationOffset].mapping = visualId; + + // TODO duplicates!!! + if(const pugi::xml_attribute rpy = origin.attribute("rpy")) { + // TODO hacky, doesn't have error handling + Vector3 data = Utility::ConfigurationValue::fromString(rpy.as_string(), {}); + // TODO is this alright?! + transformations[transformationOffset].rotation = + Quaternion::rotation(Rad(data.z()), Vector3::yAxis())* + Quaternion::rotation(Rad(data.y()), Vector3::zAxis())* + Quaternion::rotation(Rad(data.x()), Vector3::xAxis()); + } else transformations[transformationOffset].rotation = {}; + + if(const pugi::xml_attribute xyz = origin.attribute("xyz")) { + // TODO hacky, doesn't have error handling + transformations[transformationOffset].translation = Utility::ConfigurationValue::fromString(xyz.as_string(), {}); + // is Y up or how?? + } else transformations[transformationOffset].translation = {}; + + ++transformationOffset; + } + if(visual.child("material")) { + // TODO, without the incremented linkVisualOffset!! + } + } + if(const pugi::xml_node collision = node.child("collision")) { + const UnsignedInt collisionId = parentOffset; + parents[parentOffset].mapping = collisionId; + parents[parentOffset].parent = id; + ++parentOffset; + + linkCollisionMeshObjects[linkCollisionOffset] = collisionId; + // TODO + ++linkCollisionOffset; + + if(const pugi::xml_node origin = collision.child("origin")) { + transformations[transformationOffset].mapping = collisionId; + // TODO duplicated code, 3 times!!! + if(const pugi::xml_attribute rpy = origin.attribute("rpy")) { + // TODO hacky, doesn't have error handling + Vector3 data = Utility::ConfigurationValue::fromString(rpy.as_string(), {}); + // TODO is this alright?! + transformations[transformationOffset].rotation = + Quaternion::rotation(Rad(data.z()), Vector3::yAxis())* + Quaternion::rotation(Rad(data.y()), Vector3::zAxis())* + Quaternion::rotation(Rad(data.x()), Vector3::xAxis()); + } else transformations[transformationOffset].rotation = {}; + + if(const pugi::xml_attribute xyz = origin.attribute("xyz")) { + // TODO hacky, doesn't have error handling + transformations[transformationOffset].translation = Utility::ConfigurationValue::fromString(xyz.as_string(), {}); + // is Y up or how?? + } else transformations[transformationOffset].translation = {}; + + + ++transformationOffset; + } + } + } + } + + // TODO assert offsets match + + Containers::Array fields; + // TODO these are first now because GltfSceneConverter puts them to extra if some custom field is before, put them back before linkCollisionCount + if(linkVisualCount) arrayAppend(fields, { + SceneFieldData{SceneField::Mesh, linkVisualMeshObjects, linkVisualMeshes} + }); + if(hasMaterials) arrayAppend(fields, { + SceneFieldData{SceneField::MeshMaterial, linkVisualMeshObjects, linkVisualMeshMaterials} + }); + arrayAppend(fields, { + /* These are always present */ + SceneFieldData{SceneField::Parent, parents.slice(&Parent::mapping), parents.slice(&Parent::parent)}, + SceneFieldData{SceneField::Translation, transformations.slice(&Transformation::mapping), transformations.slice(&Transformation::translation)}, + SceneFieldData{SceneField::Rotation, transformations.slice(&Transformation::mapping), transformations.slice(&Transformation::rotation)}, + }); + if(jointAxesCount) arrayAppend(fields, { + SceneFieldData{SceneFieldJointAxis, jointAxes.slice(&JointAxis::mapping), jointAxes.slice(&JointAxis::axis)} + }); + if(jointLimitCount) arrayAppend(fields, { + SceneFieldData{SceneFieldJointLimitLower, jointLimits.slice(&JointLimit::mapping), jointLimits.slice(&JointLimit::lower)}, + SceneFieldData{SceneFieldJointLimitUpper, jointLimits.slice(&JointLimit::mapping), jointLimits.slice(&JointLimit::upper)}, + SceneFieldData{SceneFieldJointLimitEffort, jointLimits.slice(&JointLimit::mapping), jointLimits.slice(&JointLimit::effort)}, + SceneFieldData{SceneFieldJointLimitVelocity, jointLimits.slice(&JointLimit::mapping), jointLimits.slice(&JointLimit::velocity)}, + }); + if(jointDynamicsCount) arrayAppend(fields, { + SceneFieldData{SceneFieldJointDynamicsDamping, jointDynamics.slice(&JointDynamics::mapping), jointDynamics.slice(&JointDynamics::damping)}, + SceneFieldData{SceneFieldJointDynamicsFriction, jointDynamics.slice(&JointDynamics::mapping), jointDynamics.slice(&JointDynamics::friction)}, + }); + if(linkInertialCount) arrayAppend(fields, { + SceneFieldData{SceneFieldLinkInertialMass, linkInertials.slice(&LinkInertial::mapping), linkInertials.slice(&LinkInertial::mass)}, + SceneFieldData{SceneFieldLinkInertia, linkInertials.slice(&LinkInertial::mapping), linkInertials.slice(&LinkInertial::inertia)}, + }); + if(linkCollisionCount) arrayAppend(fields, { + SceneFieldData{SceneFieldCollisionMesh, linkCollisionMeshObjects, linkCollisionMeshes} + }); + + /* Convert back to the default deleter to avoid dangling deleter function + pointer issues when unloading the plugin */ + arrayShrink(fields, DefaultInit); + return SceneData{SceneMappingType::UnsignedInt, parentCount, std::move(data), std::move(fields)}; +} + +UnsignedLong UrdfImporter::doObjectCount() const { + return _in->nodeNames.size(); +} + +Long UrdfImporter::doObjectForName(const Containers::StringView name) { + const auto found = _in->nodesForName.find(name); + return found == _in->nodesForName.end() ? -1 : found->second; +} + +Containers::String UrdfImporter::doObjectName(const UnsignedLong id) { + return _in->nodeNames[id]; +} + +}} + +CORRADE_PLUGIN_REGISTER(UrdfImporter, Magnum::Trade::UrdfImporter, + "cz.mosra.magnum.Trade.AbstractImporter/0.5") diff --git a/src/MagnumPlugins/UrdfImporter/UrdfImporter.h b/src/MagnumPlugins/UrdfImporter/UrdfImporter.h new file mode 100644 index 000000000..d167907a6 --- /dev/null +++ b/src/MagnumPlugins/UrdfImporter/UrdfImporter.h @@ -0,0 +1,149 @@ +#ifndef Magnum_Trade_UrdfImporter_h +#define Magnum_Trade_UrdfImporter_h +/* + This file is part of Magnum. + + Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, + 2020, 2021, 2022 Vladimír Vondruš + + Permission is hereby granted, free of charge, to any person obtaining a + copy of this software and associated documentation files (the "Software"), + to deal in the Software without restriction, including without limitation + the rights to use, copy, modify, merge, publish, distribute, sublicense, + and/or sell copies of the Software, and to permit persons to whom the + Software is furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included + in all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + DEALINGS IN THE SOFTWARE. +*/ + +/** @file + * @brief Class @ref Magnum::Trade::UrdfImporter + * @m_since_latest_{plugins} + */ + +#include +#include +#include + +#include "MagnumPlugins/UrdfImporter/configure.h" + +#ifndef DOXYGEN_GENERATING_OUTPUT +#ifndef MAGNUM_URDFIMPORTER_BUILD_STATIC + #ifdef UrdfImporter_EXPORTS + #define MAGNUM_URDFIMPORTER_EXPORT CORRADE_VISIBILITY_EXPORT + #else + #define MAGNUM_URDFIMPORTER_EXPORT CORRADE_VISIBILITY_IMPORT + #endif +#else + #define MAGNUM_URDFIMPORTER_EXPORT CORRADE_VISIBILITY_STATIC +#endif +#define MAGNUM_URDFIMPORTER_LOCAL CORRADE_VISIBILITY_LOCAL +#else +#define MAGNUM_URDFIMPORTER_EXPORT +#define MAGNUM_URDFIMPORTER_LOCAL +#endif + +namespace Magnum { namespace Trade { + +/** +@brief URDF importer plugin +@m_since_latest_{plugins} + +@ref TODOTODO + +@section Trade-UrdfImporter-usage Usage + +This plugin depends on the @ref Trade library and is built if +`WITH_URDFIMPORTER` is enabled when building Magnum Plugins. To use as a +dynamic plugin, load @cpp "UrdfImporter" @ce via +@ref Corrade::PluginManager::Manager. + +Additionally, if you're using Magnum as a CMake subproject, bundle the +[magnum-plugins repository](https://github.com/mosra/magnum-plugins) and do the +following: + +@ref TODOTODO pugixml dependency + +@code{.cmake} +set(WITH_URDFIMPORTER ON CACHE BOOL "" FORCE) +add_subdirectory(magnum-plugins EXCLUDE_FROM_ALL) + +# So the dynamically loaded plugin gets built implicitly +add_dependencies(your-app MagnumPlugins::UrdfImporter) +@endcode + +To use as a static plugin or as a dependency of another plugin with CMake, put +[FindMagnumPlugins.cmake](https://github.com/mosra/magnum-plugins/blob/master/modules/FindMagnumPlugins.cmake) +into your `modules/` directory, request the `UrdfImporter` component of the +`MagnumPlugins` package and link to the `MagnumPlugins::UrdfImporter` +target: + +@code{.cmake} +find_package(MagnumPlugins REQUIRED UrdfImporter) + +# ... +target_link_libraries(your-app PRIVATE MagnumPlugins::UrdfImporter) +@endcode + +See @ref building-plugins, @ref cmake-plugins, @ref plugins and +@ref file-formats for more information. + +@section Trade-UrdfImporter-behavior Behavior and limitations + +@ref TODOTODO + +@section Trade-UrdfImporter-configuration Plugin-specific config + +It's possible to tune various import options through @ref configuration(). See +below for all options and their default values: + +@snippet MagnumPlugins/UrdfImporter/UrdfImporter.conf config + +See @ref plugins-configuration for more information and an example showing how +to edit the configuration values. +*/ +class MAGNUM_URDFIMPORTER_EXPORT UrdfImporter: public AbstractImporter { + public: + /** @brief Default constructor */ + explicit UrdfImporter(); + + /** @brief Plugin manager constructor */ + explicit UrdfImporter(PluginManager::AbstractManager& manager, const Containers::StringView& plugin); + + ~UrdfImporter(); + + private: + MAGNUM_URDFIMPORTER_LOCAL ImporterFeatures doFeatures() const override; + + MAGNUM_URDFIMPORTER_LOCAL bool doIsOpened() const override; + MAGNUM_URDFIMPORTER_LOCAL void doOpenData(Containers::Array&& data, DataFlags dataFlags) override; + MAGNUM_URDFIMPORTER_LOCAL void doClose() override; + + MAGNUM_URDFIMPORTER_LOCAL Int doDefaultScene() const override; + MAGNUM_URDFIMPORTER_LOCAL UnsignedInt doSceneCount() const override; + MAGNUM_URDFIMPORTER_LOCAL Containers::String doSceneName(UnsignedInt id) override; + MAGNUM_URDFIMPORTER_LOCAL Int doSceneForName(Containers::StringView name) override; + MAGNUM_URDFIMPORTER_LOCAL Containers::String doSceneFieldName(UnsignedInt name) override; + MAGNUM_URDFIMPORTER_LOCAL SceneField doSceneFieldForName(Containers::StringView name) override; + MAGNUM_URDFIMPORTER_LOCAL Containers::Optional doScene(UnsignedInt id) override; + + MAGNUM_URDFIMPORTER_LOCAL UnsignedLong doObjectCount() const override; + MAGNUM_URDFIMPORTER_LOCAL Long doObjectForName(Containers::StringView name) override; + MAGNUM_URDFIMPORTER_LOCAL Containers::String doObjectName(UnsignedLong id) override; + + struct State; + Containers::Pointer _in; +}; + +}} + +#endif diff --git a/src/MagnumPlugins/UrdfImporter/configure.h.cmake b/src/MagnumPlugins/UrdfImporter/configure.h.cmake new file mode 100644 index 000000000..5da93b923 --- /dev/null +++ b/src/MagnumPlugins/UrdfImporter/configure.h.cmake @@ -0,0 +1,26 @@ +/* + This file is part of Magnum. + + Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, + 2020, 2021, 2022 Vladimír Vondruš + + Permission is hereby granted, free of charge, to any person obtaining a + copy of this software and associated documentation files (the "Software"), + to deal in the Software without restriction, including without limitation + the rights to use, copy, modify, merge, publish, distribute, sublicense, + and/or sell copies of the Software, and to permit persons to whom the + Software is furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included + in all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + DEALINGS IN THE SOFTWARE. +*/ + +#cmakedefine MAGNUM_URDFIMPORTER_BUILD_STATIC diff --git a/src/MagnumPlugins/UrdfImporter/importStaticPlugin.cpp b/src/MagnumPlugins/UrdfImporter/importStaticPlugin.cpp new file mode 100644 index 000000000..741d691ce --- /dev/null +++ b/src/MagnumPlugins/UrdfImporter/importStaticPlugin.cpp @@ -0,0 +1,35 @@ +/* + This file is part of Magnum. + + Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, + 2020, 2021, 2022 Vladimír Vondruš + + Permission is hereby granted, free of charge, to any person obtaining a + copy of this software and associated documentation files (the "Software"), + to deal in the Software without restriction, including without limitation + the rights to use, copy, modify, merge, publish, distribute, sublicense, + and/or sell copies of the Software, and to permit persons to whom the + Software is furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included + in all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + DEALINGS IN THE SOFTWARE. +*/ + +#include "MagnumPlugins/UrdfImporter/configure.h" + +#ifdef MAGNUM_URDFIMPORTER_BUILD_STATIC +#include + +static int magnumUrdfImporterStaticImporter() { + CORRADE_PLUGIN_IMPORT(UrdfImporter) + return 1; +} CORRADE_AUTOMATIC_INITIALIZER(magnumUrdfImporterStaticImporter) +#endif