From 54edb4aaf7387dfc105190cfe1b2e7567276bc10 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 24 Sep 2024 11:27:26 +0200
Subject: [PATCH] Hand Unit / Controller: reloading of preshapes
---
.../common/AbstractHandControllerWrapper.cpp | 29 ++++++++++++++++---
.../common/AbstractHandControllerWrapper.h | 25 ++++++++++------
.../AbstractHandUnitControllerWrapper.h | 4 +++
3 files changed, 45 insertions(+), 13 deletions(-)
diff --git a/source/devices/ethercat/hand/common/AbstractHandControllerWrapper.cpp b/source/devices/ethercat/hand/common/AbstractHandControllerWrapper.cpp
index a27dc40b..bc554e85 100644
--- a/source/devices/ethercat/hand/common/AbstractHandControllerWrapper.cpp
+++ b/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
diff --git a/source/devices/ethercat/hand/common/AbstractHandControllerWrapper.h b/source/devices/ethercat/hand/common/AbstractHandControllerWrapper.h
index 111300cb..1e1b5471 100644
--- a/source/devices/ethercat/hand/common/AbstractHandControllerWrapper.h
+++ b/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;
diff --git a/source/devices/ethercat/hand/common/AbstractHandUnitControllerWrapper.h b/source/devices/ethercat/hand/common/AbstractHandUnitControllerWrapper.h
index 9c3daa69..35902146 100644
--- a/source/devices/ethercat/hand/common/AbstractHandUnitControllerWrapper.h
+++ b/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
--
GitLab