diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 90a62cd3087bd6e2462865fb26a21c4140e4863c..5e26cffe5a1a66ffd836f782caf2b14050a58e40 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -19,3 +19,5 @@ add_subdirectory(natik)
add_subdirectory(armem)
add_subdirectory(aron)
+
+add_subdirectory(NJointControllerGuiPluginUtility)
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b044f98be56f2ee7fe799cf3272aac3120d30ccb
--- /dev/null
+++ b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt
@@ -0,0 +1,23 @@
+set(LIB_NAME NJointControllerGuiPluginUtility)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(COMPONENT_LIBS
+ SimpleConfigDialog
+ RobotAPIComponentPlugins
+)
+set(SOURCES
+ detail/NJointControllerGuiPluginBase.cpp
+)
+set(HEADERS
+ SpinBoxToPose.h
+ SpinBoxToVector.h
+ NJointControllerGuiPluginBase.h
+)
+set(GUI_MOC_HDRS detail/NJointControllerGuiPluginBase.h)
+set(GUI_UIS)
+
+if(ArmarXGui_FOUND)
+ armarx_gui_library("${LIB_NAME}" "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+endif()
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..75dce9850c6af59121ba763d95ce65eda06a6376
--- /dev/null
+++ b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h
@@ -0,0 +1,110 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::NJointControllerGuiPluginUtility
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "detail/NJointControllerGuiPluginBase.h"
+
+namespace armarx
+{
+ template<class Derived, class Proxy>
+ class NJointControllerGuiPluginBase:
+ public detail::_NJointControllerGuiPluginBase::Base
+ {
+ public:
+ NJointControllerGuiPluginBase(const std::string& ctrlClass) :
+ detail::_NJointControllerGuiPluginBase::Base(),
+ _ctrlClass{ctrlClass}
+ {}
+ ~NJointControllerGuiPluginBase()
+ {
+ deleteController();
+ }
+ public:
+ QString getWidgetName() const final override
+ {
+ return Derived::GetWidgetName();
+ }
+ QIcon getWidgetIcon() const final override
+ {
+ return Derived::GetWidgetIcon();
+ }
+ QIcon getWidgetCategoryIcon() const final override
+ {
+ return Derived::GetWidgetCategoryIcon();
+ }
+ public:
+ void createController() override
+ {
+ std::lock_guard g{_allMutex};
+ if (_controller || !_robotUnit)
+ {
+ return;
+ }
+ ARMARX_IMPORTANT << "Creating " << _ctrlClass << " '"
+ << _controllerName << "'";
+ _controller = Proxy::checkedCast(
+ _robotUnit->createNJointController(
+ _ctrlClass,
+ _controllerName,
+ readFullCFG()
+ ));
+ ARMARX_CHECK_NOT_NULL(_controller);
+ }
+ void activateController() override
+ {
+ std::lock_guard g{_allMutex};
+ if (!_controller)
+ {
+ return;
+ }
+ _controller->activateController();
+ }
+ void deactivateController() override
+ {
+ std::lock_guard g{_allMutex};
+ if (!_controller)
+ {
+ return;
+ }
+ _controller->deactivateController();
+ }
+ void deleteController() override
+ {
+ std::lock_guard g{_allMutex};
+ if (!_controller || !_robotUnit)
+ {
+ return;
+ }
+ ARMARX_IMPORTANT << "deleting old controller '" << _controllerName << "'";
+ try
+ {
+ _controller->deactivateAndDeleteController();
+ }
+ catch (...) {}
+ _controller = nullptr;
+ }
+ protected:
+ Proxy _controller;
+ std::string _ctrlClass;
+ };
+}
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h
new file mode 100644
index 0000000000000000000000000000000000000000..df5cb36988ca188cb9c8a9a0104df00efa00c3f0
--- /dev/null
+++ b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h
@@ -0,0 +1,102 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::NJointControllerGuiPluginUtility
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/math/convert.h>
+
+#include "SpinBoxToVector.h"
+
+namespace armarx
+{
+ template<class Qwid = QDoubleSpinBox>
+ class SpinBoxToPose
+ {
+ //set
+ public:
+ void setPos(const Eigen::Vector3f& pos)
+ {
+ _xyz.set(pos);
+ }
+ void setOri(const Eigen::Vector3f& rpy)
+ {
+ _rpy.set(rpy);
+ }
+ void setOri(const Eigen::Quaternionf& q)
+ {
+ setOri(simox::math::quat_to_rpy(q));
+ }
+ void setOri(const Eigen::Matrix3f& m)
+ {
+ setOri(simox::math::mat3f_to_rpy(m));
+ }
+ void setPose(const auto& pos, const auto& ori)
+ {
+ setPos(pos);
+ setOri(ori);
+ }
+ void setPose(const Eigen::Matrix4f& m)
+ {
+ setPos(simox::math::mat4f_to_pos(m));
+ setOri(simox::math::mat4f_to_rpy(m));
+ }
+ //get
+ public:
+ Eigen::Vector3f getPos() const
+ {
+ return _xyz.template get<Eigen::Vector3f>();
+ }
+ Eigen::Vector3f getRPY() const
+ {
+ return _rpy.template get<Eigen::Vector3f>();
+ }
+ Eigen::Quaternionf getQuat() const
+ {
+ return simox::math::rpy_to_quat(getRPY());
+ }
+ Eigen::Matrix3f getMat3() const
+ {
+ return simox::math::rpy_to_mat3f(getRPY());
+ }
+ Eigen::Matrix4f getMat4() const
+ {
+ return simox::math::pos_rpy_to_mat4f(getPos(), getRPY());
+ }
+ //set up
+ public:
+ void setXYZBoxes(Qwid* x, Qwid* y, Qwid* z)
+ {
+ _xyz.addBox(x);
+ _xyz.addBox(y);
+ _xyz.addBox(z);
+ }
+ void setRPYBoxes(Qwid* r, Qwid* p, Qwid* y)
+ {
+ _rpy.addBox(r);
+ _rpy.addBox(p);
+ _rpy.addBox(y);
+ }
+ private:
+ SpinBoxToVector<Qwid, 3> _xyz;
+ SpinBoxToVector<Qwid, 3> _rpy;
+ };
+}
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h
new file mode 100644
index 0000000000000000000000000000000000000000..b8da4b54d36b8beac8a87dc65cee3f49f51707f6
--- /dev/null
+++ b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h
@@ -0,0 +1,155 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::NJointControllerGuiPluginUtility
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <array>
+#include <vector>
+
+#include <Eigen/Dense>
+
+#include <QDoubleSpinBox>
+
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+namespace armarx
+{
+ template<class Qwid = QDoubleSpinBox, std::size_t Size = 0>
+ class SpinBoxToVector
+ {
+ public:
+ template<class Scalar, int Rows, int Cols>
+ void get(Eigen::Matrix<Scalar, Rows, Cols>& m) const
+ {
+ if constexpr(Size)
+ {
+ ARMARX_CHECK_EQUAL(Size, _spin.size());
+ }
+ if constexpr(Rows == -1 && Cols == -1)
+ {
+ m.resize(Size, 1);
+ }
+ else if constexpr(Rows == 1 && Cols == -1)
+ {
+ m.resize(Size);
+ }
+ else if constexpr(Rows == -1 && Cols == 1)
+ {
+ m.resize(Size);
+ }
+ else if constexpr(Cols == -1)
+ {
+ m.resize(Rows, Size / Rows);
+ }
+ else if constexpr(Rows == -1)
+ {
+ m.resize(Size / Cols, Cols);
+ }
+
+ ARMARX_CHECK_EQUAL(static_cast<std::size_t>(m.size()), _spin.size());
+ for (std::size_t i = 0; i < _spin.size(); ++i)
+ {
+ m.data()[i] = _spin.at(i)->value();
+ }
+ }
+ template<class Scalar>
+ void get(std::vector<Scalar>& m) const
+ {
+ if constexpr(Size)
+ {
+ ARMARX_CHECK_EQUAL(Size, _spin.size());
+ }
+ m.resize(_spin.size());
+ ARMARX_CHECK_EQUAL(m.size(), _spin.size());
+ for (std::size_t i = 0; i < _spin.size(); ++i)
+ {
+ m.at(i) = _spin.at(i)->value();
+ }
+ }
+ template<class T>
+ T get() const
+ {
+ if constexpr(Size)
+ {
+ ARMARX_CHECK_EQUAL(Size, _spin.size());
+ }
+ T m;
+ get(m);
+ return m;
+ }
+ template<class Scalar, int Rows, int Cols>
+ void set(const Eigen::Matrix<Scalar, Rows, Cols>& m)
+ {
+ if constexpr(Size)
+ {
+ ARMARX_CHECK_EQUAL(Size, _spin.size());
+ }
+ ARMARX_CHECK_EQUAL(m.size(), _spin.size());
+ for (int i = 0; i < _spin.size(); ++i)
+ {
+ _spin.at(i)->setValue(m.data()[i]);
+ }
+ }
+ void set(const VirtualRobot::RobotNodeSetPtr& set)
+ {
+ ARMARX_CHECK_NOT_NULL(set);
+ ARMARX_CHECK_EQUAL(set->getSize(), _spin.size());
+ for (std::size_t i = 0; i < _spin.size(); ++i)
+ {
+ _spin.at(i)->setValue(set->getNode(i)->getJointValue());
+ }
+ }
+ public:
+ void addBox(Qwid* s)
+ {
+ ARMARX_CHECK_NOT_NULL(s);
+ if constexpr(Size)
+ {
+ ARMARX_CHECK_LESS(_sz, Size);
+ _spin.at(_sz) = s;
+ }
+ else
+ {
+ _spin.emplace_back(s);
+ }
+ ++_sz;
+ }
+ void clear()
+ {
+ _sz = 0;
+ if constexpr(!Size)
+ {
+ _spin.clear();
+ }
+ }
+ private:
+ unsigned _sz = 0;
+ std::conditional_t <
+ Size,
+ std::array<Qwid*, Size>,
+ std::vector<Qwid*>
+ > _spin;
+ };
+}
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.cpp b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e1c51ffa21d7dd4489d28d548311b66275f6e4b6
--- /dev/null
+++ b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.cpp
@@ -0,0 +1,116 @@
+#include <QPushButton>
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+#include "NJointControllerGuiPluginBase.h"
+
+namespace armarx::detail::_NJointControllerGuiPluginBase
+{
+ void Base::createController() {}
+ void Base::activateController() {}
+ void Base::deactivateController() {}
+ void Base::deleteController() {}
+
+ Base::Base() :
+ _controllerName{getName() + "_Controller"}
+ {
+ std::replace(_controllerName.begin(), _controllerName.end(), '.', '_');
+ std::replace(_controllerName.begin(), _controllerName.end(), ':', '_');
+ }
+
+ Base::~Base()
+ {
+ if (_timer)
+ {
+ killTimer(_timer);
+ }
+ }
+
+ void Base::connectCreateAcivateDeactivateDelete(QPushButton* cr, QPushButton* ac, QPushButton* dc, QPushButton* de)
+ {
+ if (cr)
+ {
+ connect(cr, &QPushButton::clicked, this, &Base::createController);
+ }
+ if (ac)
+ {
+ connect(ac, &QPushButton::clicked, this, &Base::activateController);
+ }
+ if (dc)
+ {
+ connect(dc, &QPushButton::clicked, this, &Base::deactivateController);
+ }
+ if (de)
+ {
+ connect(de, &QPushButton::clicked, this, &Base::deleteController);
+ }
+ }
+
+ void Base::loadSettings(QSettings* settings)
+ {
+ std::lock_guard g{_allMutex};
+ getRobotStateComponentPlugin().setRobotStateComponentName(settings->value("rsc", "Armar6StateComponent").toString().toStdString());
+ getRobotUnitComponentPlugin().setRobotUnitName(settings->value("ru", "Armar6Unit").toString().toStdString());
+ }
+
+ void Base::saveSettings(QSettings* settings)
+ {
+ std::lock_guard g{_allMutex};
+ settings->setValue("rsc", QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
+ settings->setValue("ru", QString::fromStdString(getRobotUnitComponentPlugin().getRobotUnitName()));
+ }
+
+ QPointer<QDialog> Base::getConfigDialog(QWidget* parent)
+ {
+ std::lock_guard g{_allMutex};
+ if (!_dialog)
+ {
+ _dialog = new SimpleConfigDialog(parent);
+ _dialog->addProxyFinder<RobotUnitInterfacePrx>({"ru", "Robot Unit", "*Unit"});
+ _dialog->addProxyFinder<RobotStateComponentInterfacePrx>({"rsc", "Robot State Component", "*Component"});
+ }
+ return qobject_cast<SimpleConfigDialog*>(_dialog);
+ }
+
+ void Base::configured()
+ {
+ std::lock_guard g{_allMutex};
+ getRobotStateComponentPlugin().setRobotStateComponentName(_dialog->getProxyName("rsc"));
+ getRobotUnitComponentPlugin().setRobotUnitName(_dialog->getProxyName("ru"));
+ }
+
+ void Base::onInitComponent()
+ {
+ }
+
+ void Base::onConnectComponent()
+ {
+ std::lock_guard g{_allMutex};
+ //robot
+ _robot = addRobot("state robot", VirtualRobot::RobotIO::eStructure);
+ _timer = startTimer(100);
+ }
+
+ void Base::onDisconnectComponent()
+ {
+ std::lock_guard g{_allMutex};
+ deleteController();
+ }
+
+ void Base::timerEvent(QTimerEvent*)
+ {
+ std::lock_guard g{_allMutex};
+ killTimer(_timer);
+ _timer = 0;
+ if (_robot)
+ {
+ synchronizeLocalClone(_robot);
+ }
+ setupGuiAfterConnect();
+ }
+
+ void Base::setupGuiAfterConnect()
+ {
+
+ }
+}
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.h b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..21b15ab25e99fd7a6c30eb7742c9bfad3d5ac17b
--- /dev/null
+++ b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.h
@@ -0,0 +1,84 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::NJointControllerGuiPluginUtility
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <VirtualRobot/Robot.h>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+
+
+namespace armarx::detail::_NJointControllerGuiPluginBase
+{
+ class Base :
+ public ArmarXComponentWidgetController,
+ public virtual RobotUnitComponentPluginUser,
+ public virtual RobotStateComponentPluginUser
+ {
+ Q_OBJECT
+ public:
+ virtual NJointControllerConfigPtr readFullCFG() const = 0;
+ public Q_SLOTS: //for some reason using slots does not work -> use Q_SLOTS
+ virtual void createController();
+ virtual void activateController();
+ virtual void deactivateController();
+ virtual void deleteController();
+ public:
+ Base();
+ ~Base() override;
+
+ void connectCreateAcivateDeactivateDelete(QPushButton* cr, QPushButton* ac, QPushButton* dc, QPushButton* de);
+ public:
+ void loadSettings(QSettings* settings) override;
+ void saveSettings(QSettings* settings) override;
+ QPointer<QDialog> getConfigDialog(QWidget* parent) override;
+ void configured() override;
+ public:
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ public:
+ void timerEvent(QTimerEvent*) override;
+ virtual void setupGuiAfterConnect();
+ protected:
+ QPointer<SimpleConfigDialog> _dialog;
+ int _timer;
+
+ mutable std::recursive_mutex _allMutex;
+ RobotStateComponentInterfacePrx _robotStateComponent;
+ RobotUnitInterfacePrx _robotUnit;
+ VirtualRobot::RobotPtr _robot;
+
+ std::string _controllerName;
+ };
+}
+