source/ArmarXSimulation/components/HandUnitDynamicSimulation/HandUnitDynamicSimulation.cpp

@@ -368,9 +368,9 @@ void armarx::HandUnitDynamicSimulation::setJointAngles(const NameValueMap& joint
 
 armarx::NameValueMap HandUnitDynamicSimulation::handUnitJointsToRobotJoints(const NameValueMap& joints)
 {
-    ARMARX_INFO << getRobotNameFromHandUnitName();
-    ARMARX_INFO << getHandSideFromHandUnitName();
-    ARMARX_INFO << joints;
+    ARMARX_VERBOSE << getRobotNameFromHandUnitName();
+    ARMARX_VERBOSE << getHandSideFromHandUnitName();
+    ARMARX_VERBOSE << joints;
     std::map<std::string, std::vector<std::string>> conversion_dict;
     conversion_dict["Fingers"] = {"Index", "Middle", "Ring", "Pinky"};
     conversion_dict["Thumb"] = {"Thumb"};

source/ArmarXSimulation/components/ImageProviderDynamicSimulation/ImageProviderDynamicSimulation.cpp

@@ -26,6 +26,8 @@
 
 #include <Ice/Properties.h>
 
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 
 // VisionXCore
@@ -39,9 +41,11 @@
 
 #include "ImageProviderDynamicSimulation.h"
 
+#include <Calibration/Calibration.h>
 #include <Inventor/SoInteraction.h>
 #include <Inventor/nodes/SoDirectionalLight.h>
 #include <Inventor/nodes/SoUnits.h>
+#include <Math/Math3d.h>
 
 
 namespace armarx
@@ -51,38 +55,17 @@ namespace armarx
         // init SoDB / Coin3D
         //SoDB::init();
 
+        ARMARX_INFO << "1";
+
         VirtualRobot::init(this->getName());
 
         // needed for SoSelection
         SoInteraction::init();
 
         // image format
-        setImageFormat(visionx::ImageDimension(getProperty<visionx::ImageDimension>("ImageSize").getValue()), visionx::eRgb);
         setImageSyncMode(visionx::eFpsSynchronization);
         frameRate = getProperty<float>("FrameRate").getValue();
 
-        const visionx::ImageDimension dimensions = getProperty<visionx::ImageDimension>("ImageSize").getValue();
-
-        renderImg_bpp = 3;
-        renderImg_width =  dimensions.width;
-        renderImg_height = dimensions.height;
-
-        setNumberImages(2);
-        images = new CByteImage*[getNumberImages()];
-
-        for (int i = 0; i < getNumberImages(); i++)
-        {
-            images[i] = new CByteImage(dimensions.width, dimensions.height, CByteImage::eRGB24);
-        }
-
-        resizedImages = new CByteImage*[getNumberImages()];
-
-        for (int i = 0 ; i < getNumberImages() ; i++)
-        {
-            resizedImages[i] = visionx::tools::createByteImage(getImageFormat(), getImageFormat().type);
-        }
-
-
         std::string calibrationFileName = getProperty<std::string>("CalibrationFile").getValue();
         ARMARX_VERBOSE << "Camera calibration file: " << calibrationFileName;
 
@@ -96,24 +79,58 @@ namespace armarx
             }
 
             setlocale(LC_NUMERIC, "C");
+
+            CStereoCalibration ivtStereoCalibration;
+
             if (!ivtStereoCalibration.LoadCameraParameters(fullCalibrationFileName.c_str(), true))
             {
                 ARMARX_ERROR << "Error loading camera calibration file: " << fullCalibrationFileName;
             }
+            stereoCalibration = visionx::tools::convert(ivtStereoCalibration);
+
+            renderImg_width = stereoCalibration->calibrationLeft.cameraParam.width;
+            renderImg_height = stereoCalibration->calibrationLeft.cameraParam.height;
+
+            ARMARX_CHECK_EQUAL(stereoCalibration->calibrationLeft.cameraParam.height, renderImg_height);
+            ARMARX_CHECK_EQUAL(stereoCalibration->calibrationRight.cameraParam.height, renderImg_height);
+            ARMARX_CHECK_EQUAL(stereoCalibration->calibrationLeft.cameraParam.width, renderImg_width);
+            ARMARX_CHECK_EQUAL(stereoCalibration->calibrationRight.cameraParam.width, renderImg_width);
+        }
+        else{
+            const visionx::ImageDimension dimensions = getProperty<visionx::ImageDimension>("ImageSize").getValue();
+
+            renderImg_width =  dimensions.width;
+            renderImg_height = dimensions.height;
+        }
+
+        setImageFormat(visionx::ImageDimension{renderImg_width, renderImg_height}, visionx::eRgb);
+
+        renderImg_bpp = 3;
+       
+
+        setNumberImages(2);
+        images = new CByteImage*[getNumberImages()];
+
+        for (int i = 0; i < getNumberImages(); i++)
+        {
+            images[i] = new CByteImage(renderImg_width, renderImg_height, CByteImage::eRGB24);
+        }
+
+        resizedImages = new CByteImage*[getNumberImages()];
+
+        for (int i = 0 ; i < getNumberImages() ; i++)
+        {
+            resizedImages[i] = visionx::tools::createByteImage(getImageFormat(), getImageFormat().type);
         }
 
