HomogeneousMatrixCalculatorWidgetController.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * \package RobotAPI::gui-plugins::HomogeneousMatrixCalculatorWidgetController
17  * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
18  * \date 2017
19  * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "HomogeneousMatrixCalculatorWidgetController.h"
24 
25 #include <Eigen/Dense>
26 
27 #include <string>
28 
29 
30 #include <QDoubleValidator>
31 #include <QLineEdit>
32 
33 using namespace armarx;
34 
35 HomogeneousMatrixCalculatorWidgetController::HomogeneousMatrixCalculatorWidgetController()
36 {
37  widget.setupUi(getWidget());
38 
39  std::vector<QLineEdit*> edits
40  {
41  widget.lineEdit00, widget.lineEdit01, widget.lineEdit02, widget.lineEdit03,
42  widget.lineEdit10, widget.lineEdit11, widget.lineEdit12, widget.lineEdit13,
43  widget.lineEdit20, widget.lineEdit21, widget.lineEdit22, widget.lineEdit23,
44  widget.lineEdit30, widget.lineEdit31, widget.lineEdit32, widget.lineEdit33,
45 
46  widget.lineEditI00, widget.lineEditI01, widget.lineEditI02, widget.lineEditI03,
47  widget.lineEditI10, widget.lineEditI11, widget.lineEditI12, widget.lineEditI13,
48  widget.lineEditI20, widget.lineEditI21, widget.lineEditI22, widget.lineEditI23,
49  widget.lineEditI30, widget.lineEditI31, widget.lineEditI32, widget.lineEditI33,
50 
51  widget.lineEditICheck00, widget.lineEditICheck01, widget.lineEditICheck02, widget.lineEditICheck03,
52  widget.lineEditICheck10, widget.lineEditICheck11, widget.lineEditICheck12, widget.lineEditICheck13,
53  widget.lineEditICheck20, widget.lineEditICheck21, widget.lineEditICheck22, widget.lineEditICheck23,
54  widget.lineEditICheck30, widget.lineEditICheck31, widget.lineEditICheck32, widget.lineEditICheck33,
55 
56  widget.lineEditX, widget.lineEditY, widget.lineEditZ,
57  widget.lineEditRX, widget.lineEditRY, widget.lineEditRZ,
58 
59  widget.lineEditM00, widget.lineEditM01, widget.lineEditM02, widget.lineEditM03,
60  widget.lineEditM10, widget.lineEditM11, widget.lineEditM12, widget.lineEditM13,
61  widget.lineEditM20, widget.lineEditM21, widget.lineEditM22, widget.lineEditM23,
62  widget.lineEditM30, widget.lineEditM31, widget.lineEditM32, widget.lineEditM33,
63 
64  widget.lineEditR00, widget.lineEditR01, widget.lineEditR02, widget.lineEditR03,
65  widget.lineEditR10, widget.lineEditR11, widget.lineEditR12, widget.lineEditR13,
66  widget.lineEditR20, widget.lineEditR21, widget.lineEditR22, widget.lineEditR23,
67  widget.lineEditR30, widget.lineEditR31, widget.lineEditR32, widget.lineEditR33,
68  };
69 
70  for (auto edit : edits)
71  {
72  edit->setValidator(new QDoubleValidator(this));
73  edit->setFixedWidth(100);
74  edit->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
75 
76  }
77  connect(widget.lineEdit00, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
78  connect(widget.lineEdit01, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
79  connect(widget.lineEdit02, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
80  connect(widget.lineEdit10, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
81  connect(widget.lineEdit11, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
82  connect(widget.lineEdit12, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
83  connect(widget.lineEdit20, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
84  connect(widget.lineEdit21, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
85  connect(widget.lineEdit22, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
86 
87  connect(widget.lineEditRX, SIGNAL(textEdited(QString)), this, SLOT(changedrpy()));
88  connect(widget.lineEditRY, SIGNAL(textEdited(QString)), this, SLOT(changedrpy()));
89  connect(widget.lineEditRZ, SIGNAL(textEdited(QString)), this, SLOT(changedrpy()));
90 
91  connect(widget.lineEditM00, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
92  connect(widget.lineEditM01, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
93  connect(widget.lineEditM02, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
94  connect(widget.lineEditM03, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
95  connect(widget.lineEditM10, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
96  connect(widget.lineEditM11, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
97  connect(widget.lineEditM12, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
98  connect(widget.lineEditM13, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
99  connect(widget.lineEditM20, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
100  connect(widget.lineEditM21, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
101  connect(widget.lineEditM22, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
102  connect(widget.lineEditM23, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
103  connect(widget.lineEditM30, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
104  connect(widget.lineEditM31, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
105  connect(widget.lineEditM32, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
106  connect(widget.lineEditM33, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
107 
108  connect(widget.lineEdit03, SIGNAL(textChanged(QString)), this, SLOT(recalcInvAndProd()));
109  connect(widget.lineEdit13, SIGNAL(textChanged(QString)), this, SLOT(recalcInvAndProd()));
110  connect(widget.lineEdit23, SIGNAL(textChanged(QString)), this, SLOT(recalcInvAndProd()));
111 
112 }
113 
114 
115 HomogeneousMatrixCalculatorWidgetController::~HomogeneousMatrixCalculatorWidgetController()
116  = default;
117 
118 
119 void HomogeneousMatrixCalculatorWidgetController::loadSettings(QSettings* settings)
120 {
121 
122 }
123 
124 void HomogeneousMatrixCalculatorWidgetController::saveSettings(QSettings* settings)
125 {
126 
127 }
128 
129 
130 void HomogeneousMatrixCalculatorWidgetController::onInitComponent()
131 {
132 
133 }
134 
135 
136 void HomogeneousMatrixCalculatorWidgetController::onConnectComponent()
137 {
138 
139 }
140 
141 Eigen::Matrix4d HomogeneousMatrixCalculatorWidgetController::getMatrix()
142 {
143  Eigen::Matrix4d m;
144  m << widget.lineEdit00->text().toDouble(), widget.lineEdit01->text().toDouble(), widget.lineEdit02->text().toDouble(), widget.lineEdit03->text().toDouble(),
145  widget.lineEdit10->text().toDouble(), widget.lineEdit11->text().toDouble(), widget.lineEdit12->text().toDouble(), widget.lineEdit13->text().toDouble(),
146  widget.lineEdit20->text().toDouble(), widget.lineEdit21->text().toDouble(), widget.lineEdit22->text().toDouble(), widget.lineEdit23->text().toDouble(),
147  widget.lineEdit30->text().toDouble(), widget.lineEdit31->text().toDouble(), widget.lineEdit32->text().toDouble(), widget.lineEdit33->text().toDouble();
148  return m;
149 }
150 
151 void armarx::HomogeneousMatrixCalculatorWidgetController::changed4f()
152 {
153  Eigen::Matrix4d m = getMatrix();
154  Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen4f2rpy(m.cast<float>());
155  widget.lineEditRX->setText(QString::number(rpy(0)));
156  widget.lineEditRY->setText(QString::number(rpy(1)));
157  widget.lineEditRZ->setText(QString::number(rpy(2)));
158  recalcInvAndProd();
159 }
160 
161 void armarx::HomogeneousMatrixCalculatorWidgetController::changedrpy()
162 {
163  float r = static_cast<float>(widget.lineEditRX->text().toDouble());
164  float p = static_cast<float>(widget.lineEditRY->text().toDouble());
165  float y = static_cast<float>(widget.lineEditRZ->text().toDouble());
166  Eigen::Matrix4f m = VirtualRobot::MathTools::rpy2eigen4f(r, p, y);
167  widget.lineEdit00->setText(QString::number(m(0, 0)));
168  widget.lineEdit01->setText(QString::number(m(0, 1)));
169  widget.lineEdit02->setText(QString::number(m(0, 2)));
170  widget.lineEdit10->setText(QString::number(m(1, 0)));
171  widget.lineEdit11->setText(QString::number(m(1, 1)));
172  widget.lineEdit12->setText(QString::number(m(1, 2)));
173  widget.lineEdit20->setText(QString::number(m(2, 0)));
174  widget.lineEdit21->setText(QString::number(m(2, 1)));
175  widget.lineEdit22->setText(QString::number(m(2, 2)));
176  recalcInvAndProd();
177 }
178 
179 void HomogeneousMatrixCalculatorWidgetController::recalcInv()
180 {
181  Eigen::Matrix4d om = getMatrix();
182  Eigen::Matrix4d m = om.inverse();
183  widget.lineEditI00->setText(QString::number(m(0, 0)));
184  widget.lineEditI01->setText(QString::number(m(0, 1)));
185  widget.lineEditI02->setText(QString::number(m(0, 2)));
186  widget.lineEditI03->setText(QString::number(m(0, 3)));
187  widget.lineEditI10->setText(QString::number(m(1, 0)));
188  widget.lineEditI11->setText(QString::number(m(1, 1)));
189  widget.lineEditI12->setText(QString::number(m(1, 2)));
190  widget.lineEditI13->setText(QString::number(m(1, 3)));
191  widget.lineEditI20->setText(QString::number(m(2, 0)));
192  widget.lineEditI21->setText(QString::number(m(2, 1)));
193  widget.lineEditI22->setText(QString::number(m(2, 2)));
194  widget.lineEditI23->setText(QString::number(m(2, 3)));
195  widget.lineEditI30->setText(QString::number(m(3, 0)));
196  widget.lineEditI31->setText(QString::number(m(3, 1)));
197  widget.lineEditI32->setText(QString::number(m(3, 2)));
198  widget.lineEditI33->setText(QString::number(m(3, 3)));
199  Eigen::Matrix4d check = om * m;
200  widget.lineEditICheck00->setText(QString::number(check(0, 0)));
201  widget.lineEditICheck01->setText(QString::number(check(0, 1)));
202  widget.lineEditICheck02->setText(QString::number(check(0, 2)));
203  widget.lineEditICheck03->setText(QString::number(check(0, 3)));
204  widget.lineEditICheck10->setText(QString::number(check(1, 0)));
205  widget.lineEditICheck11->setText(QString::number(check(1, 1)));
206  widget.lineEditICheck12->setText(QString::number(check(1, 2)));
207  widget.lineEditICheck13->setText(QString::number(check(1, 3)));
208  widget.lineEditICheck20->setText(QString::number(check(2, 0)));
209  widget.lineEditICheck21->setText(QString::number(check(2, 1)));
210  widget.lineEditICheck22->setText(QString::number(check(2, 2)));
211  widget.lineEditICheck23->setText(QString::number(check(2, 3)));
212  widget.lineEditICheck30->setText(QString::number(check(3, 0)));
213  widget.lineEditICheck31->setText(QString::number(check(3, 1)));
214  widget.lineEditICheck32->setText(QString::number(check(3, 2)));
215  widget.lineEditICheck33->setText(QString::number(check(3, 3)));
216 }
217 
218 void HomogeneousMatrixCalculatorWidgetController::recalcProd()
219 {
220  Eigen::Matrix4d m;
221  m << widget.lineEditM00->text().toDouble(), widget.lineEditM01->text().toDouble(), widget.lineEditM02->text().toDouble(), widget.lineEditM03->text().toDouble(),
222  widget.lineEditM10->text().toDouble(), widget.lineEditM11->text().toDouble(), widget.lineEditM12->text().toDouble(), widget.lineEditM13->text().toDouble(),
223  widget.lineEditM20->text().toDouble(), widget.lineEditM21->text().toDouble(), widget.lineEditM22->text().toDouble(), widget.lineEditM23->text().toDouble(),
224  widget.lineEditM30->text().toDouble(), widget.lineEditM31->text().toDouble(), widget.lineEditM32->text().toDouble(), widget.lineEditM33->text().toDouble();
225  Eigen::Matrix4d r = getMatrix() * m;
226  widget.lineEditR00->setText(QString::number(r(0, 0)));
227  widget.lineEditR01->setText(QString::number(r(0, 1)));
228  widget.lineEditR02->setText(QString::number(r(0, 2)));
229  widget.lineEditR03->setText(QString::number(r(0, 3)));
230  widget.lineEditR10->setText(QString::number(r(1, 0)));
231  widget.lineEditR11->setText(QString::number(r(1, 1)));
232  widget.lineEditR12->setText(QString::number(r(1, 2)));
233  widget.lineEditR13->setText(QString::number(r(1, 3)));
234  widget.lineEditR20->setText(QString::number(r(2, 0)));
235  widget.lineEditR21->setText(QString::number(r(2, 1)));
236  widget.lineEditR22->setText(QString::number(r(2, 2)));
237  widget.lineEditR23->setText(QString::number(r(2, 3)));
238  widget.lineEditR30->setText(QString::number(r(3, 0)));
239  widget.lineEditR31->setText(QString::number(r(3, 1)));
240  widget.lineEditR32->setText(QString::number(r(3, 2)));
241  widget.lineEditR33->setText(QString::number(r(3, 3)));
242 }
243 
244 void HomogeneousMatrixCalculatorWidgetController::recalcInvAndProd()
245 {
246  recalcInv();
247  recalcProd();
248 }
armarx::HomogeneousMatrixCalculatorWidgetController::saveSettings
void saveSettings(QSettings *settings) override
Definition: HomogeneousMatrixCalculatorWidgetController.cpp:124
armarx::HomogeneousMatrixCalculatorWidgetController::HomogeneousMatrixCalculatorWidgetController
HomogeneousMatrixCalculatorWidgetController()
Controller Constructor.
Definition: HomogeneousMatrixCalculatorWidgetController.cpp:35
armarx::HomogeneousMatrixCalculatorWidgetController::getMatrix
Eigen::Matrix4d getMatrix()
Definition: HomogeneousMatrixCalculatorWidgetController.cpp:141
armarx::HomogeneousMatrixCalculatorWidgetController::~HomogeneousMatrixCalculatorWidgetController
~HomogeneousMatrixCalculatorWidgetController() override
Controller destructor.
armarx::HomogeneousMatrixCalculatorWidgetController::loadSettings
void loadSettings(QSettings *settings) override
Definition: HomogeneousMatrixCalculatorWidgetController.cpp:119
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::HomogeneousMatrixCalculatorWidgetController::onConnectComponent
void onConnectComponent() override
Definition: HomogeneousMatrixCalculatorWidgetController.cpp:136
armarx::HomogeneousMatrixCalculatorWidgetController::onInitComponent
void onInitComponent() override
Definition: HomogeneousMatrixCalculatorWidgetController.cpp:130
HomogeneousMatrixCalculatorWidgetController.h
armarx::ArmarXWidgetController::getWidget
virtual QPointer< QWidget > getWidget()
getWidget returns a pointer to the a widget of this controller.
Definition: ArmarXWidgetController.cpp:54
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28