VertexDataWidget.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    MemoryX::ArmarXObjects::GraphImportExport
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "VertexDataWidget.h"
 
#include <functional>
#include <optional>
#include <string>
#include <vector>
 
#include <QComboBox>
#include <QDoubleSpinBox>
#include <QFormLayout>
#include <QHBoxLayout>
#include <QLineEdit>
#include <QList>
#include <QPushButton>
#include <QRadioButton>
#include <QSignalBlocker>
#include <qboxlayout.h>
#include <qformlayout.h>
#include <qglobal.h>
#include <qnamespace.h>
#include <qobject.h>
#include <qobjectdefs.h>
#include <qsizepolicy.h>
 
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <SimoxUtility/math/convert/rad_to_deg.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <SimoxUtility/math/pose/pose.h>
#include <VirtualRobot/XML/BaseIO.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/navigation/core/LocationUtils.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
 
#include "ObjectPoseClientWidget.h"
#include "RobotVisuWidget.h"
 
namespace armarx::navigation::qt_plugins::location_graph_editor
{
 
    VertexDataWidget::VertexDataWidget()
    {
        setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum);
 
        name = new QLineEdit(this);
        name->setReadOnly(true);
        locationID = new QLineEdit(this);
        locationID->setReadOnly(true);
 
        agent = new QComboBox(this);
        agent->setSizeAdjustPolicy(QComboBox::AdjustToContents);
 
        frame = new QComboBox(this);
        frame->setSizeAdjustPolicy(QComboBox::AdjustToContents);
 
        removeInstanceButton = new QPushButton("Remove Instance");
        removeInstanceButton->setToolTip(
            "Remove the instance id from the agent and make this location relative to an object "
            "class instead of a specific object instance.");
 
        x = new QDoubleSpinBox(this);
        y = new QDoubleSpinBox(this);
        z = new QDoubleSpinBox(this);
 
        roll = new QDoubleSpinBox(this);
        pitch = new QDoubleSpinBox(this);
        yaw = new QDoubleSpinBox(this);
 
        angleUnitDeg = new QRadioButton("Degrees");
        angleUnitRad = new QRadioButton("Radians");
 
        _useCurrentRobotPose = new QPushButton("Use Current Robot Pose");
        _useCurrentRobotPose->setEnabled(false);
        _useCurrentRobotPose->setToolTip(
            "Use the robot's current global pose (requires connection to robot state).");
 
 
        for (QDoubleSpinBox* pos : _positionSpinBoxes())
        {
            pos->setSuffix(" mm");
            pos->setMinimum(-1e6);
            pos->setMaximum(+1e6);
            pos->setSingleStep(50);
        }
        for (QDoubleSpinBox* spinBox : _allSpinBoxes())
        {
            spinBox->setValue(0);
            spinBox->setAlignment(Qt::AlignRight);
        }
 
        QHBoxLayout* angleUnitLayout = new QHBoxLayout();
        for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
        {
            angleUnitLayout->addWidget(angleUnit);
        }
 
        QFormLayout* layout = new QFormLayout();
        this->setLayout(layout);
 
        layout->addRow("Name", name);
        layout->addRow("Entity ID", locationID);
        layout->addRow("Agent", agent);
        layout->addRow("Frame", frame);
        layout->addRow(removeInstanceButton);
        layout->addRow("X", x);
        layout->addRow("Y", y);
        layout->addRow("Z", z);
        layout->addRow("Roll", roll);
        layout->addRow("Pitch", pitch);
        layout->addRow("Yaw", yaw);
        layout->addRow("Angle Units", angleUnitLayout);
        layout->addRow(_useCurrentRobotPose);
 
 
        // Connect
 
        for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
        {
            connect(angleUnit, &QRadioButton::toggled, this, &This::_updateAngleUnit);
        }
 
        connect(agent,
                QOverload<int>::of(&QComboBox::currentIndexChanged),
                this,
                &This::_updateVertexAgent);
        connect(frame,
                QOverload<int>::of(&QComboBox::currentIndexChanged),
                this,
                &This::_updateVertexFrame);
        connect(removeInstanceButton, &QPushButton::pressed, this, &This::_removeInstance);
 