-        stereoCalibration = visionx::tools::convert(ivtStereoCalibration);
 
 
-        fovLeft = 2.0 * std::atan(renderImg_height / (2.0 * stereoCalibration.calibrationLeft.cameraParam.focalLength[1]));
-        fovRight = 2.0 * std::atan(renderImg_height / (2.0 * stereoCalibration.calibrationRight.cameraParam.focalLength[1]));
 
-        ARMARX_LOG << "fov left: " << (fovLeft / M_PI * 180.0) << " fov right: " << (fovRight / M_PI * 180.0);
+       
         std::stringstream svName;
         svName << getName() << "_PhysicsWorldVisualization";
         simVisu = Component::create<ArmarXPhysicsWorldVisualization>(getIceProperties(), svName.str());
         getArmarXManager()->addObject(simVisu);
-
     }
 
 
@@ -156,6 +173,7 @@ namespace armarx
         robotName = getProperty<std::string>("RobotName").getValue();
         leftNodeName = getProperty<std::string>("RobotNodeLeftCamera").getValue();
         rightNodeName = getProperty<std::string>("RobotNodeRightCamera").getValue();
+        focalLength = getProperty<float>("focalLength").getValue();
 
         // ensure that all data is loaded
         if (simVisu)
@@ -242,12 +260,20 @@ namespace armarx
         }
 
 
+        if(not stereoCalibration.has_value())
+        {
+            initStereoCalib();
+        }
+
+        float fovLeft = 2.0 * std::atan(renderImg_height / (2.0 * stereoCalibration->calibrationLeft.cameraParam.focalLength[1]));
+        float fovRight = 2.0 * std::atan(renderImg_height / (2.0 * stereoCalibration->calibrationRight.cameraParam.focalLength[1]));
+        // ARMARX_LOG << "fov left: " << (fovLeft / M_PI * 180.0) << " fov right: " << (fovRight / M_PI * 180.0);
 
 
 
         // render Left Camera
 #if 1
-        renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreen(rendererLeft.get(), cameraNodeL, simVisu->getVisualization(), &renderBuffer, 10.0f, 100000.0f, fovLeft);
+        renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreen(rendererLeft.get(), cameraNodeL, simVisu->getVisualization(), &renderBuffer, 100.0f, 100000.0f, fovLeft);
 #else
         //////////////// optional: we render by our own
         SoPerspectiveCamera* cam = new SoPerspectiveCamera();
@@ -298,7 +324,7 @@ namespace armarx
 
 
         // render Right Camera
-        renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreen(rendererRight.get(), cameraNodeR, simVisu->getVisualization(), &renderBuffer, 10.0f, 100000.0f, fovRight);
+        renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreen(rendererRight.get(), cameraNodeR, simVisu->getVisualization(), &renderBuffer, 100.0f, 100000.0f, fovRight);
 
         if (renderOK && renderBuffer != NULL)
         {
@@ -363,10 +389,51 @@ namespace armarx
     }
 
 
