Merge branch 'feature/extended-robot-description' into 'master'

Feature/extended robot description

See merge request !473

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;

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;

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> -->

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
 

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()));

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>;