BimanualCartesianAdmittanceControllerGuiWidgetController.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::BimanualCartesianAdmittanceControllerGuiWidgetController
17 * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
18 * \date 2020
19 * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22
23#include "BimanualCartesianAdmittanceControllerGuiWidgetController.h"
24
25#include <string>
26
27#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
28#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
29
30#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
31
32namespace armarx
33{
34 void
35 clearLayout(QLayout* layout)
36 {
37 QLayoutItem* item;
38 while ((item = layout->takeAt(0)))
39 {
40 if (item->layout())
41 {
42 clearLayout(item->layout());
43 delete item->layout();
44 }
45 if (item->widget())
46 {
47 delete item->widget();
48 }
49 delete item;
50 }
51 }
52
53 BimanualCartesianAdmittanceControllerGuiWidgetController::
54 BimanualCartesianAdmittanceControllerGuiWidgetController() :
55 NJointControllerGuiPluginBase("NJointBimanualCartesianAdmittanceController")
56 {
57 _ui.setupUi(getWidget());
58 connectCreateAcivateDeactivateDelete(_ui.pushButtonCtrlCreate,
59 _ui.pushButtonCtrlActivate,
60 _ui.pushButtonCtrlDeactivate,
61 _ui.pushButtonCtrlDelete);
62 using T = BimanualCartesianAdmittanceControllerGuiWidgetController;
63 connect(_ui.pushButtonReadCurrentPose,
64 &QPushButton::clicked,
65 this,
66 &T::on_pushButtonReadCurrentPose_clicked);
67 connect(
68 _ui.pushButtonTargAdd, &QPushButton::clicked, this, &T::on_pushButtonTargAdd_clicked);
69 connect(_ui.pushButtonCfgSendDefaultPose,
70 &QPushButton::clicked,
71 this,
72 &T::on_pushButtonCfgSendDefaultPose_clicked);
73 connect(_ui.pushButtonCfgSendNullspace,
74 &QPushButton::clicked,
75 this,
76 &T::on_pushButtonCfgSendNullspace_clicked);
77 connect(_ui.pushButtonCfgSendImpedance,
78 &QPushButton::clicked,
79 this,
80 &T::on_pushButtonCfgSendImpedance_clicked);
81 connect(_ui.pushButtonCfgSendAdmittance,
82 &QPushButton::clicked,
83 this,
84 &T::on_pushButtonCfgSendAdmittance_clicked);
85 connect(_ui.pushButtonCfgSendForce,
86 &QPushButton::clicked,
87 this,
88 &T::on_pushButtonCfgSendForce_clicked);
89 connect(_ui.pushButtonCfgSendAll,
90 &QPushButton::clicked,
91 this,
92 &T::on_pushButtonCfgSendAll_clicked);
93 connect(
94 _ui.pushButtonTargSend, &QPushButton::clicked, this, &T::on_pushButtonTargSend_clicked);
95 }
96
97 void
98 BimanualCartesianAdmittanceControllerGuiWidgetController::setupGuiAfterConnect()
99 {
100 //fill rns combo box
101 {
102 const auto fill = [&](auto rnsname, auto& lay, auto& sp)
103 {
104 static const std::map<std::string, double> defaults{
105 ///TODO
106 };
107
108 clearLayout(lay);
109 int i = 0;
110 for (const auto& rn : _robot->getRobotNodeSet(rnsname)->getAllRobotNodes())
111 {
112 const auto&& n = rn->getName();
113 lay->addWidget(new QLabel{QString::fromStdString(n)}, i, 0);
114 auto b = new QDoubleSpinBox;
115 sp.addWidget(b);
116 lay->addWidget(b, i, 1);
117 const auto lo = rn->getJointLimitLow();
118 const auto hi = rn->getJointLimitHigh();
119 b->setMinimum(lo);
120 b->setMaximum(hi);
121 b->setValue(defaults.count(n) ? defaults.at(n) : (lo + hi) / 2);
122 ++i;
123 }
124 };
125 fill("LeftArm", _ui.gridLayoutDefaultPoseL, _desiredJointValuesLeft);
126 fill("RightArm", _ui.gridLayoutDefaultPoseR, _desiredJointValuesRight);
127 }
128 }
129} // namespace armarx
130
131//read config
132namespace armarx
133{
134 std::array<Ice::FloatSeq, 2>
135 BimanualCartesianAdmittanceControllerGuiWidgetController::readDesiredJointCFG() const
136 {
137 return {_desiredJointValuesLeft.get<std::vector<float>>(),
138 _desiredJointValuesRight.get<std::vector<float>>()};
139 }
140
141 detail::NJBmanCartAdmCtrl::Nullspace
142 BimanualCartesianAdmittanceControllerGuiWidgetController::readNullspaceCFG() const
143 {
144 detail::NJBmanCartAdmCtrl::Nullspace c;
145 c.k = _ui.doubleSpinBoxNK->value();
146 c.d = _ui.doubleSpinBoxND->value();
147 const auto arms = readDesiredJointCFG();
148 c.desiredJointValuesLeft = arms.at(0);
149 c.desiredJointValuesRight = arms.at(1);
150 return c;
151 }
152
153 std::array<detail::NJBmanCartAdmCtrl::Impedance, 2>
154 BimanualCartesianAdmittanceControllerGuiWidgetController::readImpedanceCFG() const
155 {
156 detail::NJBmanCartAdmCtrl::Impedance l;
157 detail::NJBmanCartAdmCtrl::Impedance r;
158
159 l.KpXYZ(0) = _ui.doubleSpinBoxIPTXL->value();
160 l.KpXYZ(1) = _ui.doubleSpinBoxIPTYL->value();
161 l.KpXYZ(2) = _ui.doubleSpinBoxIPTZL->value();
162
163 l.KpRPY(0) = _ui.doubleSpinBoxIPRXL->value();
164 l.KpRPY(1) = _ui.doubleSpinBoxIPRYL->value();
165 l.KpRPY(2) = _ui.doubleSpinBoxIPRZL->value();
166
167 l.KdXYZ(0) = _ui.doubleSpinBoxIDTXL->value();
168 l.KdXYZ(1) = _ui.doubleSpinBoxIDTYL->value();
169 l.KdXYZ(2) = _ui.doubleSpinBoxIDTZL->value();
170
171 l.KdRPY(0) = _ui.doubleSpinBoxIDRXL->value();
172 l.KdRPY(1) = _ui.doubleSpinBoxIDRYL->value();
173 l.KdRPY(2) = _ui.doubleSpinBoxIDRZL->value();
174
175 r.KpXYZ(0) = _ui.doubleSpinBoxIPTXR->value();
176 r.KpXYZ(1) = _ui.doubleSpinBoxIPTYR->value();
177 r.KpXYZ(2) = _ui.doubleSpinBoxIPTZR->value();
178
179 r.KpRPY(0) = _ui.doubleSpinBoxIPRXR->value();
180 r.KpRPY(1) = _ui.doubleSpinBoxIPRYR->value();
181 r.KpRPY(2) = _ui.doubleSpinBoxIPRZR->value();
182
183 r.KdXYZ(0) = _ui.doubleSpinBoxIDTXR->value();
184 r.KdXYZ(1) = _ui.doubleSpinBoxIDTYR->value();
185 r.KdXYZ(2) = _ui.doubleSpinBoxIDTZR->value();
186
187 r.KdRPY(0) = _ui.doubleSpinBoxIDRXR->value();
188 r.KdRPY(1) = _ui.doubleSpinBoxIDRYR->value();
189 r.KdRPY(2) = _ui.doubleSpinBoxIDRZR->value();
190
191 return {l, r};
192 }
193
194 std::array<detail::NJBmanCartAdmCtrl::Force, 2>
195 BimanualCartesianAdmittanceControllerGuiWidgetController::readForceCFG() const
196 {
197 detail::NJBmanCartAdmCtrl::Force l;
198 detail::NJBmanCartAdmCtrl::Force r;
199
200 l.wrenchXYZ(0) = _ui.doubleSpinBoxFWTXL->value();
201 l.wrenchXYZ(1) = _ui.doubleSpinBoxFWTYL->value();
202 l.wrenchXYZ(2) = _ui.doubleSpinBoxFWTZL->value();
203
204 l.wrenchRPY(0) = _ui.doubleSpinBoxFWRXL->value();
205 l.wrenchRPY(1) = _ui.doubleSpinBoxFWRYL->value();
206 l.wrenchRPY(2) = _ui.doubleSpinBoxFWRZL->value();
207
208 l.mass = _ui.doubleSpinBoxFML->value();
209
210 l.offsetForce(0) = _ui.doubleSpinBoxFOFXL->value();
211 l.offsetForce(1) = _ui.doubleSpinBoxFOFYL->value();
212 l.offsetForce(2) = _ui.doubleSpinBoxFOFZL->value();
213
214 l.offsetTorque(0) = _ui.doubleSpinBoxFOTXL->value();
215 l.offsetTorque(1) = _ui.doubleSpinBoxFOTYL->value();
216 l.offsetTorque(2) = _ui.doubleSpinBoxFOTZL->value();
217
218 l.forceThreshold(0) = _ui.doubleSpinBoxFFTXL->value();
219 l.forceThreshold(1) = _ui.doubleSpinBoxFFTYL->value();
220 l.forceThreshold(2) = _ui.doubleSpinBoxFFTZL->value();
221
222 r.wrenchXYZ(0) = _ui.doubleSpinBoxFWTXR->value();
223 r.wrenchXYZ(1) = _ui.doubleSpinBoxFWTYR->value();
224 r.wrenchXYZ(2) = _ui.doubleSpinBoxFWTZR->value();
225
226 r.wrenchRPY(0) = _ui.doubleSpinBoxFWRXR->value();
227 r.wrenchRPY(1) = _ui.doubleSpinBoxFWRYR->value();
228 r.wrenchRPY(2) = _ui.doubleSpinBoxFWRZR->value();
229
230 r.mass = _ui.doubleSpinBoxFMR->value();
231
232 r.offsetForce(0) = _ui.doubleSpinBoxFOFXR->value();
233 r.offsetForce(1) = _ui.doubleSpinBoxFOFYR->value();
234 r.offsetForce(2) = _ui.doubleSpinBoxFOFZR->value();
235
236 r.offsetTorque(0) = _ui.doubleSpinBoxFOTXR->value();
237 r.offsetTorque(1) = _ui.doubleSpinBoxFOTYR->value();
238 r.offsetTorque(2) = _ui.doubleSpinBoxFOTZR->value();
239
240 r.forceThreshold(0) = _ui.doubleSpinBoxFFTXR->value();
241 r.forceThreshold(1) = _ui.doubleSpinBoxFFTYR->value();
242 r.forceThreshold(2) = _ui.doubleSpinBoxFFTZR->value();
243 return {l, r};
244 }
245
246 detail::NJBmanCartAdmCtrl::Admittance
247 BimanualCartesianAdmittanceControllerGuiWidgetController::readAdmittanceCFG() const
248 {
249 detail::NJBmanCartAdmCtrl::Admittance c;
250
251 c.KpXYZ(0) = _ui.doubleSpinBoxAPTX->value();
252 c.KpXYZ(1) = _ui.doubleSpinBoxAPTY->value();
253 c.KpXYZ(2) = _ui.doubleSpinBoxAPTZ->value();
254
255 c.KpRPY(0) = _ui.doubleSpinBoxAPRX->value();
256 c.KpRPY(1) = _ui.doubleSpinBoxAPRY->value();
257 c.KpRPY(2) = _ui.doubleSpinBoxAPRZ->value();
258
259 c.KdXYZ(0) = _ui.doubleSpinBoxADTX->value();
260 c.KdXYZ(1) = _ui.doubleSpinBoxADTY->value();
261 c.KdXYZ(2) = _ui.doubleSpinBoxADTZ->value();
262
263 c.KdRPY(0) = _ui.doubleSpinBoxADRX->value();
264 c.KdRPY(1) = _ui.doubleSpinBoxADRY->value();
265 c.KdRPY(2) = _ui.doubleSpinBoxADRZ->value();
266
267 c.KmXYZ(0) = _ui.doubleSpinBoxAMTX->value();
268 c.KmXYZ(1) = _ui.doubleSpinBoxAMTY->value();
269 c.KmXYZ(2) = _ui.doubleSpinBoxAMTZ->value();
270
271 c.KmRPY(0) = _ui.doubleSpinBoxAMRX->value();
272 c.KmRPY(1) = _ui.doubleSpinBoxAMRY->value();
273 c.KmRPY(2) = _ui.doubleSpinBoxAMRZ->value();
274
275 return c;
276 }
277
278 NJointControllerConfigPtr
279 BimanualCartesianAdmittanceControllerGuiWidgetController::readFullCFG() const
280 {
281 NJointBimanualCartesianAdmittanceControllerConfigPtr c =
282 new NJointBimanualCartesianAdmittanceControllerConfig;
283 c->kinematicChainRight = "RightArm";
284 c->kinematicChainLeft = "LeftArm";
285 c->ftSensorRight = "FT R";
286 c->ftSensorLeft = "FT L";
287 c->ftSensorRightFrame = "ArmR_FT";
288 c->ftSensorLeftFrame = "ArmL_FT";
289 c->box.width = _ui.doubleSpinBoxOBoxw->value();
290 c->filterCoeff = _ui.doubleSpinBoxOFiltCoeff->value();
291 c->torqueLimit = _ui.doubleSpinBoxOTorqueLim->value();
292 c->ftCalibrationTime = _ui.doubleSpinBoxOCalibTime->value();
293 c->nullspace = readNullspaceCFG();
294 c->admittanceObject = readAdmittanceCFG();
295 const auto f = readForceCFG();
296 c->forceLeft = f.at(0);
297 c->forceRight = f.at(1);
298 const auto i = readImpedanceCFG();
299 c->impedanceLeft = i.at(0);
300 c->impedanceRight = i.at(1);
301 return NJointControllerConfigPtr::dynamicCast(c);
302 }
303
304 std::array<Eigen::Vector3f, 2>
305 BimanualCartesianAdmittanceControllerGuiWidgetController::readPosTarg() const
306 {
307 Eigen::Vector3f xyz;
308 Eigen::Vector3f rpy;
309
310 xyz(0) = _ui.doubleSpinBoxTargTX->value();
311 xyz(1) = _ui.doubleSpinBoxTargTY->value();
312 xyz(2) = _ui.doubleSpinBoxTargTZ->value();
313
314 rpy(0) = _ui.doubleSpinBoxTargRX->value();
315 rpy(1) = _ui.doubleSpinBoxTargRY->value();
316 rpy(2) = _ui.doubleSpinBoxTargRZ->value();
317
318 return {xyz, rpy};
319 }
320
321 std::array<Eigen::Vector3f, 2>
322 BimanualCartesianAdmittanceControllerGuiWidgetController::readVelTarg() const
323 {
324 Eigen::Vector3f xyz;
325 Eigen::Vector3f rpy;
326
327 xyz(0) = _ui.doubleSpinBoxTargVTX->value();
328 xyz(1) = _ui.doubleSpinBoxTargVTY->value();
329 xyz(2) = _ui.doubleSpinBoxTargVTZ->value();
330
331 rpy(0) = _ui.doubleSpinBoxTargVRX->value();
332 rpy(1) = _ui.doubleSpinBoxTargVRY->value();
333 rpy(2) = _ui.doubleSpinBoxTargVRZ->value();
334
335 return {xyz, rpy};
336 }
337} // namespace armarx
338
339//push buttons
340namespace armarx
341{
342 void
343 BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendAll_clicked()
344 {
345 std::lock_guard g{_allMutex};
346 if (!_controller)
347 {
348 return;
349 }
350 _controller->setConfig(
351 NJointBimanualCartesianAdmittanceControllerConfigPtr::dynamicCast(readFullCFG()));
352 }
353
354 void
355 BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendForce_clicked()
356 {
357 std::lock_guard g{_allMutex};
358 if (!_controller)
359 {
360 return;
361 }
362 const auto c = readForceCFG();
363 _controller->setForceConfig(c.at(0), c.at(1));
364 }
365
366 void
367 BimanualCartesianAdmittanceControllerGuiWidgetController::
368 on_pushButtonCfgSendAdmittance_clicked()
369 {
370 std::lock_guard g{_allMutex};
371 if (!_controller)
372 {
373 return;
374 }
375 _controller->setAdmittanceConfig(readAdmittanceCFG());
376 }
377
378 void
379 BimanualCartesianAdmittanceControllerGuiWidgetController::
380 on_pushButtonCfgSendImpedance_clicked()
381 {
382 std::lock_guard g{_allMutex};
383 if (!_controller)
384 {
385 return;
386 }
387 const auto c = readImpedanceCFG();
388 _controller->setImpedanceConfig(c.at(0), c.at(1));
389 }
390
391 void
392 BimanualCartesianAdmittanceControllerGuiWidgetController::
393 on_pushButtonCfgSendNullspace_clicked()
394 {
395 std::lock_guard g{_allMutex};
396 if (!_controller)
397 {
398 return;
399 }
400 _controller->setNullspaceConfig(readNullspaceCFG());
401 }
402
403 void
404 BimanualCartesianAdmittanceControllerGuiWidgetController::
405 on_pushButtonCfgSendDefaultPose_clicked()
406 {
407 std::lock_guard g{_allMutex};
408 if (!_controller)
409 {
410 return;
411 }
412 const auto c = readDesiredJointCFG();
413 _controller->setDesiredJointValuesLeft(c.at(0));
414 _controller->setDesiredJointValuesRight(c.at(1));
415 }
416
417 void
418 BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonTargSend_clicked()
419 {
420 std::lock_guard g{_allMutex};
421 if (!_controller)
422 {
423 return;
424 }
425 const auto t = readPosTarg();
426 const auto v = readVelTarg();
427 const Eigen::Matrix4f m = simox::math::pos_rpy_to_mat4f(t.at(0), t.at(1));
428 _controller->setBoxPoseAndVelocity(m, v.at(0), v.at(1));
429 }
430
431 void
432 BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonTargAdd_clicked()
433 {
434 std::lock_guard g{_allMutex};
435 if (!_controller)
436 {
437 return;
438 }
439 const auto t = readPosTarg();
440 const Eigen::Matrix4f m = simox::math::pos_rpy_to_mat4f(t.at(0), t.at(1));
441 _controller->moveBoxPose(m);
442 }
443
444 void
445 BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonReadCurrentPose_clicked()
446 {
447 std::lock_guard g{_allMutex};
448 if (!_controller)
449 {
450 return;
451 }
452 const Eigen::Matrix4f m = _controller->getBoxPose();
453 const auto rpy = simox::math::mat4f_to_rpy(m);
454 _ui.doubleSpinBoxTargTX->setValue(m(0, 3));
455 _ui.doubleSpinBoxTargTY->setValue(m(1, 3));
456 _ui.doubleSpinBoxTargTZ->setValue(m(2, 3));
457 _ui.doubleSpinBoxTargRX->setValue(rpy(0));
458 _ui.doubleSpinBoxTargRY->setValue(rpy(1));
459 _ui.doubleSpinBoxTargRZ->setValue(rpy(2));
460 }
461} // namespace armarx
lo
#define lo(x)
Definition AbstractInterface.h:47
hi
#define hi(x)
Definition AbstractInterface.h:46
c
constexpr T c
Definition UnscentedKalmanFilterTest.cpp:46
armarx::ArmarXWidgetController::getWidget
virtual QPointer< QWidget > getWidget()
getWidget returns a pointer to the a widget of this controller.
Definition ArmarXWidgetController.cpp:54
armarx::detail::_NJointControllerGuiPluginBase::Base::_allMutex
std::recursive_mutex _allMutex
Definition NJointControllerGuiPluginBase.h:106
armarx::detail::_NJointControllerGuiPluginBase::Base::connectCreateAcivateDeactivateDelete
void connectCreateAcivateDeactivateDelete(QPushButton *cr, QPushButton *ac, QPushButton *dc, QPushButton *de)
Definition NJointControllerGuiPluginBase.cpp:44
armarx::detail::_NJointControllerGuiPluginBase::Base::_robot
VirtualRobot::RobotPtr _robot
Definition NJointControllerGuiPluginBase.h:107
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition CtrlUtil.h:39
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition ArmarXTimeserver.cpp:28
armarx::clearLayout
void clearLayout(QLayout *layout)
Definition RemoteGuiWidgetController.cpp:39