diff --git a/source/ArmarXSimulation/components/Simulator/CMakeLists.txt b/source/ArmarXSimulation/components/Simulator/CMakeLists.txt index a18135f27631723d25c49d863a917605f37e5863..cc924feed95bc2130e3cfa49a44aa4a0c3077ae5 100644 --- a/source/ArmarXSimulation/components/Simulator/CMakeLists.txt +++ b/source/ArmarXSimulation/components/Simulator/CMakeLists.txt @@ -10,7 +10,7 @@ if (MujocoX_FOUND) ) endif() -find_package(SDFormat REQUIRED) +find_package(sdformat13 REQUIRED) set(COMPONENT_LIBS ArmarXSimulationInterfaces diff --git a/source/ArmarXSimulation/components/Simulator/Simulator.cpp b/source/ArmarXSimulation/components/Simulator/Simulator.cpp index 4be9d192c9b25d101a42f7666a72a9b757fbf0cf..7964f005ad1f162f15900be719aeaf7a4b6b9202 100644 --- a/source/ArmarXSimulation/components/Simulator/Simulator.cpp +++ b/source/ArmarXSimulation/components/Simulator/Simulator.cpp @@ -454,7 +454,7 @@ void Simulator::addSceneFromSdfFile(const std::string& scenePath) ARMARX_WARNING << "Could not find CMake package " << scenePackage << "."; return; } - + std::filesystem::path dataDir = cmakeFinder.getDataDir(); std::filesystem::path path = dataDir / scenePath; @@ -478,37 +478,43 @@ void Simulator::addSceneFromSdfFile(const std::string& scenePath) return ""; }); - sdf::Errors errors; sdf::SDFPtr sdfFile(new sdf::SDF()); sdf::init(sdfFile); - bool ok = sdf::readFile(path.string(), sdfFile, errors); + sdf::Root root; + sdf::Errors errors = root.Load(path.string()); for (auto const& error : errors) { ARMARX_ERROR << "Error on reading SDF: " << error; } - if (!ok) + if (!errors.empty()) { ARMARX_WARNING << "Could not load SDF from '" << path << "'"; sdf::setFindCallback([](const std::string&) -> std::string { return ""; }); return; } - const sdf::ElementPtr root = sdfFile->Root(); - if (!root->HasElement("world")) + if (root.WorldCount() == 0) { ARMARX_WARNING << "SDF does not contain a world"; sdf::setFindCallback([](const std::string&) -> std::string { return ""; }); return; } - const sdf::ElementPtr world = root->GetElement("world"); + if (root.WorldCount() > 1) + { + ARMARX_WARNING << "SDF contains multiple worlds"; + sdf::setFindCallback([](const std::string&) -> std::string { return ""; }); + return; + } + + sdf::World* world = root.WorldByIndex(0); - auto addSceneObject = [&](const sdf::ElementPtr& model) + auto addSceneObject = [&](sdf::Model* model) { - auto name = model->Get<std::string>("name"); + auto name = model->Name(); if (physicsWorld->hasObject(name)) { @@ -527,22 +533,22 @@ void Simulator::addSceneFromSdfFile(const std::string& scenePath) std::filesystem::path modelPath; std::tie(std::ignore, modelPath) = *it; - auto pose = model->Get<ignition::math::Pose3f>("pose"); + gz::math::Pose3d pose = model->RawPose(); const float meterToMillimeter = 1000.0F; Eigen::Vector3f position; - position.x() = pose.Pos().X() * meterToMillimeter; - position.y() = pose.Pos().Y() * meterToMillimeter; - position.z() = pose.Pos().Z() * meterToMillimeter; + position.x() = static_cast<float>(pose.Pos().X() * meterToMillimeter); + position.y() = static_cast<float>(pose.Pos().Y() * meterToMillimeter); + position.z() = static_cast<float>(pose.Pos().Z() * meterToMillimeter); Eigen::Quaternionf orientation; - orientation.x() = pose.Rot().X(); - orientation.y() = pose.Rot().Y(); - orientation.z() = pose.Rot().Z(); - orientation.w() = pose.Rot().W(); + orientation.x() = static_cast<float>(pose.Rot().X()); + orientation.y() = static_cast<float>(pose.Rot().Y()); + orientation.z() = static_cast<float>(pose.Rot().Z()); + orientation.w() = static_cast<float>(pose.Rot().W()); - auto isStatic = model->Get<bool>("static"); + auto isStatic = model->Static(); std::string robotName = modelPath.filename().generic_string() + ".urdf"; std::filesystem::path robotPath = modelPath / robotName; @@ -590,12 +596,9 @@ void Simulator::addSceneFromSdfFile(const std::string& scenePath) } }; - for ( - sdf::ElementPtr model = world->GetElement("model"); - model; - model = model->GetNextElement("model")) + for (size_t index = 0; index < world->ModelCount(); index++) { - addSceneObject(model); + addSceneObject(world->ModelByIndex(index)); } sdf::setFindCallback([](const std::string&) -> std::string { return ""; });