added CartesianNaturalPositionControllerGui

source/RobotAPI/gui-plugins/CMakeLists.txt

@@ -14,3 +14,4 @@ add_subdirectory(GuiHealthClient)
 add_subdirectory(CartesianWaypointControlGui)
 add_subdirectory(DebugDrawerGuiPlugin)
 add_subdirectory(ArViz)
+add_subdirectory(CartesianNaturalPositionController)

source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CMakeLists.txt

+armarx_set_target("CartesianNaturalPositionControllerGuiPlugin")
+
+armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
+
+set(SOURCES
+    CartesianNaturalPositionControllerGuiPlugin.cpp
+    CartesianNaturalPositionControllerWidgetController.cpp
+)
+
+set(HEADERS
+    CartesianNaturalPositionControllerGuiPlugin.h
+    CartesianNaturalPositionControllerWidgetController.h
+)
+
+set(GUI_MOC_HDRS ${HEADERS})
+
+set(GUI_UIS CartesianNaturalPositionControllerWidget.ui)
+
+set(COMPONENT_LIBS
+    VirtualRobot
+
+    SimpleConfigDialog
+
+    RobotAPIInterfaces
+    RobotAPICore
+    natik
+)
+
+if(ArmarXGui_FOUND)
+    armarx_gui_library(CartesianNaturalPositionControllerGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+endif()

source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.cpp

+/*
+ * 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::gui-plugins::CartesianNaturalPositionControllerGuiPlugin
+ * \author     armar-user ( armar-user at kit dot edu )
+ * \date       2020
+ * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "CartesianNaturalPositionControllerGuiPlugin.h"
+
+#include "CartesianNaturalPositionControllerWidgetController.h"
+
+namespace armarx
+{
+    CartesianNaturalPositionControllerGuiPlugin::CartesianNaturalPositionControllerGuiPlugin()
+    {
+        addWidget < CartesianNaturalPositionControllerWidgetController > ();
+    }
+}

source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.h

+/*
+ * 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::gui-plugins::CartesianNaturalPositionController
+ * \author     armar-user ( armar-user at kit dot edu )
+ * \date       2020
+ * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#pragma once
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+namespace armarx
+{
+    /**
+     * \class CartesianNaturalPositionControllerGuiPlugin
+     * \ingroup ArmarXGuiPlugins
+     * \brief CartesianNaturalPositionControllerGuiPlugin brief description
+     *
+     * Detailed description
+     */
+    class ARMARXCOMPONENT_IMPORT_EXPORT CartesianNaturalPositionControllerGuiPlugin:
+        public armarx::ArmarXGuiPlugin
+    {
+        Q_OBJECT
+        Q_INTERFACES(ArmarXGuiInterface)
+        Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
+    public:
+        /**
+         * All widgets exposed by this plugin are added in the constructor
+         * via calls to addWidget()
+         */
+        CartesianNaturalPositionControllerGuiPlugin();
+    };
+}

source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui

+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>CartesianNaturalPositionControllerWidget</class>
+ <widget class="QWidget" name="CartesianNaturalPositionControllerWidget">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>400</width>
+    <height>300</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>CartesianNaturalPositionControllerWidget</string>
+  </property>
+  <widget class="QWidget" name="widget" native="true">
+   <property name="geometry">
+    <rect>
+     <x>20</x>
+     <y>20</y>
+     <width>129</width>
+     <height>101</height>
+    </rect>
+   </property>
+   <layout class="QGridLayout" name="gridLayout">
+    <item row="0" column="0">
+     <widget class="QLabel" name="label">
+      <property name="text">
+       <string>Side</string>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="1">
+     <widget class="QComboBox" name="comboBoxSide"/>
+    </item>
+    <item row="1" column="0">
+     <widget class="QPushButton" name="pushButtonCreateController">
+      <property name="text">
+       <string>Create</string>
+      </property>
+     </widget>
+    </item>
+   </layout>
+  </widget>
+  <widget class="QWidget" name="widget_2" native="true">
+   <property name="geometry">
+    <rect>
+     <x>20</x>
+     <y>140</y>
+     <width>301</width>
+     <height>70</height>
+    </rect>
+   </property>
+   <layout class="QGridLayout" name="gridLayout_2">
+    <item row="1" column="4">
+     <widget class="QPushButton" name="pushButtonRyn">
+      <property name="text">
+       <string>ry-</string>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="2">
+     <widget class="QPushButton" name="pushButtonPzp">
+      <property name="text">
+       <string>pz+</string>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="3">
+     <widget class="QPushButton" name="pushButtonRxp">
+      <property name="text">
+       <string>rx+</string>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="1">
+     <widget class="QPushButton" name="pushButtonPyp">
+      <property name="text">
+       <string>py+</string>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="3">
+     <widget class="QPushButton" name="pushButtonRxn">
+      <property name="text">
+       <string>rx-</string>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="1">
+     <widget class="QPushButton" name="pushButtonPyn">
+      <property name="text">
+       <string>py-</string>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="5">
+     <widget class="QPushButton" name="pushButtonRzn">
+      <property name="text">
+       <string>rz-</string>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="5">
+     <widget class="QPushButton" name="pushButtonRzp">
+      <property name="text">
+       <string>rz+</string>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="0">
+     <widget class="QPushButton" name="pushButtonPxp">
+      <property name="text">
+       <string>px+</string>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="2">
+     <widget class="QPushButton" name="pushButtonPzn">
+      <property name="text">
+       <string>pz-</string>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="0">
+     <widget class="QPushButton" name="pushButtonPxn">
+      <property name="text">
+       <string>px-</string>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="4">
+     <widget class="QPushButton" name="pushButtonRyp">
+      <property name="text">
+       <string>ry+</string>
+      </property>
+     </widget>
+    </item>
+   </layout>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp

+/*
+ * 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::gui-plugins::CartesianNaturalPositionControllerWidgetController
+ * \author     armar-user ( armar-user at kit dot edu )
+ * \date       2020
+ * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+#include "CartesianNaturalPositionControllerWidgetController.h"
+
+#include <string>
+
+namespace armarx
+{
+    CartesianNaturalPositionControllerWidgetController::CartesianNaturalPositionControllerWidgetController()
+    {
+        std::lock_guard g{_allMutex};
+        _ui.setupUi(getWidget());
+
+
+
+        //connect(_ui.pushButtonStop,             SIGNAL(clicked()), this, SLOT(on_pushButtonStop_clicked()));
+        //connect(_ui.pushButtonExecute,          SIGNAL(clicked()), this, SLOT(on_pushButtonExecute_clicked()));
+        //connect(_ui.pushButtonZeroFT,           SIGNAL(clicked()), this, SLOT(on_pushButtonZeroFT_clicked()));
+        //connect(_ui.pushButtonSendSettings,     SIGNAL(clicked()), this, SLOT(on_pushButtonSendSettings_clicked()));
+        //connect(_ui.pushButtonCreateController, SIGNAL(clicked()), this, SLOT(on_pushButtonCreateController_clicked()));
+
+        //_ui.widgetSpacer->setVisible(false);
+        //_timer = startTimer(100);
+    }
+
+
+    CartesianNaturalPositionControllerWidgetController::~CartesianNaturalPositionControllerWidgetController()
+    {
+
+    }
+
+
+    void CartesianNaturalPositionControllerWidgetController::loadSettings(QSettings* settings)
+    {
+        std::lock_guard g{_allMutex};
+        _robotStateComponentName = settings->value("rsc", "Armar6StateComponent").toString().toStdString();
+        _robotUnitName = settings->value("ru", "Armar6Unit").toString().toStdString();
+    }
+
+    void CartesianNaturalPositionControllerWidgetController::saveSettings(QSettings* settings)
+    {
+        std::lock_guard g{_allMutex};
+        settings->setValue("rsc", QString::fromStdString(_robotStateComponentName));
+        settings->setValue("ru", QString::fromStdString(_robotUnitName));
+    }
+
+
+    void CartesianNaturalPositionControllerWidgetController::onInitComponent()
+    {
+        std::lock_guard g{_allMutex};
+        usingProxy(_robotStateComponentName);
+        usingProxy(_robotUnitName);
+    }
+
+
+    void CartesianNaturalPositionControllerWidgetController::onConnectComponent()
+    {
+        std::lock_guard g{_allMutex};
+        //proxies
+        {
+            _robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(_robotStateComponentName);
+            _robotUnit = getProxy<RobotUnitInterfacePrx>(_robotUnitName);
+        }
+        //robot
+        {
+            _robot = RemoteRobot::createLocalCloneFromFile(_robotStateComponent, VirtualRobot::RobotIO::eStructure);
+        }
+
+        _ui.comboBoxSide->addItem("Right");
+        _ui.comboBoxSide->addItem("Left");
+        _ui.comboBoxSide->setCurrentIndex(0);
+
+    }
+    void CartesianNaturalPositionControllerWidgetController::onDisconnectComponent()
+    {
+        std::lock_guard g{_allMutex};
+        deleteOldController();
+    }
+    void CartesianNaturalPositionControllerWidgetController::on_pushButtonCreateController_clicked()
+    {
+        std::lock_guard g{_allMutex};
+        deleteOldController();
+
+        static const std::string njointControllerClassName = "NJointCartesianWaypointController";
+        std::string side = _ui.comboBoxSide->currentText().toStdString();
+
+        VirtualRobot::RobotNodeSetPtr rns = _robot->getRobotNodeSet(side+"Arm");
+
+        VirtualRobot::RobotNodePtr sho1 = rns->getNode(1);
+        Eigen::Vector3f shoulder = sho1->getPositionInRootFrame();
+        VirtualRobot::RobotNodePtr elb = rns->getNode(4);
+
+        NaturalIK ik(side, shoulder, 1.3f);
+        _controller.reset(new CartesianNaturalPositionControllerProxy(ik, _robotUnit, side+"NaturalPosition", CartesianNaturalPositionControllerProxy::MakeConfig(rns->getName(), elb->getName())));
+        _controller->init();
+    }
+
+    //void CartesianNaturalPositionControllerWidgetController::on_pushButtonSendSettings_clicked()
+    //{
+    //    std::lock_guard g{_allMutex};
+    //    if (_controller)
+    //    {
+    //        ARMARX_IMPORTANT << "sending new config to " << _controllerName;
+    //        _controller->setConfig(readRunCfg());
+    //    }
+    //}
+
+
+    QPointer<QDialog> CartesianNaturalPositionControllerWidgetController::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 CartesianNaturalPositionControllerWidgetController::configured()
+    {
+        std::lock_guard g{_allMutex};
+        _robotStateComponentName = _dialog->getProxyName("rsc");
+        _robotUnitName = _dialog->getProxyName("ru");
+    }
+
+    /*NJointCartesianWaypointControllerRuntimeConfig CartesianWaypointControlGuiWidgetController::readRunCfg() const
+    {
+        std::lock_guard g{_allMutex};
+        NJointCartesianWaypointControllerRuntimeConfig cfg;
+
+        cfg.wpCfg.maxPositionAcceleration     = static_cast<float>(_ui.doubleSpinBoxMaxAccPos->value());
+        cfg.wpCfg.maxOrientationAcceleration  = static_cast<float>(_ui.doubleSpinBoxMaxAccOri->value());
+        cfg.wpCfg.maxNullspaceAcceleration    = static_cast<float>(_ui.doubleSpinBoxMaxAccNull->value());
+
+        cfg.wpCfg.kpJointLimitAvoidance       = static_cast<float>(_ui.doubleSpinBoxLimitAvoidKP->value());
+        cfg.wpCfg.jointLimitAvoidanceScale    = static_cast<float>(_ui.doubleSpinBoxLimitAvoidScale->value());
+
+        cfg.wpCfg.thresholdOrientationNear    = static_cast<float>(_ui.doubleSpinBoxOriNear->value());
+        cfg.wpCfg.thresholdOrientationReached = static_cast<float>(_ui.doubleSpinBoxOriReached->value());
+        cfg.wpCfg.thresholdPositionNear       = static_cast<float>(_ui.doubleSpinBoxPosNear->value());
+        cfg.wpCfg.thresholdPositionReached    = static_cast<float>(_ui.doubleSpinBoxPosReached->value());
+
+        cfg.wpCfg.maxOriVel                   = static_cast<float>(_ui.doubleSpinBoxOriMaxVel->value());
+        cfg.wpCfg.maxPosVel                   = static_cast<float>(_ui.doubleSpinBoxPosMaxVel->value());
+        cfg.wpCfg.kpOri                       = static_cast<float>(_ui.doubleSpinBoxOriKP->value());
+        cfg.wpCfg.kpPos                       = static_cast<float>(_ui.doubleSpinBoxPosKP->value());
+
+        cfg.forceThreshold                    = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
+        cfg.optimizeNullspaceIfTargetWasReached = _ui.checkBoxKeepOptimizing->isChecked();
+        cfg.forceThresholdInRobotRootZ          = _ui.checkBoxLimitinZ->isChecked();
+
+        return cfg;
+    }*/
+
+    void CartesianNaturalPositionControllerWidgetController::deleteOldController()
+    {
+        std::lock_guard g{_allMutex};
+        _controller->cleanup();
+    }
+
+
+}