+    void ImageProviderDynamicSimulation::initStereoCalib()
+    {
+        
+
+        CCalibration leftCalibration;
+        leftCalibration.SetCameraParameters(focalLength,focalLength, renderImg_width / 2, renderImg_height / 2, 0,0,0,0, Math3d::unit_mat, Math3d::zero_vec, renderImg_width, renderImg_height);
+        
+        ARMARX_CHECK_NOT_NULL(simVisu);
+        while (!simVisu->getRobot(robotName))
+        {
+            usleep(100000);
+            simVisu->synchronizeVisualizationData();
+            ARMARX_INFO << deactivateSpam(3) << "Waiting for visu";
+        }
+
+        ARMARX_CHECK(simVisu->synchronizeVisualizationData());
+        auto robot = simVisu->getRobot(robotName);
+        ARMARX_CHECK_NOT_NULL(robot);
+
+        cameraNodeL = robot->getRobotNode(leftNodeName);
+        cameraNodeR = robot->getRobotNode(rightNodeName);
+
+        ARMARX_CHECK_NOT_NULL(cameraNodeL);
+        ARMARX_CHECK_NOT_NULL(cameraNodeR);
+
+        const Eigen::Vector3f left_P_right = cameraNodeR->getPositionInFrame(cameraNodeL);
+        ARMARX_IMPORTANT << VAROUT(left_P_right);
+
+        CCalibration rightCalibration;
+        Vec3d transRight{-left_P_right.x(), 0, 0};
+        rightCalibration.SetCameraParameters(focalLength,focalLength, renderImg_width / 2, renderImg_height / 2, 0,0,0,0, Math3d::unit_mat, transRight, renderImg_width, renderImg_height);
+
+        CStereoCalibration ivtStereoCalibration;
+        ivtStereoCalibration.SetSingleCalibrations(leftCalibration, rightCalibration);
+        stereoCalibration = visionx::tools::convert(ivtStereoCalibration);
+    }
 
     visionx::StereoCalibration ImageProviderDynamicSimulation::getStereoCalibration(const Ice::Current& c)
     {
-        return stereoCalibration;
+        if(not stereoCalibration.has_value())
+        {
+            initStereoCalib();
+        }
+
+        return stereoCalibration.value();
     }
 
     bool ImageProviderDynamicSimulation::getImagesAreUndistorted(const Ice::Current& c)
@@ -380,5 +447,3 @@ namespace armarx
     }
 
 }
-
-

source/ArmarXSimulation/components/ImageProviderDynamicSimulation/ImageProviderDynamicSimulation.h

@@ -142,9 +142,13 @@ namespace armarx
          */
         armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
         {
-            return armarx::PropertyDefinitionsPtr(
+            auto props = armarx::PropertyDefinitionsPtr(
                        new ImageSequenceProviderDynamicSimulationPropertyDefinitions(
                            getConfigIdentifier()));
+            
+            props->defineOptionalProperty<float>("focalLength", 600, "The focal length.");
+
+            return props;
         }
         CByteImage**             images;
         CByteImage**             resizedImages;
@@ -157,7 +161,6 @@ namespace armarx
         int renderImg_bpp;
         int renderImg_width;
         int renderImg_height;
-        float fovLeft, fovRight;
 
         //RemoteRobotPtr remoteRobot;
 
@@ -178,15 +181,16 @@ namespace armarx
         VirtualRobot::RobotNodePtr cameraNodeL;
         VirtualRobot::RobotNodePtr cameraNodeR;
 
-        CStereoCalibration ivtStereoCalibration;
-        visionx::StereoCalibration stereoCalibration;
+        float focalLength;
+
+        std::optional<visionx::StereoCalibration> stereoCalibration;
 
         bool setupCameraRendering(const std::string& robotName, const std::string& cameraSensorNameLeft, const std::string& cameraSensorNameRight);
         bool updateCameraRendering();
+
+        void initStereoCalib();
     private:
 
     };
 
 }
-
-

source/ArmarXSimulation/components/KinematicSelfLocalization/KinematicSelfLocalization.cpp

@@ -21,6 +21,10 @@
  */
 
 #include "KinematicSelfLocalization.h"
+#include <Eigen/Geometry>
+#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
+#include "RobotAPI/libraries/core/FramedPose.h"
+#include <RobotAPI/interface/core/GeometryBase.h>
 
 #include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
 #include <MemoryX/core/MemoryXCoreObjectFactories.h>
@@ -67,12 +71,18 @@ void KinematicSelfLocalization::onConnectComponent()
     ARMARX_INFO << "Creating robot agent...done";
 
     // Simulating platform movement to initial pose
