CartesianNaturalPositionControllerWidgetController.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::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 "CartesianNaturalPositionControllerWidgetController.h"
 
#include <string>
 
#include <VirtualRobot/math/Helpers.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
namespace armarx
{
    CartesianNaturalPositionControllerWidgetController::
        CartesianNaturalPositionControllerWidgetController()
    {
        std::lock_guard g{_allMutex};
        _ui.setupUi(getWidget());
 
        connect(this,
                SIGNAL(invokeConnectComponentQt()),
                this,
                SLOT(onConnectComponentQt()),
                Qt::QueuedConnection);
        connect(this,
                SIGNAL(invokeDisconnectComponentQt()),
                this,
                SLOT(onDisconnectComponentQt()),
                Qt::QueuedConnection);
 
 
        //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()));
        connect(
            _ui.sliderKpElbow, SIGNAL(valueChanged(int)), this, SLOT(on_sliders_valueChanged(int)));
        connect(
            _ui.sliderKpJla, SIGNAL(valueChanged(int)), this, SLOT(on_sliders_valueChanged(int)));
        connect(_ui.checkBoxAutoKp,
                SIGNAL(stateChanged(int)),
                this,
                SLOT(on_checkBoxAutoKp_stateChanged(int)));
        connect(_ui.checkBoxSetOri,
                SIGNAL(stateChanged(int)),
                this,
                SLOT(on_checkBoxSetOri_stateChanged(int)));
        connect(_ui.sliderPosVel,
                SIGNAL(valueChanged(int)),
                this,
                SLOT(on_horizontalSliderPosVel_valueChanged(int)));
 
        auto addBtn =
            [&](QPushButton* btn, float px, float py, float pz, float rx, float ry, float rz)
        {
            _deltaMapPos[btn] = {px, py, pz};
            _deltaMapOri[btn] = {rx, ry, rz};
            //ARMARX_IMPORTANT << "connect";
            //ARMARX_IMPORTANT << btn->text().toStdString();
            connect(btn, SIGNAL(clicked()), this, SLOT(on_anyDeltaPushButton_clicked()));
        };
        addBtn(_ui.pushButtonPxp, +1, 0, 0, 0, 0, 0);
        addBtn(_ui.pushButtonPxn, -1, 0, 0, 0, 0, 0);
        addBtn(_ui.pushButtonPyp, 0, +1, 0, 0, 0, 0);
        addBtn(_ui.pushButtonPyn, 0, -1, 0, 0, 0, 0);
        addBtn(_ui.pushButtonPzp, 0, 0, +1, 0, 0, 0);
        addBtn(_ui.pushButtonPzn, 0, 0, -1, 0, 0, 0);
 
        addBtn(_ui.pushButtonRxp, 0, 0, 0, +1, 0, 0);
        addBtn(_ui.pushButtonRxn, 0, 0, 0, -1, 0, 0);
        addBtn(_ui.pushButtonRyp, 0, 0, 0, 0, +1, 0);
        addBtn(_ui.pushButtonRyn, 0, 0, 0, 0, -1, 0);
        addBtn(_ui.pushButtonRzp, 0, 0, 0, 0, 0, +1);
        addBtn(_ui.pushButtonRzn, 0, 0, 0, 0, 0, -1);
 
        //_ui.widgetSpacer->setVisible(false);
        _timer = startTimer(100);
    }
 
