From 52b8c46d7708b4b97cd885e45b6ded007c8b0b45 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 29 Jul 2021 12:25:53 +0200
Subject: [PATCH] Add RobotUnitConverter base class and Armar6 version
---
.../Armar6RobotUnitConverter.cpp | 252 ++++++++++++++++++
.../Armar6RobotUnitConverter.h | 70 +++++
.../server/RobotStateMemory/CMakeLists.txt | 6 +
.../RobotUnitConverterInterface.cpp | 12 +
.../RobotUnitConverterInterface.h | 29 ++
5 files changed, 369 insertions(+)
create mode 100644 source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.cpp
create mode 100644 source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.h
create mode 100644 source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitConverterInterface.cpp
create mode 100644 source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitConverterInterface.h
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.cpp
new file mode 100644
index 000000000..d85e38be8
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.cpp
@@ -0,0 +1,252 @@
+#include "Armar6RobotUnitConverter.h"
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+
+
+namespace armarx::armem::server::robot_state
+{
+
+ Armar6RobotUnitConverter::Armar6RobotUnitConverter()
+ {
+ {
+ vector3fSetters["X"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.x() = f;
+ };
+ vector3fSetters["Y"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.y() = f;
+ };
+ vector3fSetters["Z"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.z() = f;
+ };
+ vector3fSetters["x"] = vector3fSetters["X"];
+ vector3fSetters["y"] = vector3fSetters["Y"];
+ vector3fSetters["z"] = vector3fSetters["Z"];
+ vector3fSetters["Rotation"] = vector3fSetters["Z"];
+ }
+ {
+ platformPoseGetters["acceleration"] = [](prop::arondto::Platform & p)
+ {
+ return &p.acceleration;
+ };
+ platformPoseGetters["absolutePosition"] = [](prop::arondto::Platform & p)
+ {
+ return &p.absolutePosition;
+ };
+ platformPoseGetters["relativePosition"] = [](prop::arondto::Platform & p)
+ {
+ return &p.relativePosition;
+ };
+ platformPoseGetters["velocity"] = [](prop::arondto::Platform & p)
+ {
+ return &p.velocity;
+ };
+ }
+ {
+ ftGetters["gravCompF"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.gravityCompensationForce;
+ };
+ ftGetters["gravCompT"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.gravityCompensationTorque;
+ };
+ ftGetters["f"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.force;
+ };
+ ftGetters["t"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.torque;
+ };
+ }
+ {
+ jointGetters["acceleration"] = [](prop::arondto::Joints & j)
+ {
+ return &j.accellerations;
+ };
+ jointGetters["gravityTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.gravityTorques;
+ };
+ jointGetters["inertiaTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.inertiaTorques;
+ };
+ jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.inverseDynamicsTorques;
+ };
+ jointGetters["position"] = [](prop::arondto::Joints & j)
+ {
+ return &j.positions;
+ };
+ jointGetters["torque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.torques;
+ };
+ jointGetters["velocity"] = [](prop::arondto::Joints & j)
+ {
+ return &j.velocities;
+ };
+ }
+ }
+
+
+ Armar6RobotUnitConverter::~Armar6RobotUnitConverter()
+ {
+ }
+
+
+ arondto::Proprioception Armar6RobotUnitConverter::convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description)
+ {
+ arondto::Proprioception dto;
+ dto.iterationID = data.iterationId;
+
+ for (const auto& [dataEntryName, dataEntry] : description.entries)
+ {
+ switch (dataEntry.type)
+ {
+ case RobotUnitDataStreaming::NodeTypeFloat:
+ {
+ const float value = RobotUnitDataStreamingReceiver::GetAs<float>(data, dataEntry);
+
+ const std::vector<std::string> split = simox::alg::split(dataEntryName, ".", false, false);
+ ARMARX_CHECK_EQUAL(split.size(), 3) << split;
+ const std::string& category = split.at(0);
+ const std::string& name = split.at(1);
+ const std::string& field = split.at(2);
+
+ ARMARX_CHECK_EQUAL(category, "sens") << category << " | " << dataEntryName;
+
+ if (name == "Platform")
+ {
+ // Platform
+ processPlatformEntry(dto.platform, field, value);
+ }
+ else if (simox::alg::starts_with(name, "FT"))
+ {
+ // Force Torque
+ processForceTorqueEntry(dto.forceTorque, name, field, value);
+ }
+ else
+ {
+ // Joint
+ processJointEntry(dto.joints, name, field, value);
+ }
+ }
+ break;
+
+ default:
+ {
+ ARMARX_DEBUG << "Ignoring non-float entry '" << dataEntryName << "'.";
+ }
+ break;
+ }
+ }
+ return dto;
+ }
+
+
+ template <class ValueT>
+ ValueT findByPrefix(const std::string& key,
+ const std::map<std::string, ValueT>& map)
+ {
+ for (const auto& [prefix, value] : map)
+ {
+ if (simox::alg::starts_with(key, prefix))
+ {
+ return value;
+ }
+ }
+ return nullptr;
+ }
+ template <class ValueT>
+ ValueT findBySuffix(const std::string& key,
+ const std::map<std::string, ValueT>& map)
+ {
+ for (const auto& [suffix, value] : map)
+ {
+ if (simox::alg::ends_with(key, suffix))
+ {
+ return value;
+ }
+ }
+ return nullptr;
+ }
+
+
+ void Armar6RobotUnitConverter::processPlatformEntry(
+ prop::arondto::Platform& dto,
+ const std::string& fieldName,
+ float value)
+ {
+ if (auto getter = findByPrefix(fieldName, platformPoseGetters))
+ {
+ if (Eigen::Vector3f* dst = getter(dto))
+ {
+ if (auto setter = findBySuffix(fieldName, vector3fSetters))
+ {
+ setter(*dst, value);
+ }
+ }
+ }
+ }
+
+
+ void Armar6RobotUnitConverter::processForceTorqueEntry(
+ std::map<std::string, prop::arondto::ForceTorque>& fts,
+ const std::string& name,
+ const std::string& fieldName,
+ float value)
+ {
+ std::vector<std::string> split = simox::alg::split(name, " ", false, false);
+ ARMARX_CHECK_EQUAL(split.size(), 2);
+ ARMARX_CHECK_EQUAL(split.at(0), "FT");
+
+ auto it = sidePrefixMap.find(split.at(1));
+ ARMARX_CHECK(it != sidePrefixMap.end()) << split.at(1);
+
+ const std::string& side = it->second;
+ processForceTorqueEntry(fts[side], fieldName, value);
+ }
+
+
+ void Armar6RobotUnitConverter::processForceTorqueEntry(
+ prop::arondto::ForceTorque& dto,
+ const std::string& fieldName,
+ float value)
+ {
+ if (auto getter = findByPrefix(fieldName, ftGetters))
+ {
+ if (Eigen::Vector3f* dst = getter(dto))
+ {
+ if (auto setter = findBySuffix(fieldName, vector3fSetters))
+ {
+ setter(*dst, value);
+ }
+ }
+ }
+ }
+
+
+ void Armar6RobotUnitConverter::processJointEntry(
+ prop::arondto::Joints& dto,
+ const std::string& name,
+ const std::string& fieldName,
+ float value)
+ {
+ if (auto getter = findByPrefix(fieldName, jointGetters))
+ {
+ if (std::map<std::string, float>* map = getter(dto))
+ {
+ (*map)[name] = value;
+ }
+ }
+ }
+
+}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.h b/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.h
new file mode 100644
index 000000000..d365edbe0
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/Armar6RobotUnitConverter.h
@@ -0,0 +1,70 @@
+#pragma once
+
+#include <map>
+#include <string>
+
+#include <Eigen/Core>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
+#include "RobotUnitConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state
+{
+
+ class Armar6RobotUnitConverter : public RobotUnitConverterInterface
+ {
+ public:
+
+ Armar6RobotUnitConverter();
+ virtual ~Armar6RobotUnitConverter() override;
+
+
+ arondto::Proprioception convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) override;
+
+
+ private:
+
+ void processPlatformEntry(
+ prop::arondto::Platform& dto,
+ const std::string& fieldName,
+ float value);
+
+ void processForceTorqueEntry(
+ std::map<std::string, prop::arondto::ForceTorque>& fts,
+ const std::string& name,
+ const std::string& fieldName,
+ float value);
+
+ void processForceTorqueEntry(
+ prop::arondto::ForceTorque& ft,
+ const std::string& fieldName,
+ float value);
+
+ void processJointEntry(
+ prop::arondto::Joints& dto,
+ const std::string& name,
+ const std::string& fieldName,
+ float value);
+
+
+ private:
+
+ std::map<std::string, std::function<void(Eigen::Vector3f&, float)> > vector3fSetters;
+
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::Platform&)> > platformPoseGetters;
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::ForceTorque&)> > ftGetters;
+ std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)> > jointGetters;
+
+ std::map<std::string, std::string> sidePrefixMap
+ {
+ { "R", "Right" },
+ { "L", "Left" },
+ };
+
+ };
+}
+
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
index a25161ff2..e4cfc53a9 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
@@ -20,6 +20,9 @@ set(SOURCES
RobotStateWriter.cpp
RobotUnitData.cpp
RobotUnitReader.cpp
+
+ RobotUnitConverterInterface.cpp
+ Armar6RobotUnitConverter.cpp
)
set(HEADERS
aron_conversions.h
@@ -27,6 +30,9 @@ set(HEADERS
RobotStateWriter.h
RobotUnitData.h
RobotUnitReader.h
+
+ RobotUnitConverterInterface.h
+ Armar6RobotUnitConverter.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitConverterInterface.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitConverterInterface.cpp
new file mode 100644
index 000000000..b72742bbc
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitConverterInterface.cpp
@@ -0,0 +1,12 @@
+#include "RobotUnitConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state
+{
+
+ RobotUnitConverterInterface::~RobotUnitConverterInterface()
+ {
+
+ }
+
+}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitConverterInterface.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitConverterInterface.h
new file mode 100644
index 000000000..f7ba3fcbf
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitConverterInterface.h
@@ -0,0 +1,29 @@
+#pragma once
+
+
+namespace armarx::RobotUnitDataStreaming
+{
+ struct TimeStep;
+ struct DataStreamingDescription;
+ struct DataEntry;
+}
+namespace armarx::armem::arondto
+{
+ class Proprioception;
+}
+
+namespace armarx::armem::server::robot_state
+{
+ class RobotUnitConverterInterface
+ {
+ public:
+
+ virtual ~RobotUnitConverterInterface();
+
+ virtual arondto::Proprioception convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) = 0;
+
+ };
+}
+
--
GitLab