-    PlatformPose pose;
-    pose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
-    pose.x = getProperty<float>("InitialPlatformPoseX").getValue();
-    pose.y = getProperty<float>("InitialPlatformPoseY").getValue();
-    pose.rotationAroundZ = getProperty<float>("InitialPlatformPoseAngle").getValue();
-    reportPlatformPose(pose);
+    Eigen::Isometry3f global_T_robot_initial = Eigen::Isometry3f::Identity();
+    global_T_robot_initial.translation().x() = getProperty<float>("InitialPlatformPoseX").getValue();
+    global_T_robot_initial.translation().y() = getProperty<float>("InitialPlatformPoseY").getValue();
+    global_T_robot_initial.linear() = simox::math::rpy_to_mat3f(0, 0, getProperty<float>("InitialPlatformPoseAngle").getValue());
+
+    TransformStamped transformStamped;
+    transformStamped.transform = global_T_robot_initial.matrix();
+    transformStamped.header.agent = agentName;
+    transformStamped.header.frame = GlobalFrame;
+    transformStamped.header.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+
+    reportGlobalRobotPose(transformStamped);
 
     // periodic task setup
     cycleTime = 30;
@@ -129,29 +139,12 @@ void KinematicSelfLocalization::reportRobotPose()
     }
 }
 
-void armarx::KinematicSelfLocalization::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current&)
+void KinematicSelfLocalization::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped, const ::Ice::Current&) 
 {
     std::unique_lock lock(dataMutex);
-    Eigen::Matrix3f ori;
-    ori.setIdentity();
-    ori = Eigen::AngleAxisf(currentPose.rotationAroundZ, Eigen::Vector3f::UnitZ());
-    Eigen::Vector3f pos;
-    pos[0] = currentPose.x;
-    pos[1] = currentPose.y;
-    pos[2] = robotPoseZ;
-
-    currentRobotPose = new FramedPose(ori, pos, armarx::GlobalFrame, "");
-}
-
-void armarx::KinematicSelfLocalization::reportNewTargetPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
-{
-}
 