        for (QDoubleSpinBox* spinBox : _allSpinBoxes())
        {
            connect(spinBox,
                    QOverload<qreal>::of(&QDoubleSpinBox::valueChanged),
                    this,
                    &This::_updateVertexAttribs);
        }
 
        connect(_useCurrentRobotPose, &QPushButton::pressed, this, &This::_setFromCurrentRobotPose);
 
 
        angleUnitDeg->click();
    }
 
    std::optional<GuiGraph::Vertex>
    VertexDataWidget::vertex()
    {
        return _vertex;
    }
 
    void
    VertexDataWidget::setVertex(GuiGraph::Vertex vertex)
    {
        QSignalBlocker blocker(this);
 
        _vertex = vertex;
        _setFromVertex(vertex);
        setEnabled(true);
    }
 
    void
    VertexDataWidget::clearVertex()
    {
        _vertex = std::nullopt;
        setEnabled(false);
    }
 
    Eigen::Vector3d
    VertexDataWidget::xyz() const
    {
        return {x->value(), y->value(), z->value()};
    }
 
    Eigen::Vector3d
    VertexDataWidget::rpyDeg() const
    {
        Eigen::Vector3d raw = _rpyRaw();
        if (angleUnitRad->isChecked())
        {
            return {}; // return simox::math::rad_to_deg(raw);
        }
        else
        {
            return raw;
        }
    }
 
    Eigen::Vector3d
    VertexDataWidget::rpyRad() const
    {
        Eigen::Vector3d raw = _rpyRaw();
        if (angleUnitDeg->isChecked())
        {
            return simox::math::deg_to_rad(raw);
        }
        else
        {
            return raw;
        }
    }
 
    core::Pose_d
    VertexDataWidget::pose() const
    {
        return core::Pose_d(simox::math::pose(xyz(), simox::math::rpy_to_mat3f(rpyRad())));
    }
 
    void
    VertexDataWidget::setRobotConnection(robotvisu::Connection* connection)
    {
        this->_robotConnection = connection;
        _useCurrentRobotPose->setEnabled(_robotConnection != nullptr);
    }
 
    void
    VertexDataWidget::setObjectPoseConnection(objectposes::Connection* connection)
    {
        this->_objectPosesConnection = connection;
        if (_vertex.has_value())
        {
            QSignalBlocker blocker(this);
 
            _loadFramedPoseFromVertex(_vertex.value());
        }
    }
 
    void
    VertexDataWidget::_setFromVertex(const GuiGraph::Vertex& vertex)
    {
        const VertexData& attrib = vertex.attrib();
 
        name->setText(QString::fromStdString(attrib.getLocationName()));
        locationID->setText(
            QString::fromStdString(aron::fromAron<armem::MemoryID>(attrib.aron.locationID).str()));
 
        _loadFramedPoseFromVertex(vertex);
 
        _setPose(core::Pose_d(attrib.getPose().toEigen().cast<qreal>()));
    }
 
    void
    VertexDataWidget::_getToVertex(GuiGraph::Vertex& vertex)
    {
        VertexData& attrib = vertex.attrib();
 
        // name is read-only
        // locationID is read-only
 
        FramedPose framed(pose().cast<float>().matrix(),
                          frame->currentText().toStdString(),
                          agent->currentText().toStdString());
        attrib.setPose(framed);
    }
 
    void
    VertexDataWidget::_updateAngleUnit()
    {
        std::function<double(double)> convertValue;
        QString suffix = " \u00b0";
        double min = -360;
        double max = +360;
        double step = 5.;
        int decimals = 2;
 
        if (angleUnitRad->isChecked())
        {
            convertValue = [](double deg) { return simox::math::deg_to_rad(deg); };
            suffix = " rad";
            min = simox::math::deg_to_rad(min);
            max = simox::math::deg_to_rad(max);
            step = simox::math::deg_to_rad(step);
            decimals = 3;
        }
        else
        {
            ARMARX_CHECK(angleUnitDeg->isChecked());
            convertValue = [](double rad) { return simox::math::rad_to_deg(rad); };
        }
 
        std::vector<QSignalBlocker> blockers;
        for (QDoubleSpinBox* angle : _angleSpinBoxes())
        {
            blockers.emplace_back(angle);
 
            angle->setSuffix(suffix);
            angle->setMinimum(min);
            angle->setMaximum(max);
            angle->setSingleStep(step);
            angle->setDecimals(decimals);
        }
        if (_vertex.has_value())
        {
            _setRpyRad(
                simox::math::mat4f_to_rpy(_vertex->attrib().getPose().toEigen().cast<qreal>()));
        }
 
        // blockers will disable blocking for all spin boxes on destruction.
    }
 
    void
    VertexDataWidget::_updateVertexAttribs()
    {
        if (not signalsBlocked() and _vertex.has_value())
        {
            _getToVertex(_vertex.value());
            _vertex->attrib().changed = true;
            emit vertexDataChanged();
        }
    }
 
    void
    VertexDataWidget::_updateVertexAgent()
    {
        if (not signalsBlocked() and _vertex.has_value() and _objectPosesConnection != nullptr)
        {
            {
                QSignalBlocker blocker(this);
 
                _updateFrameMenu();
                frame->setCurrentIndex(0);
 
                _framedPoseCoordinateConversion();
            }
            _updateVertexAttribs();
        }
    }
 
    void
    VertexDataWidget::_updateVertexFrame()
    {
        if (not signalsBlocked() and _vertex.has_value() and _objectPosesConnection != nullptr)
        {
            {
                QSignalBlocker blocker(this);
 
                _framedPoseCoordinateConversion();
            }
            _updateVertexAttribs();
        }
    }
 
    void
    VertexDataWidget::_removeInstance()
    {
        if (not signalsBlocked() and _vertex.has_value())
        {
            // when no agent is set, we don't have to do anything
            if (agent->currentText().isEmpty())
            {
                return;
            }
 
            {
                QSignalBlocker blocker(this);
 
                ObjectID currentAgent(agent->currentText().toStdString());
                QString newAgent = QString::fromStdString(currentAgent.getClassID().str());
                if (agent->findText(newAgent) < 0)
                {
                    agent->insertItem(0, newAgent);
                }
                agent->setCurrentText(newAgent);
            }
            _updateVertexAttribs();
        }
    }
 
    std::vector<QDoubleSpinBox*>
    VertexDataWidget::_positionSpinBoxes()
    {
        return {x, y, z};
    }
 
    std::vector<QDoubleSpinBox*>
    VertexDataWidget::_angleSpinBoxes()
    {
        return {roll, pitch, yaw};
    }
 
    std::vector<QDoubleSpinBox*>
    VertexDataWidget::_allSpinBoxes()
    {
        return {x, y, z, roll, pitch, yaw};
    }
 
    void
    VertexDataWidget::_setPose(const core::Pose_d& pose)
    {
        _setXyz(simox::math::position(pose.matrix()));
        _setRpyRad(simox::math::mat4f_to_rpy(pose.matrix()));
    }
 
    void
    VertexDataWidget::_setXyz(const Eigen::Vector3d& xyz)
    {
        x->setValue(xyz.x());
        y->setValue(xyz.y());
        z->setValue(xyz.z());
    }
 
    Eigen::Vector3d
    VertexDataWidget::_rpyRaw() const
    {
        return {roll->value(), pitch->value(), yaw->value()};
    }
 
    void
    VertexDataWidget::_setRpyRaw(const Eigen::Vector3d& rpy) const
    {
        roll->setValue(rpy(0));
        pitch->setValue(rpy(1));
        yaw->setValue(rpy(2));
    }
 
    void
    VertexDataWidget::_setRpyDeg(const Eigen::Vector3d& rpyDeg) const
    {
        if (angleUnitDeg->isChecked())
        {
            _setRpyRaw(rpyDeg);
        }
        else if (angleUnitRad->isChecked())
        {
            _setRpyRaw(simox::math::deg_to_rad(rpyDeg));
        }
    }
 
    void
    VertexDataWidget::_setRpyRad(const Eigen::Vector3d& rpyRad) const
    {
        if (angleUnitDeg->isChecked())
        {
            _setRpyRaw(simox::math::rad_to_deg(rpyRad));
        }
        else if (angleUnitRad->isChecked())
        {
            _setRpyRaw(rpyRad);
        }
    }
 
    void
    VertexDataWidget::_setFromCurrentRobotPose()
    {
        ARMARX_CHECK_NOT_NULL(_robotConnection);
        {
            QSignalBlocker blocker(this);
            _setPose(_robotConnection->getGlobalPose().cast<qreal>());
        }
        _updateVertexAttribs();
    }
 
    void
    VertexDataWidget::_loadFramedPoseFromVertex(const GuiGraph::Vertex& vertex)
    {
        _updateAgentMenu();
        QString currAgent = QString::fromStdString(vertex.attrib().getPose().agent);
        if (agent->findText(currAgent) < 0)
        {
            agent->insertItem(0, currAgent);
        }
        agent->setCurrentText(currAgent);
 
        _updateFrameMenu();
 
        QString currFrame = QString::fromStdString(vertex.attrib().getPose().frame);
        if (frame->findText(currFrame) < 0)
        {
            frame->insertItem(0, currFrame);
        }
        frame->setCurrentText(currFrame);
    }
 
    void
    VertexDataWidget::_updateAgentMenu()
    {
        agent->clear();
 
        if (_objectPosesConnection == nullptr)
        {
            return;
        }
 
        agent->addItem(QString::fromStdString(""));
        objpose::ObjectPoseMap map = _objectPosesConnection->getObjectPoseMap();
        for (const auto& [id, object] : map)
        {
            agent->addItem(QString::fromStdString(id.str()));
        }
    }
 
    void
    VertexDataWidget::_updateFrameMenu()
    {
        frame->clear();
 
        if (_objectPosesConnection == nullptr)
        {
            return;
        }
 
        const auto nodes = [&]() -> std::optional<std::vector<std::string>>
        {
            const auto& currentObj = agent->currentText().toStdString();
            if (currentObj.empty())
            {
                return std::vector<std::string>({armarx::GlobalFrame});
            }
            else
            {
                const auto& finder = _objectPosesConnection->getObjectClient().getObjectFinder();
                std::optional<ObjectInfo> info = finder.findObject(currentObj);
                if (!info.has_value())
                {
                    return std::nullopt;
                }
 
                std::optional<PackageFileLocation> articulatedModel = info->articulatedSimoxXML();
                if (!articulatedModel.has_value())
                {
                    return std::nullopt;
                }
 
                const auto modelPath = armarx::PackagePath(articulatedModel.value().package,
                                                           articulatedModel.value().relativePath);
                const auto robot = VirtualRobot::RobotIO::loadRobot(
                    modelPath.toSystemPath(), VirtualRobot::BaseIO::eStructure);
                if (robot == nullptr)
                {
                    return std::nullopt;
                }
 
                return robot->getRobotNodeNames();
            }
        };
        std::vector<std::string> frames = nodes().value_or(std::vector<std::string>{"root"});
 
        for (const auto& i : frames)
        {
            frame->addItem(QString::fromStdString(i));
        };
    }
 
    void
    VertexDataWidget::_framedPoseCoordinateConversion()
    {
        objpose::ObjectPoseMap objects = _objectPosesConnection->getObjectPoseMap();
        std::vector<ObjectInfo> objectInfo = _objectPosesConnection->getObjectInfo();
 
        // convert current location into the global frame
        const auto globalPose =
            core::resolveLocation(objects, objectInfo, _vertex->attrib().getPose());
        if (!globalPose.pose.has_value())
        {
            return;
        }
        const core::Pose_d global_T_location = globalPose.pose.value().cast<double>();
 
        // convert from the global frame into the new object relative frame
        const std::string newAgent = agent->currentText().toStdString();
        const std::string newFrame = frame->currentText().toStdString();
 
        const auto object = core::resolveLocation(
            objects, objectInfo, FramedPose(core::Pose::Identity().matrix(), newFrame, newAgent));
        if (!object.pose.has_value())
        {
            return;
        }
        const core::Pose_d global_T_object = object.pose.value().cast<double>();
        const core::Pose_d object_T_global = global_T_object.inverse();
 
        const core::Pose_d object_T_location = object_T_global * global_T_location;
        _setPose(object_T_location);
    }
 
} // namespace armarx::navigation::qt_plugins::location_graph_editor