CartesianWaypointControlGuiWidgetController.cpp

Go to the documentation of this file.

/*
 * 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::CartesianWaypointControlGuiWidgetController
 * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * \date       2019
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CartesianWaypointControlGuiWidgetController.h"
 
#include <random>
#include <string>
 
#include <QClipboard>
 
#include <SimoxUtility/json.h>
 
#include <ArmarXCore/util/CPPUtility/container.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
namespace armarx
{
    CartesianWaypointControlGuiWidgetController::CartesianWaypointControlGuiWidgetController() :
        NJointControllerGuiPluginBase("NJointCartesianWaypointController")
    {
        using T = CartesianWaypointControlGuiWidgetController;
        std::lock_guard g{_allMutex};
        _ui.setupUi(getWidget());
        connect(_ui.radioButtonWPJson, &QPushButton::clicked, this, &T::triggerParsing);
        connect(_ui.radioButton4f, &QPushButton::clicked, this, &T::triggerParsing);
        connect(_ui.textEditWPs, &QTextEdit::textChanged, this, &T::triggerParsing);
 
        connect(
            _ui.pushButtonExecute, &QPushButton::clicked, this, &T::on_pushButtonExecute_clicked);
        connect(_ui.pushButtonZeroFT, &QPushButton::clicked, this, &T::on_pushButtonZeroFT_clicked);
        connect(_ui.pushButtonSendSettings,
                &QPushButton::clicked,
                this,
                &T::on_pushButtonSendSettings_clicked);
        connect(_ui.pushButtonCopyCurrentPose, &QPushButton::clicked, this, &T::copyCurrentPose);
 
        connectCreateAcivateDeactivateDelete(
            _ui.pushButtonCreateController, nullptr, _ui.pushButtonStop, nullptr);
 
        _ui.widgetSpacer->setVisible(false);
        _timer = startTimer(50);
    }
 
    CartesianWaypointControlGuiWidgetController::~CartesianWaypointControlGuiWidgetController()
    {
        if (_timer)
        {
            killTimer(_timer);
        }
    }
 
    void
    CartesianWaypointControlGuiWidgetController::onConnectComponent()
    {
        std::lock_guard g{_allMutex};
        NJointControllerGuiPluginBase::onConnectComponent();
    }
 
    void
    CartesianWaypointControlGuiWidgetController::createController()
    {
        std::lock_guard g{_allMutex};
        NJointControllerGuiPluginBase::createController();
        _supportsFT = !_ui.lineEditFTName->text().toStdString().empty();
    }
 
    void
    CartesianWaypointControlGuiWidgetController::on_pushButtonSendSettings_clicked()
    {
        std::lock_guard g{_allMutex};
        if (_controller)
        {
            ARMARX_IMPORTANT << "sending new config to " << getControllerName();
            _controller->setConfig(readRunCfg());
        }
    }
 
    void
    CartesianWaypointControlGuiWidgetController::copyCurrentPose()
    {
        std::lock_guard g{_allMutex};
        if (!_robot)
        {
            return;
        }
        synchronizeLocalClone(_robot);
        const auto rns = _robot->getRobotNodeSet(_ui.comboBoxChain->currentText().toStdString());
        if (!rns)
        {
            return;
        }
        const auto tcp = rns->getTCP();
        if (!tcp)
        {
            return;
        }
        const auto str = simox::json::eigen4f2posquatJson(tcp->getPoseInRootFrame());
        QApplication::clipboard()->setText(QString::fromStdString(str));
    }
 
    void
    CartesianWaypointControlGuiWidgetController::on_pushButtonZeroFT_clicked()
    {
        std::lock_guard g{_allMutex};
        if (_controller && _supportsFT)
        {
            ARMARX_IMPORTANT << "setting ft offset for " << getControllerName();
            _controller->setCurrentFTAsOffset();
        }
    }
 
    void
    CartesianWaypointControlGuiWidgetController::on_pushButtonExecute_clicked()
    {
        std::lock_guard g{_allMutex};
        if (_controller)
        {
            _controller->activateController();
            ARMARX_IMPORTANT << "trigger execution of " << _lastParsedWPs.size() << " waypoints on "
                             << getControllerName();
            _controller->setWaypoints(_ui.checkBoxWPReverse->isChecked() ? reverse(_lastParsedWPs)
                                                                         : _lastParsedWPs);
        }
    }
 
    void
    CartesianWaypointControlGuiWidgetController::triggerParsing()
    {
        std::lock_guard g{_allMutex};
        _lastParsedWPs.clear();
        _ui.labelParsingSuccess->setText("<pre parsing>");
        if (_ui.radioButtonWPJson->isChecked())
        {
            //parse json
            try
            {
                _lastParsedWPs = simox::json::posquatArray2eigen4fVector(
                    _ui.textEditWPs->toPlainText().toStdString());
            }
            catch (...)
            {
                _ui.labelParsingSuccess->setText("Failed to parse json array!");
                return;
            }
        }
        else if (_ui.radioButton4f->isChecked())
        {
            //parse lines
            ///TODO parse Matrix4f style input
            _ui.labelParsingSuccess->setText("NYI");
            return;
        }
        //test reachability
        {
            ///TODO test reachability and visualize
        }
        _ui.labelParsingSuccess->setText("Parsed " + QString::number(_lastParsedWPs.size()) +
                                         " waypoints.");
    }
 
    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();
        cfg.skipToClosestWaypoint = _ui.checkBoxSkipWaypoints->isChecked();
 
        return cfg;
    }
 
    void
    CartesianWaypointControlGuiWidgetController::timerEvent(QTimerEvent* e)
    {
        std::lock_guard g{_allMutex};
        if (_robot)
        {
            synchronizeLocalClone(_robot);
        }
        if (_controller)
        {
            ARMARX_INFO << deactivateSpam() << "setting visu gp to:\n" << _robot->getGlobalPose();
            _controller->setVisualizationRobotGlobalPose(_robot->getGlobalPose());
        }
    }
 
    void
    CartesianWaypointControlGuiWidgetController::setupGuiAfterConnect()
    {
        bool found = false;
        for (const auto& rnsn : _robot->getRobotNodeSetNames())
        {
            _ui.comboBoxChain->addItem(QString::fromStdString(rnsn));
            if (rnsn == "RightArm")
            {
                _ui.comboBoxChain->setCurrentIndex(_ui.comboBoxChain->count() - 1);
                found = true;
            }
        }
        if (found && _robot->hasRobotNode("FT R"))
        {
            _ui.lineEditFTName->setText("FT R");
        }
    }
 
    NJointControllerConfigPtr
    CartesianWaypointControlGuiWidgetController::readFullCFG() const
    {
        NJointCartesianWaypointControllerConfigPtr cfg =
            new NJointCartesianWaypointControllerConfig;
        cfg->rns = _ui.comboBoxChain->currentText().toStdString();
        cfg->ftName = _ui.lineEditFTName->text().toStdString();
        cfg->runCfg = readRunCfg();
        return NJointControllerConfigPtr::dynamicCast(cfg);
    }
} // namespace armarx