CartesianImpedanceControllerWidgetController.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::CartesianImpedanceControllerWidgetController
 * \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
 */
 
#include "CartesianImpedanceControllerWidgetController.h"
 
#include <string>
 
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <SimoxUtility/math/convert/rpy_to_quat.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
namespace armarx
{
    CartesianImpedanceControllerWidgetController::CartesianImpedanceControllerWidgetController() :
        NJointControllerGuiPluginBase("NJointTaskSpaceImpedanceController")
    {
        _ui.setupUi(getWidget());
        connectCreateAcivateDeactivateDelete(_ui.pushButtonCreate,
                                             _ui.pushButtonActivate,
                                             _ui.pushButtonDeactivate,
                                             _ui.pushButtonDelete);
        _settings = new CartesianImpedanceControllerConfigWidget(getWidget());
        _ui.gridLayoutSettings->addWidget(_settings);
 
        _targ.setXYZWidgets(
            _ui.doubleSpinBoxTargTX, _ui.doubleSpinBoxTargTY, _ui.doubleSpinBoxTargTZ);
 
        _targ.setRPYWidgets(
            _ui.doubleSpinBoxTargRX, _ui.doubleSpinBoxTargRY, _ui.doubleSpinBoxTargRZ);
 
        _targ.setDefaultLimits();
        _timer = startTimer(10);
    }
 
    CartesianImpedanceControllerWidgetController::~CartesianImpedanceControllerWidgetController()
    {
        if (_timer)
        {
            killTimer(_timer);
        }
    }
 
    void
    CartesianImpedanceControllerWidgetController::setupGuiAfterConnect()
    {
        ARMARX_TRACE;
        std::lock_guard g{_allMutex};
        ARMARX_CHECK_NOT_NULL(_robot);
        ARMARX_INFO << "setup comboBoxRNS";
        for (const auto& rnsName : _robot->getRobotNodeSetNames())
        {
            _ui.comboBoxRNS->addItem(QString::fromStdString(rnsName));
        }
        ARMARX_TRACE;
        for (const auto& nodeName : _robot->getRobotNodeNames())
        {
            _ui.comboBoxLockY->addItem(QString::fromStdString(nodeName));
            _ui.comboBoxLockZ->addItem(QString::fromStdString(nodeName));
        }
 
        using T = CartesianImpedanceControllerWidgetController;
        connect(
            _ui.pushButtonTargGet, &QPushButton::clicked, this, &T::on_pushButtonTargGet_clicked);
        connect(
            _ui.pushButtonTargAdd, &QPushButton::clicked, this, &T::on_pushButtonTargAdd_clicked);
        connect(
            _ui.pushButtonTargSet, &QPushButton::clicked, this, &T::on_pushButtonTargSet_clicked);
        connect(_ui.comboBoxRNS,
                SIGNAL(currentIndexChanged(const QString&)),
                this,
                SLOT(on_comboBoxRNS_currentIndexChanged(const QString&)));
        connect(_ui.radioButtonSelectLeft,
                &QRadioButton::clicked,
                this,
                &T::on_radioButtonSelect_clicked);
        connect(_ui.radioButtonSelectRight,
                &QRadioButton::clicked,
                this,
                &T::on_radioButtonSelect_clicked);
        connect(_ui.radioButtonSelectRNS,
                &QRadioButton::clicked,
                this,
                &T::on_radioButtonSelect_clicked);
 
        _ui.groupBoxAdvanced->setChecked(false);
        _ui.radioButtonSelectLeft->setChecked(true);
    }
 
    void
    CartesianImpedanceControllerWidgetController::onConnectComponent()
    {
        ARMARX_TRACE;
        std::lock_guard g{_allMutex};
        _left = getRobotNameHelper().getArm("Left");
        _right = getRobotNameHelper().getArm("Right");
    }
 
    void
    CartesianImpedanceControllerWidgetController::timerEvent(QTimerEvent*)
    {
        ARMARX_TRACE;
        std::lock_guard g{_allMutex};
        if (_robot)
        {
            synchronizeLocalClone(_robot);
        }
        if (_ui.checkBoxAutoUpdatePose->isChecked())
        {
            ARMARX_TRACE;
            on_pushButtonTargSet_clicked();
        }
        const bool lockY = _ui.checkBoxLockY->isChecked();
        const bool lockZ = _ui.checkBoxLockZ->isChecked();
        bool anyTracking = lockY || lockZ;
        _ui.widgetTarget->setEnabled(!anyTracking);
        if (_rns && (lockY || lockZ))
        {
            ARMARX_TRACE;
            Eigen::Matrix4f pose = _rns->getTCP()->getPoseInRootFrame();
            if (lockY)
            {
                pose(1, 3) = _robot->getRobotNode(_ui.comboBoxLockY->currentText().toStdString())
                                 ->getPositionInRootFrame()(1);
            }
            if (lockZ)
            {
                pose(2, 3) = _robot->getRobotNode(_ui.comboBoxLockZ->currentText().toStdString())
                                 ->getPositionInRootFrame()(2);
            }
            if (_controller)
            {
                _controller->setPosition(pose.topRightCorner<3, 1>());
            }
            visuHandAtPose(pose);
            ARMARX_INFO << "set Target pose to\n" << pose;
        }
        else if (!(lockY || lockZ))
        {
            const Eigen::Matrix4f rootTcpTarg =
                _ui.radioButtonVisuSet ? _targ.getMat4()
                                       : _rns->getTCP()->getPoseInRootFrame() * _targ.getMat4();
            visuHandAtPose(rootTcpTarg);
        }
    }
 
