HomogeneousMatrixCalculatorWidgetController.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::HomogeneousMatrixCalculatorWidgetController
 * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * \date       2017
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "HomogeneousMatrixCalculatorWidgetController.h"
 
#include <RobotAPI/gui-plugins/HomogeneousMatrixCalculator/ui_HomogeneousMatrixCalculatorWidget.h>
 
#include <string>
 
#include <QDoubleValidator>
#include <QLineEdit>
 
#include <Eigen/Dense>
 
using namespace armarx;
 
HomogeneousMatrixCalculatorWidgetController::HomogeneousMatrixCalculatorWidgetController()
{
    widget = std::make_unique<Ui::HomogeneousMatrixCalculatorWidget>();
    widget->setupUi(getWidget());
 
    std::vector<QLineEdit*> edits{
        widget->lineEdit00,       widget->lineEdit01,       widget->lineEdit02,
        widget->lineEdit03,       widget->lineEdit10,       widget->lineEdit11,
        widget->lineEdit12,       widget->lineEdit13,       widget->lineEdit20,
        widget->lineEdit21,       widget->lineEdit22,       widget->lineEdit23,
        widget->lineEdit30,       widget->lineEdit31,       widget->lineEdit32,
        widget->lineEdit33,
 
        widget->lineEditI00,      widget->lineEditI01,      widget->lineEditI02,
        widget->lineEditI03,      widget->lineEditI10,      widget->lineEditI11,
        widget->lineEditI12,      widget->lineEditI13,      widget->lineEditI20,
        widget->lineEditI21,      widget->lineEditI22,      widget->lineEditI23,
        widget->lineEditI30,      widget->lineEditI31,      widget->lineEditI32,
        widget->lineEditI33,
 
        widget->lineEditICheck00, widget->lineEditICheck01, widget->lineEditICheck02,
        widget->lineEditICheck03, widget->lineEditICheck10, widget->lineEditICheck11,
        widget->lineEditICheck12, widget->lineEditICheck13, widget->lineEditICheck20,
        widget->lineEditICheck21, widget->lineEditICheck22, widget->lineEditICheck23,
        widget->lineEditICheck30, widget->lineEditICheck31, widget->lineEditICheck32,
        widget->lineEditICheck33,
 
        widget->lineEditX,        widget->lineEditY,        widget->lineEditZ,
        widget->lineEditRX,       widget->lineEditRY,       widget->lineEditRZ,
 
        widget->lineEditM00,      widget->lineEditM01,      widget->lineEditM02,
        widget->lineEditM03,      widget->lineEditM10,      widget->lineEditM11,
        widget->lineEditM12,      widget->lineEditM13,      widget->lineEditM20,
        widget->lineEditM21,      widget->lineEditM22,      widget->lineEditM23,
        widget->lineEditM30,      widget->lineEditM31,      widget->lineEditM32,
        widget->lineEditM33,
 
        widget->lineEditR00,      widget->lineEditR01,      widget->lineEditR02,
        widget->lineEditR03,      widget->lineEditR10,      widget->lineEditR11,
        widget->lineEditR12,      widget->lineEditR13,      widget->lineEditR20,
        widget->lineEditR21,      widget->lineEditR22,      widget->lineEditR23,
        widget->lineEditR30,      widget->lineEditR31,      widget->lineEditR32,
        widget->lineEditR33,
    };
 
    for (auto edit : edits)
    {
        edit->setValidator(new QDoubleValidator(this));
        edit->setFixedWidth(100);
        edit->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
    }
    connect(widget->lineEdit00, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
    connect(widget->lineEdit01, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
    connect(widget->lineEdit02, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
    connect(widget->lineEdit10, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
    connect(widget->lineEdit11, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
    connect(widget->lineEdit12, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
    connect(widget->lineEdit20, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
    connect(widget->lineEdit21, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
    connect(widget->lineEdit22, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
 
    connect(widget->lineEditRX, SIGNAL(textEdited(QString)), this, SLOT(changedrpy()));
    connect(widget->lineEditRY, SIGNAL(textEdited(QString)), this, SLOT(changedrpy()));
    connect(widget->lineEditRZ, SIGNAL(textEdited(QString)), this, SLOT(changedrpy()));
 
    connect(widget->lineEditM00, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM01, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM02, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM03, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM10, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM11, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM12, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM13, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM20, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM21, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM22, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM23, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM30, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM31, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM32, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
    connect(widget->lineEditM33, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
 
    connect(widget->lineEdit03, SIGNAL(textChanged(QString)), this, SLOT(recalcInvAndProd()));
    connect(widget->lineEdit13, SIGNAL(textChanged(QString)), this, SLOT(recalcInvAndProd()));
    connect(widget->lineEdit23, SIGNAL(textChanged(QString)), this, SLOT(recalcInvAndProd()));
}
 
HomogeneousMatrixCalculatorWidgetController::~HomogeneousMatrixCalculatorWidgetController() =
    default;
 
void
HomogeneousMatrixCalculatorWidgetController::loadSettings(QSettings* settings)
{
}
 
void
HomogeneousMatrixCalculatorWidgetController::saveSettings(QSettings* settings)
{
}
 
void
HomogeneousMatrixCalculatorWidgetController::onInitComponent()
{
}
 
void
HomogeneousMatrixCalculatorWidgetController::onConnectComponent()
{
}
 
Eigen::Matrix4d
HomogeneousMatrixCalculatorWidgetController::getMatrix()
{
    Eigen::Matrix4d m;
    m << widget->lineEdit00->text().toDouble(), widget->lineEdit01->text().toDouble(),
        widget->lineEdit02->text().toDouble(), widget->lineEdit03->text().toDouble(),
        widget->lineEdit10->text().toDouble(), widget->lineEdit11->text().toDouble(),
        widget->lineEdit12->text().toDouble(), widget->lineEdit13->text().toDouble(),
        widget->lineEdit20->text().toDouble(), widget->lineEdit21->text().toDouble(),
        widget->lineEdit22->text().toDouble(), widget->lineEdit23->text().toDouble(),
        widget->lineEdit30->text().toDouble(), widget->lineEdit31->text().toDouble(),
        widget->lineEdit32->text().toDouble(), widget->lineEdit33->text().toDouble();
    return m;
}
 
void
armarx::HomogeneousMatrixCalculatorWidgetController::changed4f()
{
    Eigen::Matrix4d m = getMatrix();
    Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen4f2rpy(m.cast<float>());
    widget->lineEditRX->setText(QString::number(rpy(0)));
    widget->lineEditRY->setText(QString::number(rpy(1)));
    widget->lineEditRZ->setText(QString::number(rpy(2)));
    recalcInvAndProd();
}
 
void
armarx::HomogeneousMatrixCalculatorWidgetController::changedrpy()
{
    float r = static_cast<float>(widget->lineEditRX->text().toDouble());
    float p = static_cast<float>(widget->lineEditRY->text().toDouble());
    float y = static_cast<float>(widget->lineEditRZ->text().toDouble());
    Eigen::Matrix4f m = VirtualRobot::MathTools::rpy2eigen4f(r, p, y);
    widget->lineEdit00->setText(QString::number(m(0, 0)));
    widget->lineEdit01->setText(QString::number(m(0, 1)));
    widget->lineEdit02->setText(QString::number(m(0, 2)));
    widget->lineEdit10->setText(QString::number(m(1, 0)));
    widget->lineEdit11->setText(QString::number(m(1, 1)));
    widget->lineEdit12->setText(QString::number(m(1, 2)));
    widget->lineEdit20->setText(QString::number(m(2, 0)));
    widget->lineEdit21->setText(QString::number(m(2, 1)));
    widget->lineEdit22->setText(QString::number(m(2, 2)));
    recalcInvAndProd();
}
 
void
HomogeneousMatrixCalculatorWidgetController::recalcInv()
{
    Eigen::Matrix4d om = getMatrix();
    Eigen::Matrix4d m = om.inverse();
    widget->lineEditI00->setText(QString::number(m(0, 0)));
    widget->lineEditI01->setText(QString::number(m(0, 1)));
    widget->lineEditI02->setText(QString::number(m(0, 2)));
    widget->lineEditI03->setText(QString::number(m(0, 3)));
    widget->lineEditI10->setText(QString::number(m(1, 0)));
    widget->lineEditI11->setText(QString::number(m(1, 1)));
    widget->lineEditI12->setText(QString::number(m(1, 2)));
    widget->lineEditI13->setText(QString::number(m(1, 3)));
    widget->lineEditI20->setText(QString::number(m(2, 0)));
    widget->lineEditI21->setText(QString::number(m(2, 1)));
    widget->lineEditI22->setText(QString::number(m(2, 2)));
    widget->lineEditI23->setText(QString::number(m(2, 3)));
    widget->lineEditI30->setText(QString::number(m(3, 0)));
    widget->lineEditI31->setText(QString::number(m(3, 1)));
    widget->lineEditI32->setText(QString::number(m(3, 2)));
    widget->lineEditI33->setText(QString::number(m(3, 3)));
    Eigen::Matrix4d check = om * m;
    widget->lineEditICheck00->setText(QString::number(check(0, 0)));
    widget->lineEditICheck01->setText(QString::number(check(0, 1)));
    widget->lineEditICheck02->setText(QString::number(check(0, 2)));
    widget->lineEditICheck03->setText(QString::number(check(0, 3)));
    widget->lineEditICheck10->setText(QString::number(check(1, 0)));
    widget->lineEditICheck11->setText(QString::number(check(1, 1)));
    widget->lineEditICheck12->setText(QString::number(check(1, 2)));
    widget->lineEditICheck13->setText(QString::number(check(1, 3)));
    widget->lineEditICheck20->setText(QString::number(check(2, 0)));
    widget->lineEditICheck21->setText(QString::number(check(2, 1)));
    widget->lineEditICheck22->setText(QString::number(check(2, 2)));
    widget->lineEditICheck23->setText(QString::number(check(2, 3)));
    widget->lineEditICheck30->setText(QString::number(check(3, 0)));
    widget->lineEditICheck31->setText(QString::number(check(3, 1)));
    widget->lineEditICheck32->setText(QString::number(check(3, 2)));
    widget->lineEditICheck33->setText(QString::number(check(3, 3)));
}
 
void
HomogeneousMatrixCalculatorWidgetController::recalcProd()
{
    Eigen::Matrix4d m;
    m << widget->lineEditM00->text().toDouble(), widget->lineEditM01->text().toDouble(),
        widget->lineEditM02->text().toDouble(), widget->lineEditM03->text().toDouble(),
        widget->lineEditM10->text().toDouble(), widget->lineEditM11->text().toDouble(),
        widget->lineEditM12->text().toDouble(), widget->lineEditM13->text().toDouble(),
        widget->lineEditM20->text().toDouble(), widget->lineEditM21->text().toDouble(),
        widget->lineEditM22->text().toDouble(), widget->lineEditM23->text().toDouble(),
        widget->lineEditM30->text().toDouble(), widget->lineEditM31->text().toDouble(),
        widget->lineEditM32->text().toDouble(), widget->lineEditM33->text().toDouble();
    Eigen::Matrix4d r = getMatrix() * m;
    widget->lineEditR00->setText(QString::number(r(0, 0)));
    widget->lineEditR01->setText(QString::number(r(0, 1)));
    widget->lineEditR02->setText(QString::number(r(0, 2)));
    widget->lineEditR03->setText(QString::number(r(0, 3)));
    widget->lineEditR10->setText(QString::number(r(1, 0)));
    widget->lineEditR11->setText(QString::number(r(1, 1)));
    widget->lineEditR12->setText(QString::number(r(1, 2)));
    widget->lineEditR13->setText(QString::number(r(1, 3)));
    widget->lineEditR20->setText(QString::number(r(2, 0)));
    widget->lineEditR21->setText(QString::number(r(2, 1)));
    widget->lineEditR22->setText(QString::number(r(2, 2)));
    widget->lineEditR23->setText(QString::number(r(2, 3)));
    widget->lineEditR30->setText(QString::number(r(3, 0)));
    widget->lineEditR31->setText(QString::number(r(3, 1)));
    widget->lineEditR32->setText(QString::number(r(3, 2)));
    widget->lineEditR33->setText(QString::number(r(3, 3)));
}
 
void
HomogeneousMatrixCalculatorWidgetController::recalcInvAndProd()
{
    recalcInv();
    recalcProd();
}