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 <string>
 
#include <QDoubleValidator>
#include <QLineEdit>
 
#include <Eigen/Dense>
 
using namespace armarx;
 
HomogeneousMatrixCalculatorWidgetController::HomogeneousMatrixCalculatorWidgetController()
{
    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();
}