diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 3374b9fe4d89377976fedd3b422604c424ee7f2e..d5f4844b574d3c6522f1d4461b7d9372c9f029c0 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -429,6 +429,11 @@ namespace armarx::armem::robot_state
         coreSegment.forEachEntity(
             [&jointMap](const wm::Entity& entity)
             {
+                if(not entity.getLatestSnapshot().hasInstance(0))
+                {
+                    return;
+                }
+
                 const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
 
                 const auto proprioception =
@@ -559,6 +564,37 @@ namespace armarx::armem::robot_state
     }
 
 
+
+    std::optional<std::map<RobotReader::Hand, robot::ToFArray>> RobotReader::queryToF(const robot::RobotDescription& description,
+                         const armem::Time& timestamp) const
+    {
+                // Query all entities from provider.
+        armem::client::query::Builder qb;
+
+        ARMARX_DEBUG << "Querying ToF data for robot: " << description;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(constants::proprioceptionCoreSegment)
+        .providerSegments().withName(description.name) // agent
+        .entities().all() // TODO
+        .snapshots().beforeOrAtTime(timestamp);
+        // clang-format on
+
+        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+        if (not qResult.success) /* c++20 [[unlikely]] */
+        {
+            ARMARX_VERBOSE << qResult.errorMessage;
+            return std::nullopt;
+        }
+
+        return getToF(qResult.memory, description.name);
+    }
+
+
     std::optional<robot::PlatformState>
     RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
                                        const std::string& name) const
@@ -573,6 +609,11 @@ namespace armarx::armem::robot_state
         coreSegment.forEachEntity(
             [&platformState](const wm::Entity& entity)
             {
+                if(not entity.getLatestSnapshot().hasInstance(0))
+                {
+                    return;
+                }
+
                 const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
 
                 const auto proprioception =
@@ -616,6 +657,11 @@ namespace armarx::armem::robot_state
         coreSegment.forEachEntity(
             [&forceTorques](const wm::Entity& entity)
             {
+                if(not entity.getLatestSnapshot().hasInstance(0))
+                {
+                    return;
+                }
+
                 const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
 
                 const auto proprioception =
@@ -668,6 +714,48 @@ namespace armarx::armem::robot_state
         return forceTorques;
     }
 
+    std::map<RobotReader::Hand, robot::ToFArray>
+    RobotReader::getToF(const armarx::armem::wm::Memory& memory,
+                                const std::string& name) const
+    {
+        std::map<RobotReader::Hand, robot::ToFArray> tofs;
+
+        // clang-format off
+        const armem::wm::CoreSegment& coreSegment = memory
+                .getCoreSegment(constants::proprioceptionCoreSegment);
+        // clang-format on
+
+        coreSegment.forEachEntity(
+            [&tofs](const wm::Entity& entity)
+            {
+                ARMARX_DEBUG << "Processing ToF element";
+
+                if(not entity.getLatestSnapshot().hasInstance(0))
+                {
+                    return;
+                }
+                const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+                const auto proprioception =
+                    tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+                ARMARX_CHECK(proprioception.has_value());
+
+
+                for (const auto& [handName, dtoFt] : proprioception->tof)
+                {
+                    ARMARX_DEBUG << "Processing ToF element for hand `" << handName << "`";
+
+                    robot::ToFArray tof;
+                    fromAron(dtoFt, tof);
+
+                    const auto hand = fromHandName(handName);
+                    tofs.emplace(hand, tof);
+                }
+            });
+
+        return tofs;
+    }
+
 
     std::optional<robot::RobotDescription>
     RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index 9474c52376c5df6bad41ba0360015ed6681dbb69..da57e74afa0fedc79570a31a064ef0661a0cd8f3 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -99,6 +99,10 @@ namespace armarx::armem::robot_state
                           const armem::Time& start,
                           const armem::Time& end) const;
 
+
+        std::optional<std::map<Hand, robot::ToFArray>> queryToF(const robot::RobotDescription& description,
+                         const armem::Time& timestamp) const;
+
         /**
          * @brief retrieve the robot's pose in the odometry frame. 
          *
@@ -141,6 +145,9 @@ namespace armarx::armem::robot_state
         std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
         getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
+        std::map<RobotReader::Hand, robot::ToFArray>
+        getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+
         struct Properties
         {
         } properties;