diff --git a/source/ArmarXSimulation/components/Simulator/Simulator.cpp b/source/ArmarXSimulation/components/Simulator/Simulator.cpp index 0385d150f534865ebb5be04b37bc941083cb8e1a..f12eaff9ba71805001d0914092710ef1456b5711 100644 --- a/source/ArmarXSimulation/components/Simulator/Simulator.cpp +++ b/source/ArmarXSimulation/components/Simulator/Simulator.cpp @@ -470,7 +470,7 @@ void Simulator::addSceneFromSdfFile(const std::string& scenePath) std::filesystem::path global = path.parent_path(); std::filesystem::path full = global / local; - knownPaths[local.stem().generic_string()] = full; + knownPaths[local.filename().generic_string()] = full; return full.string(); } @@ -521,31 +521,48 @@ void Simulator::addSceneFromSdfFile(const std::string& scenePath) if (it == knownPaths.end()) { ARMARX_INFO << "No known path for object \"" << name << "\" in SDF."; + return; } std::filesystem::path modelPath; std::tie(std::ignore, modelPath) = *it; + std::string xmlName = modelPath.filename().generic_string() + ".xml"; + std::filesystem::path xmlPath = modelPath / xmlName; + VirtualRobot::ManipulationObjectPtr simoxObject = - VirtualRobot::ObjectIO::loadManipulationObject(modelPath.string()); - //simoxObject->setGlobalPose(simox::math::pose(object.position, object.orientation)); + VirtualRobot::ObjectIO::loadManipulationObject(xmlPath.string()); + auto pose = model->Get<ignition::math::Pose3f>("pose"); - VirtualRobot::SceneObject::Physics::SimulationType simType = VirtualRobot::SceneObject::Physics::eKinematic; /* - if (object.isStatic.has_value()) - { - simType = *object.isStatic - ? VirtualRobot::SceneObject::Physics::eKinematic - : VirtualRobot::SceneObject::Physics::eDynamic; - } - */ + 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; + + Eigen::Quaternionf orientation; + orientation.x() = pose.Rot().X(); + orientation.y() = pose.Rot().Y(); + orientation.z() = pose.Rot().Z(); + orientation.w() = pose.Rot().W(); + + simoxObject->setGlobalPose(simox::math::pose(position, orientation)); + + auto isStatic = model->Get<bool>("static"); + VirtualRobot::SceneObject::Physics::SimulationType simType = + isStatic + ? VirtualRobot::SceneObject::Physics::eKinematic + : VirtualRobot::SceneObject::Physics::eDynamic; simoxObject->setSimulationType(simType); - simoxObject->setFilename(modelPath.string()); + simoxObject->setFilename(xmlPath.string()); simoxObject->setName(name); - physicsWorld->addObstacle(simoxObject, simType, modelPath.stem().generic_string(), name, {}, - scenePackage); + physicsWorld->addObstacle(simoxObject, simType, + xmlPath.string(), name, + {},scenePackage); }; for ( @@ -631,6 +648,7 @@ void Simulator::addJsonSceneObject( { simType = VirtualRobot::SceneObject::Physics::eKinematic; } + PackageFileLocation simoxXml = objectInfo->simoxXML(); simoxObject->setSimulationType(simType); simoxObject->setFilename(simoxXml.absolutePath); @@ -677,7 +695,7 @@ void Simulator::initializeData() if (!scenePaths.empty()) { bool trim = true; - for (std::string scenePath : simox::alg::split(scenePaths, ";", trim)) + for (const std::string& scenePath : simox::alg::split(scenePaths, ";", trim)) { std::string sdfSuffix = ".sdf";