RobotReader: querying forces and torques (interval)

source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp

@@ -352,7 +352,7 @@ namespace armarx::armem::robot_state
     // force torque for left and right
     std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
     RobotReader::queryForceTorque(const robot::RobotDescription& description,
-                                const armem::Time& timestamp) const
+                                  const armem::Time& timestamp) const
     {
 
         // Query all entities from provider.
@@ -381,6 +381,39 @@ namespace armarx::armem::robot_state
         return getForceTorque(qResult.memory, description.name);
     }
 
+    // force torque for left and right
+    std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
+    RobotReader::queryForceTorques(const robot::RobotDescription& description,
+                                   const armem::Time& start,
+                                   const armem::Time& end) const
+    {
+
+        // Query all entities from provider.
+        armem::client::query::Builder qb;
+
+        ARMARX_DEBUG << "Querying force torques description for robot: " << description;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties.proprioceptionCoreSegment)
+        .providerSegments().withName(description.name) // agent
+        .entities().all() // TODO
+        .snapshots().timeRange(start, end);
+        // 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_WARNING << qResult.errorMessage;
+            return std::nullopt;
+        }
+
+        return getForceTorques(qResult.memory, description.name);
+    }
+
 
     std::optional<robot::PlatformState>
     RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
@@ -409,14 +442,15 @@ namespace armarx::armem::robot_state
         return platformState;
     }
 
-    inline RobotReader::Hand fromHandName(const std::string& name)
+    inline RobotReader::Hand
+    fromHandName(const std::string& name)
     {
-        if(name == "Left")
+        if (name == "Left")
         {
             return RobotReader::Hand::Left;
         }
 
-        if(name == "Right")
+        if (name == "Right")
         {
             return RobotReader::Hand::Right;
         }
@@ -426,7 +460,7 @@ namespace armarx::armem::robot_state
 
     std::map<RobotReader::Hand, robot::ForceTorque>
     RobotReader::getForceTorque(const armarx::armem::wm::Memory& memory,
-                                       const std::string& name) const
+                                const std::string& name) const
     {
         std::map<RobotReader::Hand, robot::ForceTorque> forceTorques;
 
@@ -445,7 +479,7 @@ namespace armarx::armem::robot_state
                 ARMARX_CHECK(proprioception.has_value());
 
 
-                for(const auto& [handName, dtoFt]: proprioception->forceTorque)
+                for (const auto& [handName, dtoFt] : proprioception->forceTorque)
                 {
                     robot::ForceTorque forceTorque;
                     robot::fromAron(dtoFt, forceTorque);
@@ -458,6 +492,40 @@ namespace armarx::armem::robot_state
         return forceTorques;
     }
 
+    std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
+    RobotReader::getForceTorques(const armarx::armem::wm::Memory& memory,
+                                 const std::string& name) const
+    {
+        std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>> forceTorques;
+
+        // clang-format off
+        const armem::wm::CoreSegment& coreSegment = memory
+                .getCoreSegment(properties.proprioceptionCoreSegment);
+        // clang-format on
+
+        coreSegment.forEachEntity(
+            [&forceTorques](const wm::Entity& entity)
+            {
+                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->forceTorque)
+                {
+                    robot::ForceTorque forceTorque;
+                    robot::fromAron(dtoFt, forceTorque);
+
+                    const auto hand = fromHandName(handName);
+                    forceTorques[hand].emplace(entityInstance.id().timestamp, forceTorque);
+                }
+            });
+
+        return forceTorques;
+    }
+
 
     std::optional<robot::RobotDescription>
     RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,

source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h

@@ -85,6 +85,11 @@ namespace armarx::armem::robot_state
         queryForceTorque(const robot::RobotDescription& description,
                          const armem::Time& timestamp) const;
 
+        std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
+        queryForceTorques(const robot::RobotDescription& description,
+                          const armem::Time& start,
+                          const armem::Time& end) const;
+
     private:
         std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
                                                        const std::string& name) const;
@@ -102,6 +107,9 @@ namespace armarx::armem::robot_state
         std::map<RobotReader::Hand, robot::ForceTorque>
         getForceTorque(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
+ 
+        std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
+        getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
         struct Properties
         {