    void
    CartesianImpedanceControllerWidgetController::visuHandAtPose(const Eigen::Matrix4f& tcpInRoot)
    {
        ARMARX_TRACE;
        std::lock_guard g{_allMutex};
        if (_visuHandRobotFile.empty() || !_robot || !arviz.topic())
        {
            if (arviz.topic())
            {
                arviz.commitLayerContaining("hand");
            }
            return;
        }
 
        arviz.commitLayerContaining("hand",
                                    viz::Robot{"Hand"}
                                        .file(_visuHandRobotProject, _visuHandRobotFile)
                                        .useFullModel()
                                        .pose(_robot->getGlobalPose() * tcpInRoot * _tcpToBase)
                                        .overrideColor(viz::Color::green()));
    }
} // namespace armarx
 
//read config
namespace armarx
{
    NJointControllerConfigPtr
    CartesianImpedanceControllerWidgetController::readFullCFG() const
    {
        ARMARX_TRACE;
        auto [pos, quat] = readTargetCFG();
        return NJointControllerConfigPtr::dynamicCast(_settings->readFullCFG(pos, quat));
    }
 
    std::tuple<Eigen::Vector3f, Eigen::Quaternionf>
    CartesianImpedanceControllerWidgetController::readTargetCFG() const
    {
        ARMARX_TRACE;
        return {_targ.getPos(), _targ.getQuat()};
    }
} // namespace armarx
 
//push buttons
namespace armarx
{
    void
    CartesianImpedanceControllerWidgetController::createController()
    {
        ARMARX_TRACE;
        NJointControllerGuiPluginBase::createController();
        _ui.widgetRNSSelect->setEnabled(false);
        _settings->setController(_controller);
    }
 
    void
    CartesianImpedanceControllerWidgetController::deleteController()
    {
        ARMARX_TRACE;
        NJointControllerGuiPluginBase::deleteController();
        _ui.widgetRNSSelect->setEnabled(true);
        _settings->setController(nullptr);
    }
 
    void
    CartesianImpedanceControllerWidgetController::on_pushButtonTargSet_clicked()
    {
        ARMARX_TRACE;
        std::lock_guard g{_allMutex};
        if (!_controller)
        {
            return;
        }
        const Eigen::Matrix4f pose = _targ.getMat4();
        _controller->setPose(pose);
        ARMARX_INFO << "set Target pose to\n" << pose;
    }
 
    void
    CartesianImpedanceControllerWidgetController::on_pushButtonTargAdd_clicked()
    {
        ARMARX_TRACE;
        std::lock_guard g{_allMutex};
        if (!_controller || !_rns)
        {
            return;
        }
        const Eigen::Matrix4f pose = _rns->getTCP()->getPoseInRootFrame() * _targ.getMat4();
        _controller->setPose(pose);
        ARMARX_INFO << "set Target pose to\n" << pose;
    }
 
    void
    CartesianImpedanceControllerWidgetController::on_pushButtonTargGet_clicked()
    {
        ARMARX_TRACE;
        std::lock_guard g{_allMutex};
        if (!_rns)
        {
            return;
        }
        const Eigen::Matrix4f pose = _rns->getTCP()->getPoseInRootFrame();
        const Eigen::Vector3f pos = simox::math::mat4f_to_pos(pose);
        const Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(pose);
 
        _ui.doubleSpinBoxTargTX->setValue(pos(0));
        _ui.doubleSpinBoxTargTY->setValue(pos(1));
        _ui.doubleSpinBoxTargTZ->setValue(pos(2));
 
        _ui.doubleSpinBoxTargRX->setValue(rpy(0));
        _ui.doubleSpinBoxTargRY->setValue(rpy(1));
        _ui.doubleSpinBoxTargRZ->setValue(rpy(2));
    }
 
    void
    CartesianImpedanceControllerWidgetController::on_comboBoxRNS_currentIndexChanged(
        const QString& arg1)
    {
        ARMARX_TRACE;
        std::lock_guard g{_allMutex};
        if (!_robot || _controller)
        {
            return;
        }
        const std::string rnsName = arg1.toStdString();
        _rns = _robot->getRobotNodeSet(rnsName);
        _settings->setRNS(_rns);
        auto checkSide = [&](const RobotNameHelper::Arm& side)
        {
            if (!_rns->getTCP() || side.getTCP() != _rns->getTCP()->getName() ||
                !side.getRobotNameHelper())
            {
                return;
            }
            const auto arm = side.addRobot(_robot);
            _tcpToBase = arm.getTcp2HandRootTransform();
            _visuHandRobotFile = side.getHandModelPath();
            _visuHandRobotProject = side.getHandModelPackage();
        };
        if (_left.getRobotNameHelper())
        {
            checkSide(_left);
        }
        if (_right.getRobotNameHelper())
        {
            checkSide(_right);
        }
 
        on_pushButtonTargGet_clicked();
    }
 
    void
    CartesianImpedanceControllerWidgetController::on_radioButtonSelect_clicked()
    {
        ARMARX_TRACE;
        std::lock_guard g{_allMutex};
        if (_ui.radioButtonSelectLeft->isChecked())
        {
            on_comboBoxRNS_currentIndexChanged(QString::fromStdString(_left.getKinematicChain()));
        }
        else if (_ui.radioButtonSelectRight->isChecked())
        {
            on_comboBoxRNS_currentIndexChanged(QString::fromStdString(_right.getKinematicChain()));
        }
        else
        {
            on_comboBoxRNS_currentIndexChanged(_ui.comboBoxRNS->currentText());
        }
    }
} // namespace armarx