diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
index 426511a9a8a59dd5cba9545f08f8479c0104d96c..21e579c803afd33d2ad5864de6c4b283ae69733c 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -123,4 +123,16 @@ namespace armarx::armem
ARMARX_ERROR << "Not implemented yet.";
}
+ void robot::fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
+ {
+ bo.force = dto.force;
+ bo.torque = dto.torque;
+ }
+
+ void robot::toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
+ {
+ dto.force = bo.force;
+ dto.torque = bo.torque;
+ }
+
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
index e6b462fa866d0c011cb0d899f08ad4e804e3f210..62744724f1d4c42ebe882cf4072e87d43428dd02 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -30,6 +30,9 @@ namespace armarx::armem::robot
void fromAron(const armarx::armem::prop::arondto::Platform& dto, PlatformState& bo);
void toAron(armarx::armem::prop::arondto::Platform& dto, const PlatformState& bo);
+ void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, ForceTorque& bo);
+ void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const ForceTorque& bo);
+
void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index ad7454d6fb4f74b49b6909a3a6d362c302203fc9..0a8473073f5b30dce3cb9f2d96682b4184a6a9b3 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -35,6 +35,12 @@ namespace armarx::armem::robot
Twist twist;
};
+ struct ForceTorque
+ {
+ Eigen::Vector3f force;
+ Eigen::Vector3f torque;
+ };
+
struct RobotState
{
using JointMap = std::map<std::string, float>;
@@ -46,6 +52,7 @@ namespace armarx::armem::robot
JointMap jointMap;
};
+
struct Robot
{
RobotDescription description;
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 4da91df52e77ecd83fe4c061d0a969bce8bae32f..6645b8d6c4c86d4d6db30c6d0b5244e71c9cfffd 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -4,18 +4,18 @@
#include <optional>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-#include "RobotAPI/libraries/armem_robot/types.h"
+#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/PackagePath.h>
+#include "RobotAPI/libraries/armem_robot/types.h"
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
@@ -24,14 +24,15 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
-
+
RobotReader::RobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem), transformReader(memoryNameSystem)
{
}
- void RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ void
+ RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
transformReader.registerPropertyDefinitions(def);
@@ -42,7 +43,8 @@ namespace armarx::armem::robot_state
propertyPrefix + "proprioceptionSegment");
}
- void RobotReader::connect()
+ void
+ RobotReader::connect()
{
transformReader.connect();
@@ -51,7 +53,8 @@ namespace armarx::armem::robot_state
try
{
memoryReader = memoryNameSystem.useReader(properties.memoryName);
- ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName << "'";
+ ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -60,8 +63,8 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::Robot> RobotReader::get(const std::string& name,
- const armem::Time& timestamp)
+ std::optional<robot::Robot>
+ RobotReader::get(const std::string& name, const armem::Time& timestamp)
{
const auto description = queryDescription(name, timestamp);
@@ -74,20 +77,21 @@ namespace armarx::armem::robot_state
return get(*description, timestamp);
}
- robot::Robot RobotReader::get(const robot::RobotDescription& description,
- const armem::Time& timestamp)
+ robot::Robot
+ RobotReader::get(const robot::RobotDescription& description, const armem::Time& timestamp)
{
robot::Robot robot{.description = description,
- .instance = "", // TODO(fabian.reister):
- .config = {}, // will be populated by synchronize
- .timestamp = timestamp};
+ .instance = "", // TODO(fabian.reister):
+ .config = {}, // will be populated by synchronize
+ .timestamp = timestamp};
synchronize(robot, timestamp);
return robot;
}
- bool RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
+ bool
+ RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
{
auto state = queryState(obj.description, timestamp);
@@ -119,7 +123,8 @@ namespace armarx::armem::robot_state
if (not memoryReader)
{
- ARMARX_WARNING << "Memory reader is null. Did you forget to call RobotReader::connect() in onConnectComponent()?";
+ ARMARX_WARNING << "Memory reader is null. Did you forget to call "
+ "RobotReader::connect() in onConnectComponent()?";
return std::nullopt;
}
@@ -162,8 +167,7 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return robot::RobotState
- {
+ return robot::RobotState{
.timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
}
@@ -200,10 +204,9 @@ namespace armarx::armem::robot_state
}
-
std::optional<robot::PlatformState>
RobotReader::queryPlatformState(const robot::RobotDescription& description,
- const armem::Time& timestamp) const
+ const armem::Time& timestamp) const
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -266,11 +269,12 @@ namespace armarx::armem::robot_state
// clang-format on
const armem::wm::EntityInstance* instance = nullptr;
- providerSegment.forEachInstance([&instance](const wm::EntityInstance & i)
- {
- instance = &i;
- return false; // break
- });
+ providerSegment.forEachInstance(
+ [&instance](const wm::EntityInstance& i)
+ {
+ instance = &i;
+ return false; // break
+ });
if (!instance)
{
ARMARX_WARNING << "No entity snapshots found";
@@ -283,10 +287,10 @@ namespace armarx::armem::robot_state
// FIXME remove this, use armem/util/util.h
template <typename AronClass>
- std::optional<AronClass> tryCast(const wm::EntityInstance& item)
+ std::optional<AronClass>
+ tryCast(const wm::EntityInstance& item)
{
- static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass,
- AronClass>::value);
+ static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass, AronClass>::value);
try
{
@@ -312,28 +316,29 @@ namespace armarx::armem::robot_state
.getCoreSegment(properties.proprioceptionCoreSegment);
// clang-format on
- coreSegment.forEachEntity([&jointMap](const wm::Entity & entity)
- {
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ coreSegment.forEachEntity(
+ [&jointMap](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());
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
- const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
-
+ const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
- // const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
- // if (not jointState)
- // {
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- // return;
- // }
+ // const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
+ // if (not jointState)
+ // {
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ // return;
+ // }
- jointMap = joints.position;
+ jointMap = joints.position;
- // jointMap.emplace(jointState->name, jointState->position);
- });
+ // jointMap.emplace(jointState->name, jointState->position);
+ });
if (jointMap.empty())
{
@@ -343,9 +348,43 @@ namespace armarx::armem::robot_state
return jointMap;
}
- std::optional<robot::PlatformState>
- RobotReader::getRobotPlatformState (const armarx::armem::wm::Memory& memory,
- const std::string& name) const
+
+ // 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
+ {
+
+ // 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().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_WARNING << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getForceTorque(qResult.memory, description.name);
+ }
+
+
+ std::optional<robot::PlatformState>
+ RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
{
std::optional<robot::PlatformState> platformState;
@@ -354,21 +393,70 @@ namespace armarx::armem::robot_state
.getCoreSegment(properties.proprioceptionCoreSegment);
// clang-format on
- coreSegment.forEachEntity([&platformState](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());
+ coreSegment.forEachEntity(
+ [&platformState](const wm::Entity& entity)
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- platformState = robot::PlatformState(); // initialize optional
- robot::fromAron(proprioception->platform, platformState.value());
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
- });
+ platformState = robot::PlatformState(); // initialize optional
+ robot::fromAron(proprioception->platform, platformState.value());
+ });
return platformState;
}
+ inline RobotReader::Hand fromHandName(const std::string& name)
+ {
+ if(name == "Left")
+ {
+ return RobotReader::Hand::Left;
+ }
+
+ if(name == "Right")
+ {
+ return RobotReader::Hand::Right;
+ }
+
+ throw LocalException("Unknown hand name `" + name + "`!");
+ }
+
+ std::map<RobotReader::Hand, robot::ForceTorque>
+ RobotReader::getForceTorque(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+ std::map<RobotReader::Hand, 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.emplace(hand, forceTorque);
+ }
+ });
+
+ return forceTorques;
+ }
std::optional<robot::RobotDescription>
@@ -382,10 +470,8 @@ namespace armarx::armem::robot_state
// clang-format on
const armem::wm::EntityInstance* instance = nullptr;
- providerSegment.forEachInstance([&instance](const wm::EntityInstance & i)
- {
- instance = &i;
- });
+ providerSegment.forEachInstance([&instance](const wm::EntityInstance& i)
+ { instance = &i; });
if (!instance)
{
ARMARX_WARNING << "No entity snapshots found";
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 e24bc65fa8a9ca14213bfe79007fc8519f02d176..51fe24008eaba3a4edb6953a76753436c31595ef 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -26,10 +26,8 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
-
#include <RobotAPI/libraries/armem_robot/client/interfaces.h>
#include <RobotAPI/libraries/armem_robot/types.h>
-
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
@@ -59,10 +57,10 @@ namespace armarx::armem::robot_state
const armem::Time& timestamp) override;
std::optional<robot::RobotDescription> queryDescription(const std::string& name,
- const armem::Time& timestamp);
+ const armem::Time& timestamp);
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
- const armem::Time& timestamp);
+ const armem::Time& timestamp);
std::optional<robot::RobotState::JointMap>
queryJointState(const robot::RobotDescription& description,
@@ -76,25 +74,40 @@ namespace armarx::armem::robot_state
queryPlatformState(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
+
+ enum class Hand
+ {
+ Left,
+ Right
+ };
+
+ std::optional<std::map<Hand, robot::ForceTorque>>
+ queryForceTorque(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const;
+
private:
std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
- const std::string& name) const;
+ const std::string& name) const;
std::optional<robot::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
-
+
std::optional<robot::RobotState::JointMap>
getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
std::optional<robot::PlatformState>
- getRobotPlatformState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ getRobotPlatformState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
+
+ std::map<RobotReader::Hand, robot::ForceTorque>
+ getForceTorque(const armarx::armem::wm::Memory& memory, const std::string& name) const;
struct Properties
{
- std::string memoryName = "RobotState";
- std::string descriptionCoreSegment = "Description";
- std::string localizationCoreSegment = "Localization";
+ std::string memoryName = "RobotState";
+ std::string descriptionCoreSegment = "Description";
+ std::string localizationCoreSegment = "Localization";
std::string proprioceptionCoreSegment = "Proprioception";
} properties;