-void armarx::KinematicSelfLocalization::reportPlatformVelocity(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
-{
-}
+    Eigen::Isometry3f global_T_robot(transformStamped.transform);
+    global_T_robot.translation().z() = robotPoseZ; // TODO lecagy. Is this really necessary?
 
-
-void armarx::KinematicSelfLocalization::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
-{
+    currentRobotPose = new FramedPose(global_T_robot.matrix(), armarx::GlobalFrame, "");
 }

source/ArmarXSimulation/components/KinematicSelfLocalization/KinematicSelfLocalization.h

@@ -24,6 +24,7 @@
 
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <RobotAPI/interface/core/RobotLocalization.h>
 #include <RobotAPI/interface/core/RobotState.h>
 
 // MemoryX
@@ -76,7 +77,7 @@ namespace armarx
      */
     class KinematicSelfLocalization :
         virtual public armarx::Component,
-        public PlatformUnitListener
+        public GlobalRobotPoseLocalizationListener
     {
     public:
         /**
@@ -134,11 +135,6 @@ namespace armarx
         float robotPoseZ;
 
     public:
-        // PlatformUnitListener interface
-        void reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& = Ice::emptyCurrent) override;
-        void reportNewTargetPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current& = Ice::emptyCurrent) override;
-        void reportPlatformVelocity(Ice::Float, Ice::Float, Ice::Float, const Ice::Current& = Ice::emptyCurrent) override;
-        void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&) override;
+        void reportGlobalRobotPose(const ::armarx::TransformStamped&, const ::Ice::Current& = ::Ice::emptyCurrent) override;
     };
 }
-

source/ArmarXSimulation/components/LaserScannerSimulation/ArVizDrawer.cpp

@@ -15,11 +15,23 @@
 #include <RobotAPI/components/ArViz/Client/Client.h>
 #include <RobotAPI/components/ArViz/Client/Elements.h>
 
-#include <RobotComponents/libraries/cartographer/util/laser_scanner_conversion.h>
-
 namespace armarx::laser_scanner_simulation
 {
 
+    template <typename EigenVectorT>
+    EigenVectorT toCartesian(const LaserScanStep& laserScanStep)
+    {
+        EigenVectorT point = EigenVectorT::Zero();
+
+        point.x() = laserScanStep.distance * std::cos(laserScanStep.angle);
+        point.y() = laserScanStep.distance * std::sin(laserScanStep.angle);
+
+        return point;
+    }
+
+
+
+
     void ArVizDrawer::drawBoundingBoxes(const std::vector<VirtualRobot::BoundingBox>& boundingBoxes)
     {
 

source/ArmarXSimulation/components/LaserScannerSimulation/LaserScannerSimulation.cpp

@@ -213,7 +213,7 @@ namespace armarx::laser_scanner_simulation
         // collision checking by sliding an object with the grid step size over the grid map
         float boxSize = 1.1f * gridCellSize;
         VirtualRobot::VisualizationNodePtr boxVisu =
-            factory->createBox(boxSize, boxSize, boxSize, 0.0f, 1.0f, 0.0f);
+            factory->createBox(boxSize, boxSize, boxSize, VirtualRobot::VisualizationFactory::Color::Green());
         VirtualRobot::CollisionModelPtr boxCollisionModel(
             new VirtualRobot::CollisionModel(boxVisu, "", collisionChecker));
         VirtualRobot::SceneObjectPtr box(

source/ArmarXSimulation/components/PlatformUnitDynamicSimulation/PlatformUnitDynamicSimulation.cpp

@@ -25,6 +25,8 @@
 
 #include "PlatformUnitDynamicSimulation.h"
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "RobotAPI/libraries/core/FramedPose.h"
+#include "RobotAPI/libraries/core/Pose.h"
 #include <Eigen/Geometry>
 
 #include <cmath>
@@ -242,7 +244,7 @@ void PlatformUnitDynamicSimulation::moveTo(Ice::Float targetPlatformPositionX, I
     targetRotation = targetPlatformRotation;
     this->positionalAccuracy = positionalAccuracy * .001f;
     this->orientationalAccuracy = orientationalAccuracy;
-    listenerPrx->begin_reportNewTargetPose(targetPosition.x, targetPosition.y, targetRotation);
+    // listenerPrx->begin_reportNewTargetPose(targetPosition.x, targetPosition.y, targetRotation);
     ARMARX_INFO << "New Target: " << targetPosition.x << " " << targetPosition.y << " " << targetRotation << flush;
 }
 
@@ -334,12 +336,20 @@ void PlatformUnitDynamicSimulation::reportState(SimulatedRobotState const& state
 
     // Pose
     updateCurrentPose(state.pose);
-    PlatformPose currentPose;
-    currentPose.timestampInMicroSeconds = state.timestampInMicroSeconds;
-    currentPose.x = currentPosition.x;
-    currentPose.y = currentPosition.y;
-    currentPose.rotationAroundZ = currentRotation;
-    listenerPrx->begin_reportPlatformPose(currentPose);
+
+    ::armarx::TransformStamped transformStamped;
+    
+    FrameHeader header;
+    header.parentFrame = GlobalFrame;
+    header.frame = "root";
+    header.agent = robotName;
+    header.timestampInMicroSeconds = state.timestampInMicroSeconds;
+
+    TransformStamped globalRobotPose;
+    globalRobotPose.header = header;
+    globalRobotPose.transform = armarx::fromIce(state.pose);
+
+    globalPosePrx->reportGlobalRobotPose(globalRobotPose);
 
     // Velocity
     updateCurrentVelocity(state.linearVelocity, state.angularVelocity);

source/ArmarXSimulation/components/RobotUnitSimulation/RobotUnitSimulation.cpp

@@ -151,7 +151,7 @@ void RobotUnitSimulation::rtTask()
                     ARMARX_INFO << "adding devices for joints:";
                     for (const VirtualRobot::RobotNodePtr& node : * (robot->getRobotNodeSet(getProperty<std::string>("RobotNodeSetName").getValue())))
                     {
-                        if (node->isRotationalJoint() || node ->isTranslationalJoint())
+                        if (node->isJoint())
                         {
                             JointSimulationDevicePtr jdev = std::make_shared<JointSimulationDevice>(node->getName(), anglesCtrl, velocitiesCtrl, torquesCtrl);
                             jointDevs.add(jdev->getDeviceName(), jdev);

source/ArmarXSimulation/components/SimulationObjectPoseProvider/CMakeLists.txt

 armarx_component_set_name(SimulationObjectPoseProvider)
 
+find_package(range-v3 QUIET)
+armarx_build_if(range-v3_FOUND "range-v3 not available")
+
 armarx_add_component(
     SOURCES
         SimulationObjectPoseProvider.cpp

source/ArmarXSimulation/components/SimulationObjectPoseProvider/SimulationObjectPoseProvider.cpp

@@ -61,13 +61,13 @@ namespace armarx
                       "Frequency at which to update the objects in the ObjectMemory.");
 
         def->optional(properties.requestAllEntities,
-                      "p.requestAllEntites",
+                      "p.requestAllEntities",
                       "True if all entities should be requested all the time.");
 
         def->optional(properties.initiallyRequestedEntities,
                       "p.initiallyRequestedEntities",
-                      "All entities (comma separated) that should be requested from the beginning."
-                      " If you want an entity to be localized for n seconds append ':n'.");
+                      "All entities (comma separated) that should be requested from the beginning (e.g. 'Kitchen/multivitamin-juice/0'). "
+                      "If you want an entity to be localized for n seconds append ':n'. If you want to see all active entities, set the component to DEBUG.");
 
         return def;
     }
@@ -207,6 +207,10 @@ namespace armarx
             try
             {
                 sceneData = simulatorPrx->getScene();
+                for (const auto& object : sceneData.objects)
+                {
+                    ARMARX_DEBUG << "Found object in scene: " << object.name;
+                }
             }
             catch (const Ice::LocalException& e)
             {
@@ -214,6 +218,9 @@ namespace armarx
                 continue;
             }
 
+            ARMARX_DEBUG << deactivateSpam(10) << "Received " << sceneData.objects.size()
+                         << " objects and " << sceneData.robots.size() << " robots.";
+
             std::vector<objpose::ProvidedObjectPose> providedObjects;
 
             if (properties.requestAllEntities)
@@ -232,8 +239,8 @@ namespace armarx
             // Report the poses
             try
             {
-                ARMARX_IMPORTANT << deactivateSpam(5) << "reporting " << providedObjects.size()
-                                 << " object poses";
+                ARMARX_DEBUG << deactivateSpam(10) << "Reporting " << providedObjects.size()
+                             << " object poses";
                 objectPoseTopic->reportObjectPoses(getName(), objpose::toIce(providedObjects));
             }
             catch (const Ice::LocalException& e)
@@ -289,14 +296,12 @@ namespace armarx
         auto isRequestedObjectKnown = [this](const auto& request)
         {
             const auto& objectName = request.first;
-            ARMARX_INFO << "isRequestedObjectKnown (" << objectName << ")";
             return this->knownObjects.find(objectName) != this->knownObjects.end();
         };
 
         auto getPoseFromRequest = [this, time](const auto& request)
         {
             const auto& object = this->knownObjects.at(request.first);
-            ARMARX_INFO << "getPoseFromRequest (" << request.first << ")";
             return objectPoseFromVisuData(object, time);
         };
 

source/ArmarXSimulation/components/Simulator/BulletPhysicsWorld.cpp

@@ -28,6 +28,7 @@
 #include <VirtualRobot/Robot.h>
 #include <VirtualRobot/Nodes/ForceTorqueSensor.h>
 #include <VirtualRobot/CollisionDetection/CollisionChecker.h>
+#include <VirtualRobot/RobotFactory.h>
 
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
@@ -545,7 +546,7 @@ std::map< std::string, float> BulletPhysicsWorld::getRobotJointAngles(const std:
 
     for (auto& i : rn)
     {
-        if (i->isTranslationalJoint() || i->isRotationalJoint())
+        if (i->isJoint())
         {
             res[i->getName()] = i->getJointValue();
         }
@@ -610,7 +611,7 @@ std::map< std::string, float> BulletPhysicsWorld::getRobotJointVelocities(const
 
     for (auto& i : rn)
     {
-        if (i->isTranslationalJoint() || i->isRotationalJoint())
+        if (i->isJoint())
         {
             float vel = dynamicsRobot->getJointSpeed(i);
             res[i->getName()] = vel;
@@ -681,7 +682,7 @@ std::map<std::string, float> BulletPhysicsWorld::getRobotJointTorques(const std:
 
     for (auto& i : rn)
     {
-        if ((i->isTranslationalJoint() || i->isRotationalJoint()) && br)
+        if (i->isJoint() && br)
         {
             if (dynamicsRobot->isNodeActuated(i))
             {
@@ -1080,13 +1081,19 @@ void BulletPhysicsWorld::setRobotLinearVelocityRobotRootFrame(const std::string&
 
     if (!kinRob || kinRob->getName() != robotName)
     {
-        ARMARX_WARNING_S << "No robot available with name " << robotName;
+        ARMARX_WARNING_S << "No robot available with name '" << robotName << "'";
         return;
     }
 
     if (!kinRob->hasRobotNode(robotNodeName))
     {
-        ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
+        ARMARX_WARNING_S << "No robotNode available with name '" << robotNodeName << "'";
+        return;
+    }
+
+    if (!dynamicsRobot->hasDynamicsRobotNode(kinRob->getRobotNode(robotNodeName)))
+    {
+        ARMARX_WARNING_S << "No dynamic robotNode available with name '" << robotNodeName << "'";
         return;
     }
 
@@ -1118,13 +1125,19 @@ void BulletPhysicsWorld::setRobotAngularVelocityRobotRootFrame(const std::string
 
     if (!kinRob || kinRob->getName() != robotName)
     {
-        ARMARX_WARNING_S << "No robot available with name " << robotName;
+        ARMARX_WARNING_S << "No robot available with name '" << robotName << "'";
         return;
     }
 
     if (!kinRob->hasRobotNode(robotNodeName))
     {
-        ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
+        ARMARX_WARNING_S << "No robotNode available with name '" << robotNodeName << "'";
+        return;
+    }
+
+    if (!dynamicsRobot->hasDynamicsRobotNode(kinRob->getRobotNode(robotNodeName)))
+    {
+        ARMARX_WARNING_S << "No dynamic robotNode available with name '" << robotNodeName << "'";
         return;
     }
 
@@ -1136,7 +1149,10 @@ void BulletPhysicsWorld::setRobotAngularVelocityRobotRootFrame(const std::string
     globalPose(1, 3) = 0;
     globalPose(2, 3) = 0;
     auto globalNewPose = globalPose * newPose;
-    dynamicsRobot->getDynamicsRobotNode(kinRob->getRobotNode(robotNodeName))->setAngularVelocity(globalNewPose.block<3, 1>(0, 3));
+
+    RobotNodePtr robotNode = kinRob->getRobotNode(robotNodeName);
+    DynamicsObjectPtr dynamicsRobotNode = dynamicsRobot->getDynamicsRobotNode(robotNode);
+    dynamicsRobotNode->setAngularVelocity(globalNewPose.block<3, 1>(0, 3));
 }
 
 bool BulletPhysicsWorld::hasRobot(const std::string& robotName)
@@ -1530,7 +1546,7 @@ bool BulletPhysicsWorld::getRobotStatus(const std::string& robotName, NameValueM
 
     for (auto& i : rn)
     {
-        if ((i->isTranslationalJoint() or i->isRotationalJoint()))
+        if (i->isJoint())
         {
             jointAngles[i->getName()] = i->getJointValue();
 
@@ -1960,7 +1976,7 @@ void BulletPhysicsWorld::enableLogging(const std::string& robotName, const std::
 
     for (auto r : rnAll)
     {
-        if (r->isRotationalJoint() || r->isTranslationalJoint())
+        if (r->isJoint())
         {
             rnsJoints.push_back(r);
         }
@@ -2109,3 +2125,7 @@ DistanceInfo BulletPhysicsWorld::getDistance(const std::string& robotName, const
     return di;
 }
 
+RobotPtr BulletPhysicsWorld::adaptRobotToWorld(VirtualRobot::RobotPtr robot)
+{
+    return RobotFactory::createFlattenedModel(*robot);
+}

source/ArmarXSimulation/components/Simulator/BulletPhysicsWorld.h

@@ -61,6 +61,8 @@ namespace armarx
 
         virtual void initialize(int stepSizeMS, int bulletFixedTimeStepMS, int bulletFixedTimeStepMaxNrLoops, float maxRealTimeSimSpeed = 1, bool floorPlane = false, std::string floorTexture = std::string());
 
+        VirtualRobot::RobotPtr adaptRobotToWorld(VirtualRobot::RobotPtr robot) override;
+
         void actuateRobotJoints(const std::string& robotName, const std::map< std::string, float>& angles, const std::map< std::string, float>& velocities) override;
         void actuateRobotJointsPos(const std::string& robotName, const std::map< std::string, float>& angles) override;
         void actuateRobotJointsVel(const std::string& robotName, const std::map< std::string, float>& velocities) override;

source/ArmarXSimulation/components/Simulator/CMakeLists.txt

@@ -3,7 +3,6 @@ find_package(Simox 2.3.7 QUIET)
 armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
 armarx_build_if(MemoryX_FOUND "MemoryX not available")
 
-
 if (MujocoX_FOUND)
     include_mujoco()
     set(MUJOCO_LIBS
@@ -11,6 +10,7 @@ if (MujocoX_FOUND)
     )
 endif()
 
+find_package(sdformat13 REQUIRED)
 
 set(COMPONENT_LIBS
     ArmarXSimulationInterfaces
@@ -18,6 +18,7 @@ set(COMPONENT_LIBS
     MemoryXCore MemoryXMemoryTypes MemoryXVirtualRobotHelpers
     ${MUJOCO_LIBS}
     SimDynamics
+    ${SDFormat_LIBRARIES}
 )
 
 
@@ -51,6 +52,9 @@ set(HEADERS
 
 armarx_add_component("${SOURCES}" "${HEADERS}")
 
+target_include_directories(Simulator PUBLIC ${SDFormat_INCLUDE_DIRS})
+target_link_directories(Simulator PUBLIC ${SDFormat_LIBRARY_DIRS})
+
 if (MujocoX_FOUND)
     target_compile_definitions(Simulator PUBLIC MUJOCO_PHYSICS_WORLD)
 endif()

source/ArmarXSimulation/components/Simulator/KinematicsWorld.cpp

@@ -263,7 +263,7 @@ float KinematicsWorld::getRobotMass(const std::string& robotName)
 std::map< std::string, float> KinematicsWorld::getRobotJointAngles(const std::string& robotName)
 {
     ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
-    std::map< std::string, float> res;
+    std::map<std::string, float> res;
 
     VirtualRobot::RobotPtr kinRob = getRobot(robotName);
     if (!kinRob)
@@ -276,7 +276,7 @@ std::map< std::string, float> KinematicsWorld::getRobotJointAngles(const std::st
 
     for (auto& i : rn)
     {
-        if (i->isTranslationalJoint() || i->isRotationalJoint())
+        if (i->isJoint())
         {
             res[i->getName()] = i->getJointValue();
         }
@@ -929,7 +929,7 @@ void KinematicsWorld::calculateActualVelocities()
         IceUtil::Time timestamp = IceUtil::Time::secondsDouble(currentSimTimeSec);
         for (auto& i : rn)
         {
-            if ((i->isTranslationalJoint() || i->isRotationalJoint()))
+            if ((i->isJoint()))
             {
                 std::string n = i->getName();
 
@@ -1052,7 +1052,7 @@ bool KinematicsWorld::getRobotStatus(const std::string& robotName, NameValueMap&
     jointVelocities = ri.actualVelocities;
     for (auto& i : rn)
     {
-        if ((i->isTranslationalJoint() || i->isRotationalJoint()))
+        if ((i->isJoint()))
         {
             std::string n = i->getName();
 

source/ArmarXSimulation/components/Simulator/SimulatedWorld.h

@@ -241,6 +241,7 @@ namespace armarx
                               const std::string& filename, bool staticRobot = false,
                               float scaling = 1.0f,
                               bool colModel = false, bool selfCollisions = false);
+        virtual VirtualRobot::RobotPtr adaptRobotToWorld(VirtualRobot::RobotPtr);
 
         /*!
          * \brief toFramedPose Constructs a framed pose

source/ArmarXSimulation/components/Simulator/Simulator.h

@@ -394,6 +394,11 @@ namespace armarx
          */
         void addSceneFromJsonFile(const std::string& scenePath);
 
+        /**
+         * Add a scene defined in the SDF format to the simulation.
+         */
+        void addSceneFromSdfFile(const std::string& scenePath);
+
         /**
          * Add one object from an ArmarX JSON scene to the simulation.
          */