CartesianImpedanceControllerConfigWidget.cpp

Go to the documentation of this file.

#include "CartesianImpedanceControllerConfigWidget.h"
 
#include <VirtualRobot/RobotNodeSet.h>
 
namespace armarx
{
    void
    clearLayout(QLayout* layout)
    {
        QLayoutItem* item;
        while ((item = layout->takeAt(0)))
        {
            if (item->layout())
            {
                clearLayout(item->layout());
                delete item->layout();
            }
            if (item->widget())
            {
                delete item->widget();
            }
            delete item;
        }
    }
 
    CartesianImpedanceControllerConfigWidget::CartesianImpedanceControllerConfigWidget(
        QWidget* parent) :
        QWidget(parent)
    {
        ui.setupUi(this);
 
        kxyz.addWidget(ui.doubleSpinBoxKTX);
        kxyz.addWidget(ui.doubleSpinBoxKTY);
        kxyz.addWidget(ui.doubleSpinBoxKTZ);
        kxyz.setMinMax(0, 5000);
        kxyz.set(1000);
 
        krpy.addWidget(ui.doubleSpinBoxKRX);
        krpy.addWidget(ui.doubleSpinBoxKRY);
        krpy.addWidget(ui.doubleSpinBoxKRZ);
        krpy.setMinMax(0, 5000);
        krpy.set(500);
 
        dxyz.addWidget(ui.doubleSpinBoxDTX);
        dxyz.addWidget(ui.doubleSpinBoxDTY);
        dxyz.addWidget(ui.doubleSpinBoxDTZ);
        dxyz.setMinMax(0, 5000);
        dxyz.set(250);
 
        drpy.addWidget(ui.doubleSpinBoxDRX);
        drpy.addWidget(ui.doubleSpinBoxDRY);
        drpy.addWidget(ui.doubleSpinBoxDRZ);
        drpy.setMinMax(0, 5000);
        drpy.set(100);
 
        using T = CartesianImpedanceControllerConfigWidget;
        connect(ui.pushButtonNullspaceUpdateJoints,
                &QPushButton::clicked,
                this,
                &T::on_pushButtonNullspaceUpdateJoints_clicked);
        connect(ui.pushButtonNullspaceSend,
                &QPushButton::clicked,
                this,
                &T::on_pushButtonNullspaceSend_clicked);
        connect(ui.pushButtonSettingsSend,
                &QPushButton::clicked,
                this,
                &T::on_pushButtonSettingsSend_clicked);
    }
 
    void
    CartesianImpedanceControllerConfigWidget::loadSettings(QSettings* settings,
                                                           const QString& prefix)
    {
        if (prefix.size())
        {
            settings->beginGroup(prefix);
        }
        ///TODO
        if (prefix.size())
        {
            settings->endGroup();
        }
    }
 
    void
    CartesianImpedanceControllerConfigWidget::saveSettings(QSettings* settings,
                                                           const QString& prefix)
    {
        if (prefix.size())
        {
            settings->beginGroup("layer");
        }
        ///TODO
        if (prefix.size())
        {
            settings->endGroup();
        }
    }
 
    NJointTaskSpaceImpedanceControlConfigPtr
    CartesianImpedanceControllerConfigWidget::readFullCFG(const Eigen::Vector3f& targPos,
                                                          const Eigen::Quaternionf& targOri) const
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(_rns);
        NJointTaskSpaceImpedanceControlConfigPtr cfg = new NJointTaskSpaceImpedanceControlConfig;
        cfg->nodeSetName = _rns->getName();
 
        cfg->desiredPosition = targPos;
        cfg->desiredOrientation = targOri;
 
        kxyz.get(cfg->Kpos);
        krpy.get(cfg->Kori);
        dxyz.get(cfg->Dpos);
        drpy.get(cfg->Dori);
 
        auto [joint, knull, dnull] = readNullspaceCFG();
        cfg->desiredJointPositions = joint;
        cfg->Knull = knull;
        cfg->Dnull = dnull;
 
