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 <string>
26 
27 #include <QDoubleValidator>
28 #include <QLineEdit>
29 
30 #include <Eigen/Dense>
31 
32 using namespace armarx;
33 
34 HomogeneousMatrixCalculatorWidgetController::HomogeneousMatrixCalculatorWidgetController()
35 {
36  widget.setupUi(getWidget());
37 
38  std::vector<QLineEdit*> edits{
39  widget.lineEdit00, widget.lineEdit01, widget.lineEdit02,
40  widget.lineEdit03, widget.lineEdit10, widget.lineEdit11,
41  widget.lineEdit12, widget.lineEdit13, widget.lineEdit20,
42  widget.lineEdit21, widget.lineEdit22, widget.lineEdit23,
43  widget.lineEdit30, widget.lineEdit31, widget.lineEdit32,
44  widget.lineEdit33,
45 
46  widget.lineEditI00, widget.lineEditI01, widget.lineEditI02,
47  widget.lineEditI03, widget.lineEditI10, widget.lineEditI11,
48  widget.lineEditI12, widget.lineEditI13, widget.lineEditI20,
49  widget.lineEditI21, widget.lineEditI22, widget.lineEditI23,
50  widget.lineEditI30, widget.lineEditI31, widget.lineEditI32,
51  widget.lineEditI33,
52 
53  widget.lineEditICheck00, widget.lineEditICheck01, widget.lineEditICheck02,
54  widget.lineEditICheck03, widget.lineEditICheck10, widget.lineEditICheck11,
55  widget.lineEditICheck12, widget.lineEditICheck13, widget.lineEditICheck20,
56  widget.lineEditICheck21, widget.lineEditICheck22, widget.lineEditICheck23,
57  widget.lineEditICheck30, widget.lineEditICheck31, widget.lineEditICheck32,
58  widget.lineEditICheck33,
59 
60  widget.lineEditX, widget.lineEditY, widget.lineEditZ,
61  widget.lineEditRX, widget.lineEditRY, widget.lineEditRZ,
62 
63  widget.lineEditM00, widget.lineEditM01, widget.lineEditM02,
64  widget.lineEditM03, widget.lineEditM10, widget.lineEditM11,
65  widget.lineEditM12, widget.lineEditM13, widget.lineEditM20,
66  widget.lineEditM21, widget.lineEditM22, widget.lineEditM23,
67  widget.lineEditM30, widget.lineEditM31, widget.lineEditM32,
68  widget.lineEditM33,
69 
70  widget.lineEditR00, widget.lineEditR01, widget.lineEditR02,
71  widget.lineEditR03, widget.lineEditR10, widget.lineEditR11,
72  widget.lineEditR12, widget.lineEditR13, widget.lineEditR20,
73  widget.lineEditR21, widget.lineEditR22, widget.lineEditR23,
74  widget.lineEditR30, widget.lineEditR31, widget.lineEditR32,
75  widget.lineEditR33,
76  };
77 
78  for (auto edit : edits)
79  {
80  edit->setValidator(new QDoubleValidator(this));
81  edit->setFixedWidth(100);
82  edit->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
83  }
84  connect(widget.lineEdit00, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
85  connect(widget.lineEdit01, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
86  connect(widget.lineEdit02, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
87  connect(widget.lineEdit10, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
88  connect(widget.lineEdit11, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
89  connect(widget.lineEdit12, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
90  connect(widget.lineEdit20, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
91  connect(widget.lineEdit21, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
92  connect(widget.lineEdit22, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
93 
94  connect(widget.lineEditRX, SIGNAL(textEdited(QString)), this, SLOT(changedrpy()));
95  connect(widget.lineEditRY, SIGNAL(textEdited(QString)), this, SLOT(changedrpy()));
96  connect(widget.lineEditRZ, SIGNAL(textEdited(QString)), this, SLOT(changedrpy()));
97 
98  connect(widget.lineEditM00, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
99  connect(widget.lineEditM01, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
100  connect(widget.lineEditM02, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
101  connect(widget.lineEditM03, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
102  connect(widget.lineEditM10, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
103  connect(widget.lineEditM11, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
104  connect(widget.lineEditM12, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
105  connect(widget.lineEditM13, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
106  connect(widget.lineEditM20, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
107  connect(widget.lineEditM21, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
108  connect(widget.lineEditM22, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
109  connect(widget.lineEditM23, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
110  connect(widget.lineEditM30, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
111  connect(widget.lineEditM31, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
112  connect(widget.lineEditM32, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
113  connect(widget.lineEditM33, SIGNAL(textEdited(QString)), this, SLOT(recalcProd()));
114 
115  connect(widget.lineEdit03, SIGNAL(textChanged(QString)), this, SLOT(recalcInvAndProd()));
116  connect(widget.lineEdit13, SIGNAL(textChanged(QString)), this, SLOT(recalcInvAndProd()));
117  connect(widget.lineEdit23, SIGNAL(textChanged(QString)), this, SLOT(recalcInvAndProd()));
118 }
119 
120 HomogeneousMatrixCalculatorWidgetController::~HomogeneousMatrixCalculatorWidgetController() =
121  default;
122 
123 void
124 HomogeneousMatrixCalculatorWidgetController::loadSettings(QSettings* settings)
125 {
126 }
127 
128 void
129 HomogeneousMatrixCalculatorWidgetController::saveSettings(QSettings* settings)
130 {
131 }
132 
133 void
134 HomogeneousMatrixCalculatorWidgetController::onInitComponent()
135 {
136 }
137 
138 void
139 HomogeneousMatrixCalculatorWidgetController::onConnectComponent()
140 {
141 }
142 
143 Eigen::Matrix4d
144 HomogeneousMatrixCalculatorWidgetController::getMatrix()
145 {
146  Eigen::Matrix4d m;
147  m << widget.lineEdit00->text().toDouble(), widget.lineEdit01->text().toDouble(),
148  widget.lineEdit02->text().toDouble(), widget.lineEdit03->text().toDouble(),
149  widget.lineEdit10->text().toDouble(), widget.lineEdit11->text().toDouble(),
150  widget.lineEdit12->text().toDouble(), widget.lineEdit13->text().toDouble(),
151  widget.lineEdit20->text().toDouble(), widget.lineEdit21->text().toDouble(),
152  widget.lineEdit22->text().toDouble(), widget.lineEdit23->text().toDouble(),
153  widget.lineEdit30->text().toDouble(), widget.lineEdit31->text().toDouble(),
154  widget.lineEdit32->text().toDouble(), widget.lineEdit33->text().toDouble();
155  return m;
156 }
157 
158 void
159 armarx::HomogeneousMatrixCalculatorWidgetController::changed4f()
160 {
161  Eigen::Matrix4d m = getMatrix();
162  Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen4f2rpy(m.cast<float>());
163  widget.lineEditRX->setText(QString::number(rpy(0)));
164  widget.lineEditRY->setText(QString::number(rpy(1)));
165  widget.lineEditRZ->setText(QString::number(rpy(2)));
166  recalcInvAndProd();
167 }
168 
169 void
170 armarx::HomogeneousMatrixCalculatorWidgetController::changedrpy()
171 {
172  float r = static_cast<float>(widget.lineEditRX->text().toDouble());
173  float p = static_cast<float>(widget.lineEditRY->text().toDouble());
174  float y = static_cast<float>(widget.lineEditRZ->text().toDouble());
175  Eigen::Matrix4f m = VirtualRobot::MathTools::rpy2eigen4f(r, p, y);
176  widget.lineEdit00->setText(QString::number(m(0, 0)));
177  widget.lineEdit01->setText(QString::number(m(0, 1)));
178  widget.lineEdit02->setText(QString::number(m(0, 2)));
179  widget.lineEdit10->setText(QString::number(m(1, 0)));
180  widget.lineEdit11->setText(QString::number(m(1, 1)));
181  widget.lineEdit12->setText(QString::number(m(1, 2)));
182  widget.lineEdit20->setText(QString::number(m(2, 0)));
183  widget.lineEdit21->setText(QString::number(m(2, 1)));
184  widget.lineEdit22->setText(QString::number(m(2, 2)));
185  recalcInvAndProd();
186 }
187 
188 void
189 HomogeneousMatrixCalculatorWidgetController::recalcInv()
190 {
191  Eigen::Matrix4d om = getMatrix();
192  Eigen::Matrix4d m = om.inverse();
193  widget.lineEditI00->setText(QString::number(m(0, 0)));
194  widget.lineEditI01->setText(QString::number(m(0, 1)));
195  widget.lineEditI02->setText(QString::number(m(0, 2)));
196  widget.lineEditI03->setText(QString::number(m(0, 3)));
197  widget.lineEditI10->setText(QString::number(m(1, 0)));
198  widget.lineEditI11->setText(QString::number(m(1, 1)));
199  widget.lineEditI12->setText(QString::number(m(1, 2)));
200  widget.lineEditI13->setText(QString::number(m(1, 3)));
201  widget.lineEditI20->setText(QString::number(m(2, 0)));
202  widget.lineEditI21->setText(QString::number(m(2, 1)));
203  widget.lineEditI22->setText(QString::number(m(2, 2)));
204  widget.lineEditI23->setText(QString::number(m(2, 3)));
205  widget.lineEditI30->setText(QString::number(m(3, 0)));
206  widget.lineEditI31->setText(QString::number(m(3, 1)));
207  widget.lineEditI32->setText(QString::number(m(3, 2)));
208  widget.lineEditI33->setText(QString::number(m(3, 3)));
209  Eigen::Matrix4d check = om * m;
210  widget.lineEditICheck00->setText(QString::number(check(0, 0)));
211  widget.lineEditICheck01->setText(QString::number(check(0, 1)));
212  widget.lineEditICheck02->setText(QString::number(check(0, 2)));
213  widget.lineEditICheck03->setText(QString::number(check(0, 3)));
214  widget.lineEditICheck10->setText(QString::number(check(1, 0)));
215  widget.lineEditICheck11->setText(QString::number(check(1, 1)));
216  widget.lineEditICheck12->setText(QString::number(check(1, 2)));
217  widget.lineEditICheck13->setText(QString::number(check(1, 3)));
218  widget.lineEditICheck20->setText(QString::number(check(2, 0)));
219  widget.lineEditICheck21->setText(QString::number(check(2, 1)));
220  widget.lineEditICheck22->setText(QString::number(check(2, 2)));
221  widget.lineEditICheck23->setText(QString::number(check(2, 3)));
222  widget.lineEditICheck30->setText(QString::number(check(3, 0)));
223  widget.lineEditICheck31->setText(QString::number(check(3, 1)));
224  widget.lineEditICheck32->setText(QString::number(check(3, 2)));
225  widget.lineEditICheck33->setText(QString::number(check(3, 3)));
226 }
227 
228 void
229 HomogeneousMatrixCalculatorWidgetController::recalcProd()
230 {
231  Eigen::Matrix4d m;
232  m << widget.lineEditM00->text().toDouble(), widget.lineEditM01->text().toDouble(),
233  widget.lineEditM02->text().toDouble(), widget.lineEditM03->text().toDouble(),
234  widget.lineEditM10->text().toDouble(), widget.lineEditM11->text().toDouble(),
235  widget.lineEditM12->text().toDouble(), widget.lineEditM13->text().toDouble(),
236  widget.lineEditM20->text().toDouble(), widget.lineEditM21->text().toDouble(),
237  widget.lineEditM22->text().toDouble(), widget.lineEditM23->text().toDouble(),
238  widget.lineEditM30->text().toDouble(), widget.lineEditM31->text().toDouble(),
239  widget.lineEditM32->text().toDouble(), widget.lineEditM33->text().toDouble();
240  Eigen::Matrix4d r = getMatrix() * m;
241  widget.lineEditR00->setText(QString::number(r(0, 0)));
242  widget.lineEditR01->setText(QString::number(r(0, 1)));
243  widget.lineEditR02->setText(QString::number(r(0, 2)));
244  widget.lineEditR03->setText(QString::number(r(0, 3)));
245  widget.lineEditR10->setText(QString::number(r(1, 0)));
246  widget.lineEditR11->setText(QString::number(r(1, 1)));
247  widget.lineEditR12->setText(QString::number(r(1, 2)));
248  widget.lineEditR13->setText(QString::number(r(1, 3)));
249  widget.lineEditR20->setText(QString::number(r(2, 0)));
250  widget.lineEditR21->setText(QString::number(r(2, 1)));
251  widget.lineEditR22->setText(QString::number(r(2, 2)));
252  widget.lineEditR23->setText(QString::number(r(2, 3)));
253  widget.lineEditR30->setText(QString::number(r(3, 0)));
254  widget.lineEditR31->setText(QString::number(r(3, 1)));
255  widget.lineEditR32->setText(QString::number(r(3, 2)));
256  widget.lineEditR33->setText(QString::number(r(3, 3)));
257 }
258 
259 void
260 HomogeneousMatrixCalculatorWidgetController::recalcInvAndProd()
261 {
262  recalcInv();
263  recalcProd();
264 }
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::HomogeneousMatrixCalculatorWidgetController::saveSettings
void saveSettings(QSettings *settings) override
Definition: HomogeneousMatrixCalculatorWidgetController.cpp:129
armarx::HomogeneousMatrixCalculatorWidgetController::HomogeneousMatrixCalculatorWidgetController
HomogeneousMatrixCalculatorWidgetController()
Controller Constructor.
Definition: HomogeneousMatrixCalculatorWidgetController.cpp:34
armarx::HomogeneousMatrixCalculatorWidgetController::getMatrix
Eigen::Matrix4d getMatrix()
Definition: HomogeneousMatrixCalculatorWidgetController.cpp:144
armarx::HomogeneousMatrixCalculatorWidgetController::~HomogeneousMatrixCalculatorWidgetController
~HomogeneousMatrixCalculatorWidgetController() override
Controller destructor.
armarx::HomogeneousMatrixCalculatorWidgetController::loadSettings
void loadSettings(QSettings *settings) override
Definition: HomogeneousMatrixCalculatorWidgetController.cpp:124
armarx::HomogeneousMatrixCalculatorWidgetController::onConnectComponent
void onConnectComponent() override
Definition: HomogeneousMatrixCalculatorWidgetController.cpp:139
armarx::HomogeneousMatrixCalculatorWidgetController::onInitComponent
void onInitComponent() override
Definition: HomogeneousMatrixCalculatorWidgetController.cpp:134
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:27