source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h

+/*
+ * 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::gui-plugins::CartesianNaturalPositionControllerWidgetController
+ * @author     armar-user ( armar-user 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/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#include <RobotAPI/gui-plugins/CartesianNaturalPositionController/ui_CartesianNaturalPositionControllerWidget.h>
+#include <RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h>
+
+namespace armarx
+{
+    /**
+    \page RobotAPI-GuiPlugins-CartesianNaturalPositionController CartesianNaturalPositionController
+    \brief The CartesianNaturalPositionController allows visualizing ...
+
+    \image html CartesianNaturalPositionController.png
+    The user can
+
+    API Documentation \ref CartesianNaturalPositionControllerWidgetController
+
+    \see CartesianNaturalPositionControllerGuiPlugin
+    */
+
+    /**
+     * \class CartesianNaturalPositionControllerWidgetController
+     * \brief CartesianNaturalPositionControllerWidgetController brief one line description
+     *
+     * Detailed description
+     */
+    class ARMARXCOMPONENT_IMPORT_EXPORT
+        CartesianNaturalPositionControllerWidgetController:
+        public armarx::ArmarXComponentWidgetControllerTemplate < CartesianNaturalPositionControllerWidgetController >
+    {
+        Q_OBJECT
+
+    public:
+        /**
+         * Controller Constructor
+         */
+        explicit CartesianNaturalPositionControllerWidgetController();
+
+        /**
+         * Controller destructor
+         */
+        virtual ~CartesianNaturalPositionControllerWidgetController();
+
+        /**
+         * @see ArmarXWidgetController::loadSettings()
+         */
+        void loadSettings(QSettings* settings) override;
+
+        /**
+         * @see ArmarXWidgetController::saveSettings()
+         */
+        void saveSettings(QSettings* settings) override;
+
+        /**
+         * Returns the Widget name displayed in the ArmarXGui to create an
+         * instance of this class.
+         */
+        static QString GetWidgetName()
+        {
+            return "RobotControl.CartesianNaturalPositionController";
+        }
+
+        void onInitComponent() override;
+        void onConnectComponent() override;
+        void onDisconnectComponent() override;
+
+        QPointer<QDialog> getConfigDialog(QWidget* parent) override;
+        void configured() override;
+
+    public slots:
+        /* QT slot declarations */
+        void on_pushButtonCreateController_clicked();
+
+    signals:
+        /* QT signal declarations */
+
+    private:
+        void deleteOldController();
+
+
+        std::string                                     _robotStateComponentName;
+        std::string                                     _robotUnitName;
+        RobotStateComponentInterfacePrx                 _robotStateComponent;
+        RobotUnitInterfacePrx                           _robotUnit;
+        Ui::CartesianNaturalPositionControllerWidget    _ui;
+        QPointer<SimpleConfigDialog>                    _dialog;
+        CartesianNaturalPositionControllerProxyPtr      _controller;
+        std::string                                     _controllerName;
+        VirtualRobot::RobotPtr                          _robot;
+        std::vector<Eigen::Matrix4f>                    _lastParsedWPs;
+        bool                                            _supportsFT{false};
+        mutable std::recursive_mutex                    _allMutex;
+        int                                             _timer;
+    };
+}
+
+