From 991c106cc171755fcea4da5b5c6d19c4f30653c1 Mon Sep 17 00:00:00 2001
From: Christoph Pohl <christoph.pohl@kit.edu>
Date: Mon, 31 Jul 2023 11:50:32 +0200
Subject: [PATCH] Improve Force-Torque querying for RobotReader
Signed-off-by: ARMAR-DE <>
---
source/RobotAPI/libraries/armem_robot/types.h | 2 ++
.../libraries/armem_robot_state/aron_conversions.cpp | 4 ++++
.../client/common/VirtualRobotReader.cpp | 11 +++++++++++
.../client/common/VirtualRobotReader.h | 3 +++
4 files changed, 20 insertions(+)
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 2bc45c144..2eeff803f 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/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;
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 3749c892d..901d98dcb 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/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);
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 76fa10a4c..416585111 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/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
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index 3de8fbe8b..a80d9b028 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/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
--
GitLab