From 38eb0ab8f3d2eb15012a9f082aa3c1e2ac1e46eb Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 26 Oct 2021 17:34:48 +0200
Subject: [PATCH] RobotReader: querying forces and torques (interval)
---
.../client/common/RobotReader.cpp | 80 +++++++++++++++++--
.../client/common/RobotReader.h | 8 ++
2 files changed, 82 insertions(+), 6 deletions(-)
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 6645b8d6c..5a5c279ad 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/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,
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 51fe24008..884c85fd9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/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
{
--
GitLab