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 
32 namespace 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
132 namespace 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
340 namespace 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
armarx::BimanualCartesianAdmittanceControllerGuiWidgetController::BimanualCartesianAdmittanceControllerGuiWidgetController
BimanualCartesianAdmittanceControllerGuiWidgetController()
Definition: BimanualCartesianAdmittanceControllerGuiWidgetController.cpp:54
RemoteRobot.h
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::SpinBoxToVector::get
void get(Eigen::Matrix< Scalar, Rows, Cols > &m) const
Definition: SpinBoxToVector.h:46
lo
#define lo(x)
Definition: AbstractInterface.h:47
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::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
hi
#define hi(x)
Definition: AbstractInterface.h:46
armarx::ArmarXWidgetController::getWidget
virtual QPointer< QWidget > getWidget()
getWidget returns a pointer to the a widget of this controller.
Definition: ArmarXWidgetController.cpp:54
armarx::NJointControllerGuiPluginBase< BimanualCartesianAdmittanceControllerGuiWidgetController, NJointBimanualCartesianAdmittanceControllerInterfacePrx >::_controller
NJointBimanualCartesianAdmittanceControllerInterfacePrx _controller
Definition: NJointControllerGuiPluginBase.h:129
T
float T
Definition: UnscentedKalmanFilterTest.cpp:38
BimanualCartesianAdmittanceControllerGuiWidgetController.h
armarx::NJointControllerGuiPluginBase
Definition: NJointControllerGuiPluginBase.h:30
armarx::detail::_NJointControllerGuiPluginBase::Base::_robot
VirtualRobot::RobotPtr _robot
Definition: NJointControllerGuiPluginBase.h:107
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::clearLayout
void clearLayout(QLayout *layout)
Clear a layout.
Definition: RemoteGuiWidgetController.cpp:39