Hand Unit / Controller: reloading of preshapes

source/devices/ethercat/hand/common/AbstractHandControllerWrapper.cpp

@@ -4,18 +4,29 @@
 #include <VirtualRobot/EndEffector/EndEffector.h>
 #include <VirtualRobot/Robot.h>
 #include <VirtualRobot/RobotConfig.h>
+#include <VirtualRobot/XML/BaseIO.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
 
 #include <ArmarXCore/core/time.h>
 
 namespace devices::ethercat::hand::common
 {
     std::map<std::string, std::map<std::string, float>>
-    AbstractHandControllerWrapper::preshapesFromRobot(VirtualRobot::RobotPtr robot,
-                                                      std::string handName,
-                                                      std::string eefName)
+    AbstractHandControllerWrapper::preshapesFromRobot(const VirtualRobot::RobotPtr& robot,
+                                                      const std::string& handName,
+                                                      const std::string& eefName,
+                                                      const bool forceRobotFileReload)
     {
         std::map<std::string, std::map<std::string, float>> preshapesByName;
 
+        auto robotToUse = robot;
+        if(forceRobotFileReload)
+        {
+            robotToUse = VirtualRobot::RobotIO::loadRobot(robot->getFilename(), VirtualRobot::BaseIO::eStructure);
+        }
+
         const auto eef = robot->getEndEffector(eefName);
         const std::vector<std::string> preshapeNames = eef->getPreshapes();
 
@@ -38,6 +49,12 @@ namespace devices::ethercat::hand::common
         return preshapesByName;
     }
 
+    void 
+    AbstractHandControllerWrapper::reloadPreshapes()
+    {
+        preshapes_ = preshapesFromRobot(robot_, handName_, eef_, true);
+    }
+
     void
     AbstractHandControllerWrapper::setShape(const std::string& shape)
     {
@@ -99,5 +116,9 @@ namespace devices::ethercat::hand::common
         }
     }
 
-
+    std::vector<std::string>
+    AbstractHandControllerWrapper::getSupportedShapeNames() const
+    {
+        return simox::alg::get_keys(preshapes_);
+    }
 } // namespace devices::ethercat::hand::common

source/devices/ethercat/hand/common/AbstractHandControllerWrapper.h

@@ -21,7 +21,6 @@
 
 #pragma once
 
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
 #include <VirtualRobot/VirtualRobot.h>
 
 #include "RobotAPI/interface/units/RobotUnit/NJointController.h"
@@ -40,20 +39,21 @@ namespace devices::ethercat::hand::common
         AbstractHandControllerWrapper(VirtualRobot::RobotPtr robot,
                                       std::string handName,
                                       std::string eef) :
-            preshapes_(preshapesFromRobot(robot, handName, eef))
+            preshapes_(preshapesFromRobot(robot, handName, eef)),
+            robot_(robot),
+            handName_(handName),
+            eef_(eef)
         {
         }
 
-        std::vector<std::string>
-        getSupportedShapeNames() const override
-        {
-            return simox::alg::get_keys(preshapes_);
-        }
+        std::vector<std::string> getSupportedShapeNames() const override;
 
         void setShape(const std::string& shape) override;
 
         std::map<std::string, float> getShapeJointValues(const std::string& shape) override;
 
+        void reloadPreshapes() override;
+
     protected:
         virtual armarx::NJointControllerInterface& getController() = 0;
 
@@ -69,10 +69,17 @@ namespace devices::ethercat::hand::common
 
     private:
         static std::map<std::string, std::map<std::string, float>>
-        preshapesFromRobot(VirtualRobot::RobotPtr robot, std::string handName, std::string eefName);
+        preshapesFromRobot(const VirtualRobot::RobotPtr& robot,
+                           const std::string& handName,
+                           const std::string& eefName,
+                           bool forceRobotFileReload = false);
 
     private:
-        const PreshapesByNameT preshapes_;
+        PreshapesByNameT preshapes_;
+
+        const VirtualRobot::RobotPtr robot_;
+        const std::string handName_;
+        const std::string eef_;
 
     private:
         static constexpr int ENSURE_ACTIVE_TIMEOUT_MS = 200;

source/devices/ethercat/hand/common/AbstractHandUnitControllerWrapper.h

@@ -23,6 +23,9 @@ namespace devices::ethercat::hand::common
         virtual std::vector<std::string> getSupportedShapeNames() const = 0;
         virtual std::map<std::string, float> getShapeJointValues(const std::string& shape) = 0;
 
+        // virtual void reloadPreshapes() = 0;
+        virtual void reloadPreshapes() {};
+
         virtual std::string
         describeHandState() const
         {
@@ -31,6 +34,7 @@ namespace devices::ethercat::hand::common
 
         virtual void setJointAngles(const armarx::NameValueMap& targetJointAngles) = 0;
         virtual std::map<std::string, float> getActualJointValues() = 0;
+
     };
 
 } // namespace devices::ethercat::hand::common