diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index d32b20d3ec89ca41cea6b00f68d85913349189a5..f917c29d05964d81c68cc94ee418a267eae54c63 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -67,10 +67,11 @@ namespace armarx
 
     void PlatformUnitSimulation::onStartPlatformUnit()
     {
-        PoseStampedPtr currentPose(new PoseStamped);
-        // FIXME: Take the timestamp from simulation
-        currentPose->timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
-        currentPose->pose = currentPlatformPose();
+        PoseStamped currentPose;
+        // FIXME(fabian.reister): set frame and agent
+        // FIXME(fabian.reister): Take the timestamp from simulation
+        currentPose.header.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+        currentPose.pose = currentPlatformPose();
 
         listenerPrx->reportPlatformPose(currentPose);
         simulationTask->start();
@@ -93,11 +94,11 @@ namespace armarx
         }
     }
 
-    void PlatformUnitSimulation::fillCommonPlatformFields(FramedAndStamped & framedAndStamped)
+    void PlatformUnitSimulation::fillCommonPlatformFields(FrameHeader& header)
     {
-        framedAndStamped.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
-        framedAndStamped.frame = GlobalFrame;
-        framedAndStamped.agent = RobotFrame;
+        header.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+        header.frame = GlobalFrame;
+        header.agent = RobotFrame;
     }
 
     void PlatformUnitSimulation::simulationFunction()
@@ -179,13 +180,13 @@ namespace armarx
         }
 
 
-        PoseStampedPtr platformPose(new PoseStamped);
-        fillCommonPlatformFields(*platformPose);
-        platformPose->pose = currentPlatformPose();
+        PoseStamped platformPose;
+        fillCommonPlatformFields(platformPose.header);
+        platformPose.pose = currentPlatformPose();
 
-        VelocityStampedPtr platformVelocity(new VelocityStamped);
-        fillCommonPlatformFields(*platformVelocity);
-        platformVelocity->velocity << vel[0], vel[1], 0.F, 0.F, 0.F, vel[2];
+        VelocityStamped platformVelocity;
+        fillCommonPlatformFields(platformVelocity.header);
+        platformVelocity.velocity << vel[0], vel[1], 0.F, 0.F, 0.F, vel[2];
 
         listenerPrx->reportPlatformPose(platformPose);
         listenerPrx->reportPlatformVelocity(platformVelocity);
@@ -204,9 +205,9 @@ namespace armarx
             this->orientationalAccuracy = orientationalAccuracy;
         }
 
-        PoseStampedPtr targetPose(new PoseStamped);
-        fillCommonPlatformFields(*targetPose);
-        targetPose->pose = VirtualRobot::MathTools::posrpy2eigen4f(targetPositionX, targetPositionY, 0, 0, 0, targetRotation);
+        PoseStamped targetPose;
+        fillCommonPlatformFields(targetPose.header);
+        targetPose.pose = VirtualRobot::MathTools::posrpy2eigen4f(targetPositionX, targetPositionY, 0, 0, 0, targetRotation);
 
         listenerPrx->reportNewTargetPose(targetPose);
     }
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.h b/source/RobotAPI/components/units/PlatformUnitSimulation.h
index c2161535fd04c99c151c4b239048d12eaff4fa4d..504c4f2f1c704223c573ce3739221f22b81c1e88 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.h
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.h
@@ -136,7 +136,7 @@ namespace armarx
 
     private:
         Eigen::Matrix4f currentPlatformPose() const;
-        void fillCommonPlatformFields(FramedAndStamped & framedAndStamped);
+        void fillCommonPlatformFields(FrameHeader& header);
 
     };
 }
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index cde9b632f59238b9a8c8d107e7e4990a1422aaf1..4e305e90e6d54d744d8a9d1a04eee0860cf1b8ba 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -138,7 +138,7 @@ namespace armarx
         getWriterControlStruct().startPosition = currentPosition;
         getWriterControlStruct().startOrientation = currentOrientation;
 
-        getWriterControlStruct().lastUpdate = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
+        getWriterControlStruct().lastUpdate = IceUtil::Time::microSeconds(currentPose.header.timestampInMicroSeconds);
         writeControlStruct();