        cfg->torqueLimit = ui.doubleSpinBoxTorqueLim->value();
        return cfg;
    }
 
    NJointTaskSpaceImpedanceControlRuntimeConfig
    CartesianImpedanceControllerConfigWidget::readRuntimeCFG() const
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(_rns);
        NJointTaskSpaceImpedanceControlRuntimeConfig cfg;
 
        kxyz.get(cfg.Kpos);
        krpy.get(cfg.Kori);
        dxyz.get(cfg.Dpos);
        drpy.get(cfg.Dori);
 
        auto [joint, knull, dnull] = readNullspaceCFG();
        cfg.desiredJointPositions = joint;
        cfg.Knull = knull;
        cfg.Dnull = dnull;
 
        cfg.torqueLimit = ui.doubleSpinBoxTorqueLim->value();
        return cfg;
    }
 
    std::tuple<Eigen::VectorXf, Eigen::VectorXf, Eigen::VectorXf>
    CartesianImpedanceControllerConfigWidget::readNullspaceCFG() const
    {
        ARMARX_TRACE;
        return {jointValues.get<Eigen::VectorXf>(),
                jointKnull.get<Eigen::VectorXf>(),
                jointDnull.get<Eigen::VectorXf>()};
    }
 
    void
    CartesianImpedanceControllerConfigWidget::on_pushButtonNullspaceSend_clicked()
    {
        ARMARX_TRACE;
        if (!_controller)
        {
            return;
        }
        auto [joint, knull, dnull] = readNullspaceCFG();
        _controller->setNullspaceConfig(joint, knull, dnull);
    }
 
    void
    CartesianImpedanceControllerConfigWidget::on_pushButtonNullspaceUpdateJoints_clicked()
    {
        ARMARX_TRACE;
        if (!_rns)
        {
            return;
        }
        jointValues.set(_rns);
    }
 
    void
    CartesianImpedanceControllerConfigWidget::on_pushButtonSettingsSend_clicked()
    {
        ARMARX_TRACE;
        if (!_controller)
        {
            return;
        }
        _controller->setConfig(readRuntimeCFG());
    }
 
    void
    CartesianImpedanceControllerConfigWidget::setController(
        const NJointTaskSpaceImpedanceControlInterfacePrx& prx)
    {
        _controller = prx;
    }
 
    void
    CartesianImpedanceControllerConfigWidget::setRNS(const VirtualRobot::RobotNodeSetPtr& rns)
    {
        ARMARX_TRACE;
        _rns = rns;
        auto lay = ui.gridLayoutNullspace;
        clearLayout(lay);
        lay->addWidget(new QLabel{"Joint"}, 0, 0);
        lay->addWidget(new QLabel{"Target"}, 0, 1);
        lay->addWidget(new QLabel{"Knull"}, 0, 2);
        lay->addWidget(new QLabel{"Dnull"}, 0, 3);
        jointValues.clear();
        jointKnull.clear();
        jointDnull.clear();
 
        if (!_rns)
        {
            return;
        }
 
        int i = 1;
        for (const auto& rn : _rns->getAllRobotNodes())
        {
            ARMARX_TRACE;
            const auto&& n = rn->getName();
            lay->addWidget(new QLabel{QString::fromStdString(n)}, i, 0);
            {
                auto b = new QDoubleSpinBox;
                jointValues.addWidget(b);
                lay->addWidget(b, i, 1);
                const auto lo = rn->getJointLimitLow();
                const auto hi = rn->getJointLimitHigh();
                b->setMinimum(lo);
                b->setMaximum(hi);
                b->setValue((lo + hi) / 2);
            }
            {
                auto b = new QDoubleSpinBox;
                jointKnull.addWidget(b);
                lay->addWidget(b, i, 2);
                b->setMinimum(0);
                b->setMaximum(1000);
                b->setValue(2);
            }
            {
                auto b = new QDoubleSpinBox;
                jointDnull.addWidget(b);
                lay->addWidget(b, i, 3);
                b->setMinimum(0);
                b->setMaximum(1000);
                b->setValue(1);
            }
            ++i;
        }
        jointKnull.set(2);
        jointDnull.set(1);
 
        on_pushButtonNullspaceUpdateJoints_clicked();
    }
} // namespace armarx