diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index bdf8dc94e2068fb373f5f5984604d262bb78d1a1..3f8f026aa8a44e9e27158c132940720683d15045 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -91,11 +91,12 @@ void KinematicUnitObserver::onConnectObserver()
         throw UserException("RobotNodeSet not defined");
     }
 
-    VirtualRobot::RobotNodeSetPtr robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
+    auto robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
 
     std::vector< VirtualRobot::RobotNodePtr > robotNodes;
     robotNodes = robotNodeSetPtr->getAllRobotNodes();
-
+    auto robotNodeNames = robotNodeSetPtr->getNodeNames();
+    this->robotNodes = std::set<std::string>(robotNodeNames.begin(), robotNodeNames.end());
     // register all channels
     offerChannel("jointangles", "Joint values of the " + robotNodeSetName + " kinematic chain");
     offerChannel("jointvelocities", "Joint velocities of the " + robotNodeSetName + " kinematic chain");
@@ -298,14 +299,20 @@ void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelN
         {
             for (const auto& it : nameValueMap)
             {
-                map[it.first] = new Variant(it.second);
+                if (robotNodes.count(it.first))
+                {
+                    map[it.first] = new Variant(it.second);
+                }
             }
         }
         else
         {
             for (const auto& it : nameValueMap)
             {
-                map[it.first] = new TimedVariant(new Variant(it.second), IceUtil::Time::microSeconds(timestamp));
+                if (robotNodes.count(it.first))
+                {
+                    map[it.first] = new TimedVariant(new Variant(it.second), IceUtil::Time::microSeconds(timestamp));
+                }
             }
         }
         setDataFieldsFlatCopy(channelName, map);
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.h b/source/RobotAPI/components/units/KinematicUnitObserver.h
index 1f306d6863f85f203d9cd24db6486494ff15fd8f..0bbdbb054c13f40166904d51b4be2d6973993707 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.h
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.h
@@ -155,6 +155,7 @@ namespace armarx
         Mutex initializedChannelsMutex;
     private:
         std::string robotNodeSetName;
+        std::set<std::string> robotNodes;
     };