Improve Force-Torque querying for RobotReader

Signed-off-by: ARMAR-DE <>

source/RobotAPI/libraries/armem_robot/types.h

@@ -37,7 +37,9 @@ namespace armarx::armem::robot
     struct ForceTorque
     {
         Eigen::Vector3f force;
+        Eigen::Vector3f gravityCompensatedForce;
         Eigen::Vector3f torque;
+        Eigen::Vector3f gravityCompensatedTorque;
     };
 
     using ToFArray = Eigen::MatrixXf;

source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp

@@ -88,14 +88,18 @@ namespace armarx::armem
     fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
     {
         aron::fromAron(dto.force, bo.force);
+        aron::fromAron(dto.gravityCompensationForce, bo.gravityCompensatedForce);
         aron::fromAron(dto.torque, bo.torque);
+        aron::fromAron(dto.gravityCompensationTorque, bo.gravityCompensatedTorque);
     }
 
     void
     toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
     {
         aron::toAron(dto.force, bo.force);
+        aron::toAron(dto.gravityCompensationForce, bo.gravityCompensatedForce);
         aron::toAron(dto.torque, bo.torque);
+        aron::toAron(dto.gravityCompensationTorque, bo.gravityCompensatedTorque);
     }
 
 

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

@@ -117,5 +117,16 @@ namespace armarx::armem::robot_state
         return nullptr;
     }
 
+    std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
+    VirtualRobotReader::queryForceTorque(const std::string& name, const Time& timestamp)
+    {
+        const auto description = queryDescription(name, timestamp);
+        if (not description.has_value())
+        {
+            return std::nullopt;
+        }
+        return RobotReader::queryForceTorque(description.value(), timestamp);
+    }
+
 
 } // namespace armarx::armem::robot_state

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

@@ -68,6 +68,9 @@ namespace armarx::armem::robot_state
                                  VirtualRobot::RobotIO::RobotDescription::eStructure,
                              bool blocking = true);
 
+        std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
+        queryForceTorque(const std::string& name, const armem::Time& timestamp);
+        using RobotReader::queryForceTorque;
 
     private:
         [[nodiscard]] VirtualRobot::RobotPtr