    CartesianNaturalPositionControllerWidgetController::
        ~CartesianNaturalPositionControllerWidgetController()
    {
        killTimer(_timer);
    }
 
    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);
        }
 
        _controller.reset();
        invokeConnectComponentQt();
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::onConnectComponentQt()
    {
        _ui.comboBoxSide->addItem("Right");
        _ui.comboBoxSide->addItem("Left");
        _ui.comboBoxSide->setCurrentIndex(0);
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::onDisconnectComponent()
    {
        std::lock_guard g{_allMutex};
        deleteOldController();
        _robotStateComponent = nullptr;
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::on_pushButtonCreateController_clicked()
    {
        std::lock_guard g{_allMutex};
        deleteOldController();
 
        std::string side = _ui.comboBoxSide->currentText().toStdString();
 
        VirtualRobot::RobotNodeSetPtr rns = _robot->getRobotNodeSet(side + "Arm");
 
        _ui.gridLayoutNullspaceTargets->removeWidget(_ui.checkBoxExampleJoint1);
        _ui.gridLayoutNullspaceTargets->removeWidget(_ui.labelExampleJoint1);
        _ui.gridLayoutNullspaceTargets->removeWidget(_ui.horizontalSliderExampleJoint1);
 
        for (size_t i = 0; i < rns->getSize(); i++)
        {
            QCheckBox* checkBox = new QCheckBox(QString::fromStdString(rns->getNode(i)->getName()));
            QSlider* slider = new QSlider(Qt::Orientation::Horizontal);
            slider->setMinimum(rns->getNode(i)->getJointLimitLo() / M_PI * 180);
            slider->setMaximum(rns->getNode(i)->getJointLimitHi() / M_PI * 180);
            slider->setValue(rns->getNode(i)->getJointValue() / M_PI * 180);
            QLabel* label = new QLabel(QString::number(slider->value()));
            _ui.gridLayoutNullspaceTargets->addWidget(checkBox, i, 0);
            _ui.gridLayoutNullspaceTargets->addWidget(slider, i, 1);
            _ui.gridLayoutNullspaceTargets->addWidget(label, i, 2);
            connect(checkBox,
                    SIGNAL(stateChanged(int)),
                    this,
                    SLOT(on_anyNullspaceCheckbox_stateChanged(int)));
            connect(slider,
                    SIGNAL(valueChanged(int)),
                    this,
                    SLOT(on_anyNullspaceSlider_valueChanged(int)));
            NullspaceTarget nt;
            nt.checkBox = checkBox;
            nt.slider = slider;
            nt.label = label;
            nt.i = i;
            _nullspaceTargets.emplace_back(nt);
        }
 
 
        VirtualRobot::RobotNodePtr cla = rns->getNode(0);
        VirtualRobot::RobotNodePtr sho1 = rns->getNode(1);
        ARMARX_IMPORTANT << VAROUT(cla->getJointValue());
        cla->setJointValue(0);
        Eigen::Vector3f shoulder = sho1->getPositionInRootFrame();
        VirtualRobot::RobotNodePtr elb = rns->getNode(4);
        VirtualRobot::RobotNodePtr wri1 = rns->getNode(6);
        VirtualRobot::RobotNodePtr tcp = rns->getTCP();
        NaturalIK::ArmJoints arm;
 
        arm.rns = rns;
        arm.shoulder = sho1;
        arm.elbow = elb;
        arm.tcp = tcp;
 
        std::vector<float> jointCosts = std::vector<float>(rns->getSize(), 1);
        jointCosts.at(0) = 4;
 
 
        armarx::RemoteRobot::synchronizeLocalClone(_robot, _robotStateComponent);
        _tcpTarget = rns->getTCP()->getPoseInRootFrame();
 
        NaturalIK ik(side, shoulder, 1.3f);
        float upper_arm_length =
            (sho1->getPositionInRootFrame() - elb->getPositionInRootFrame()).norm();
        float lower_arm_length =
            (elb->getPositionInRootFrame() - wri1->getPositionInRootFrame()).norm() +
            (wri1->getPositionInRootFrame() - tcp->getPositionInRootFrame()).norm();
        ik.setUpperArmLength(upper_arm_length);
        ik.setLowerArmLength(lower_arm_length);
        NJointCartesianNaturalPositionControllerConfigPtr config =
            CartesianNaturalPositionControllerProxy::MakeConfig(rns->getName(), elb->getName());
        config->runCfg.jointCosts = jointCosts;
        CartesianNaturalPositionControllerConfig runCfg = config->runCfg;
        updateKpSliders(runCfg);
        //config->runCfg = runCfg;
        _controller.reset(new CartesianNaturalPositionControllerProxy(
            ik, arm, _robotUnit, side + "NaturalPosition", config));
        _controller->init();
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::on_anyDeltaPushButton_clicked()
    {
        Eigen::Matrix4f newTarget = _tcpTarget;
        ARMARX_IMPORTANT << "on_anyDeltaPushButton_clicked";
        std::lock_guard g{_allMutex};
        Eigen::Vector3f deltaPos = _deltaMapPos.at(QObject::sender());
        Eigen::Vector3f deltaOri = _deltaMapOri.at(QObject::sender());
        newTarget = math::Helpers::TranslatePose(newTarget, deltaPos * _ui.spinBoxDpos->value());
        if (deltaOri.norm() > 0)
        {
            deltaOri = deltaOri * _ui.spinBoxDori->value() / 180 * M_PI;
            Eigen::AngleAxisf aa(deltaOri.norm(), deltaOri.normalized());
            math::Helpers::Orientation(newTarget) =
                aa.toRotationMatrix() * math::Helpers::Orientation(newTarget);
        }
        updateTarget(newTarget);
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::updateTarget(
        const Eigen::Matrix4f& newTarget)
    {
        if (!_controller->setTarget(
                newTarget, _setOri ? NaturalDiffIK::Mode::All : NaturalDiffIK::Mode::Position))
        {
            return;
        }
        _tcpTarget = newTarget;
        if (_controller->getDynamicKp().enabled)
        {
            updateKpSliders(_controller->getRuntimeConfig());
        }
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::readRunCfgFromUi(
        CartesianNaturalPositionControllerConfig& runCfg)
    {
        runCfg.elbowKp = _ui.sliderKpElbow->value() * 0.01f;
        runCfg.jointLimitAvoidanceKp = _ui.sliderKpJla->value() * 0.01f;
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::updateKpSliderLabels(
        const CartesianNaturalPositionControllerConfig& runCfg)
    {
        _ui.labelKpElbow->setText(QString::number(runCfg.elbowKp, 'f', 2));
        _ui.labelKpJla->setText(QString::number(runCfg.jointLimitAvoidanceKp, 'f', 2));
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::updateKpSliders(
        const CartesianNaturalPositionControllerConfig& runCfg)
    {
        const QSignalBlocker blockKpElb(_ui.sliderKpElbow);
        const QSignalBlocker blockKpJla(_ui.sliderKpJla);
        _ui.sliderKpElbow->setValue(runCfg.elbowKp * 100);
        _ui.sliderKpJla->setValue(runCfg.jointLimitAvoidanceKp * 100);
        updateKpSliderLabels(runCfg);
        ARMARX_IMPORTANT << VAROUT(runCfg.elbowKp) << VAROUT(runCfg.jointLimitAvoidanceKp);
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::on_sliders_valueChanged(int)
    {
        CartesianNaturalPositionControllerConfig runCfg = _controller->getRuntimeConfig();
        //ARMARX_IMPORTANT << VAROUT(_runCfg.elbowKp) << VAROUT(_runCfg.jointLimitAvoidanceKp);
        readRunCfgFromUi(runCfg);
        updateKpSliderLabels(runCfg);
        _controller->setRuntimeConfig(runCfg);
        _controller->updateBaseKpValues(runCfg);
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::on_checkBoxAutoKp_stateChanged(int)
    {
        CartesianNaturalPositionControllerProxy::DynamicKp dynamicKp;
        dynamicKp.enabled = _ui.checkBoxAutoKp->isChecked();
        _controller->setDynamicKp(dynamicKp);
        if (_controller->getDynamicKp().enabled)
        {
            updateKpSliders(_controller->getRuntimeConfig());
        }
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::on_checkBoxSetOri_stateChanged(int)
    {
        _setOri = _ui.checkBoxSetOri->isChecked();
        updateTarget(_tcpTarget);
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::updateNullspaceTargets()
    {
        std::vector<float> nsTargets;
        for (const NullspaceTarget& nt : _nullspaceTargets)
        {
            nsTargets.push_back(nt.checkBox->isChecked() ? nt.slider->value() * M_PI / 180
                                                         : std::nanf(""));
            nt.label->setText(QString::number(nt.slider->value()));
        }
        _controller->setNullspaceTarget(nsTargets);
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::on_anyNullspaceCheckbox_stateChanged(int)
    {
        updateNullspaceTargets();
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::on_anyNullspaceSlider_valueChanged(int)
    {
        updateNullspaceTargets();
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::on_horizontalSliderPosVel_valueChanged(int)
    {
        _ui.labelPosVel->setText(QString::number(_ui.sliderPosVel->value()));
        float posVel = _ui.sliderPosVel->value();
        _controller->setMaxVelocities(posVel);
        //_runCfg = _controller->getRuntimeConfig();
    }
 
    //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};
        if (_controller)
        {
            _controller->cleanup();
        }
    }
 
    void
    CartesianNaturalPositionControllerWidgetController::timerEvent(QTimerEvent*)
    {
        std::lock_guard g{_allMutex};
        if (!_robot || !_robotStateComponent)
        {
            return;
        }
        RemoteRobot::synchronizeLocalClone(_robot, _robotStateComponent);
        if (_controller)
        {
            ARMARX_INFO << deactivateSpam() << "setting visu gp to:\n" << _robot->getGlobalPose();
            _controller->getInternalController()->setVisualizationRobotGlobalPose(
                _robot->getGlobalPose());
        }
    }
 
 
} // namespace armarx