diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index 4f2d0c35fa4c95375cf67d09e332c1ecec5b77fa..cf5eb9b333563c0850e9e4fa5f7b618041958f63 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -250,6 +250,12 @@ namespace armarx
return select("HandControllerName");
}
+ std::string Arm::getHandUnitName() const
+ {
+ ARMARX_TRACE;
+ return select("HandUnitName");
+ }
+
std::string Arm::getHandRootNode() const
{
ARMARX_TRACE;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index 76c9b283f5d915e54d1f240c94d01aa9774d8e08..c1fa9b4d74194e7fd4b8c2a5748d81ef4ccfdcbd 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -64,6 +64,8 @@ namespace armarx
std::string getHandRootNode() const;
+ std::string getHandUnitName() const;
+
std::string getHandModelPath() const;
std::string getAbsoluteHandModelPath() const;
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml b/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
index aae0bba8efb2b42d6b426698911b36e6331ade27..3fb008da6919db746fd335735c6c2aa094e58a12 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
@@ -27,6 +27,9 @@
<ObjectChild key='end_effector'>
<string />
</ObjectChild>
+ <ObjectChild key='hand_unit'>
+ <string />
+ </ObjectChild>
<!-- Legacy old memory -->
<!-- <ObjectChild key='memory_hand_name'>
<string />
@@ -50,6 +53,21 @@
</Object>
+ <Object name='armarx::armem::arondto::ManipulationCapability'>
+ <ObjectChild key="affordance">
+ <string />
+ </ObjectChild>
+ <ObjectChild key="tcp">
+ <string optional="true"/>
+ </ObjectChild>
+ <ObjectChild key="shape">
+ <string optional="true"/>
+ </ObjectChild>
+ <ObjectChild key="type">
+ <string optional="true"/>
+ </ObjectChild>
+ </Object>
+
<Object name='armarx::armem::arondto::RobotInfo'>
<ObjectChild key="parts">
<Dict>
@@ -121,6 +139,14 @@
<armarx::armem::arondto::RobotInfo />
</ObjectChild>
+ <ObjectChild key='manipulationCapabilities'>
+ <Dict optional="true">
+ <List>
+ <armarx::armem::arondto::ManipulationCapability />
+ </List>
+ </Dict>
+ </ObjectChild>
+
<!-- <ObjectChild key='scaling'>
<float32 />
</ObjectChild> -->
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 9d03afe51e81fa99cf707eaf09ad517825d245f6..41f86a758d9ee8137d54351162ef42ac23a83bbf 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -117,6 +117,7 @@ namespace armarx::armem::robot_state::description
fromAron(dto.xml, bo.xml);
aron::fromAron(dto.visualization, bo.visualization);
aron::fromAron(dto.info, bo.info);
+ aron::fromAron(dto.manipulationCapabilities, bo.manipulationCapabilities);
}
void
@@ -126,6 +127,7 @@ namespace armarx::armem::robot_state::description
toAron(dto.xml, bo.xml);
aron::toAron(dto.visualization, bo.visualization);
aron::toAron(dto.info, bo.info);
+ aron::toAron(dto.manipulationCapabilities, bo.manipulationCapabilities);
}
} // namespace armarx::armem::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 0fdb753ba78f100ed62681da36dc5673a3bb1af8..ef4c89379d9f243e1c7b22699b93c983370f5614 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -3,6 +3,7 @@
#include <filesystem>
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/application/properties/PluginAll.h>
@@ -20,6 +21,7 @@
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
@@ -187,6 +189,10 @@ namespace armarx::armem::server::robot_state::description
e.torso_kinematic_chain =
tryGet([&]() { return arm.getTorsoKinematicChain(); });
+ e.hand_unit =
+ tryGet([&]() { return arm.getHandUnitName(); });
+
+
info.parts.emplace(side + "Arm", e);
}
}
@@ -208,11 +214,51 @@ namespace armarx::armem::server::robot_state::description
}
}
+ std::optional<
+ std::map<std::string, std::vector<armarx::armem::arondto::ManipulationCapability>>>
+ allManipulationCapabilities;
+ {
+ const auto robot = VirtualRobot::RobotIO::loadRobot(robotPath.toSystemPath());
+ ARMARX_CHECK_NOT_NULL(robot) << robotPath.toSystemPath();
+
+ for (const auto& eef : robot->getEndEffectors())
+ {
+ const auto& capabilities = eef->getManipulationCapabilities();
+ if (capabilities.has_value())
+ {
+ std::vector<armarx::armem::arondto::ManipulationCapability>
+ manipulationCapabilities;
+
+ for (const auto& vrCap : capabilities->capabilities)
+ {
+ arondto::ManipulationCapability dto;
+
+ dto.affordance = vrCap.affordance;
+ dto.tcp = vrCap.tcp;
+ dto.shape = vrCap.shape;
+ dto.type = vrCap.type;
+
+ manipulationCapabilities.push_back(dto);
+ }
+
+ // initialize on demand
+ if (not allManipulationCapabilities)
+ {
+ allManipulationCapabilities.emplace();
+ }
+
+ allManipulationCapabilities->emplace(eef->getName(),
+ manipulationCapabilities);
+ }
+ }
+ }
+
const armem::robot_state::description::RobotDescription robotDescription{
.name = kinematicUnit->getRobotName(),
.xml = {robotPath.serialize().package, robotPath.serialize().path},
.visualization = visualization,
- .info = info};
+ .info = info,
+ .manipulationCapabilities = allManipulationCapabilities};
// make sure that the package path is valid
ARMARX_CHECK(std::filesystem::exists(robotDescription.xml.toSystemPath()));
diff --git a/source/RobotAPI/libraries/armem_robot_state/types.h b/source/RobotAPI/libraries/armem_robot_state/types.h
index 80d11cc4f556da58b9489b4fd452f12e632b093e..aa30157a5bf548f7cbcad4c63650e089d1a3730d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/types.h
+++ b/source/RobotAPI/libraries/armem_robot_state/types.h
@@ -23,6 +23,7 @@
#include <filesystem>
#include <map>
+#include <optional>
#include <vector>
#include <Eigen/Geometry>
@@ -49,6 +50,9 @@ namespace armarx::armem::robot_state
arondto::RobotDescriptionVisualization visualization;
arondto::RobotInfo info;
+ std::optional<
+ std::map<std::string, std::vector<armarx::armem::arondto::ManipulationCapability>>>
+ manipulationCapabilities = std::nullopt;
};
using RobotDescriptions = std::vector<RobotDescription>;