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 //read config
131 namespace armarx
132 {
133  std::array<Ice::FloatSeq, 2>
134  BimanualCartesianAdmittanceControllerGuiWidgetController::readDesiredJointCFG() const
135  {
136  return {_desiredJointValuesLeft.get<std::vector<float>>(),
137  _desiredJointValuesRight.get<std::vector<float>>()};
138  }
139  detail::NJBmanCartAdmCtrl::Nullspace
140  BimanualCartesianAdmittanceControllerGuiWidgetController::readNullspaceCFG() const
141  {
142  detail::NJBmanCartAdmCtrl::Nullspace c;
143  c.k = _ui.doubleSpinBoxNK->value();
144  c.d = _ui.doubleSpinBoxND->value();
145  const auto arms = readDesiredJointCFG();
146  c.desiredJointValuesLeft = arms.at(0);
147  c.desiredJointValuesRight = arms.at(1);
148  return c;
149  }
150  std::array<detail::NJBmanCartAdmCtrl::Impedance, 2>
151  BimanualCartesianAdmittanceControllerGuiWidgetController::readImpedanceCFG() const
152  {
153  detail::NJBmanCartAdmCtrl::Impedance l;
154  detail::NJBmanCartAdmCtrl::Impedance r;
155 
156  l.KpXYZ(0) = _ui.doubleSpinBoxIPTXL->value();
157  l.KpXYZ(1) = _ui.doubleSpinBoxIPTYL->value();
158  l.KpXYZ(2) = _ui.doubleSpinBoxIPTZL->value();
159 
160  l.KpRPY(0) = _ui.doubleSpinBoxIPRXL->value();
161  l.KpRPY(1) = _ui.doubleSpinBoxIPRYL->value();
162  l.KpRPY(2) = _ui.doubleSpinBoxIPRZL->value();
163 
164  l.KdXYZ(0) = _ui.doubleSpinBoxIDTXL->value();
165  l.KdXYZ(1) = _ui.doubleSpinBoxIDTYL->value();
166  l.KdXYZ(2) = _ui.doubleSpinBoxIDTZL->value();
167 
168  l.KdRPY(0) = _ui.doubleSpinBoxIDRXL->value();
169  l.KdRPY(1) = _ui.doubleSpinBoxIDRYL->value();
170  l.KdRPY(2) = _ui.doubleSpinBoxIDRZL->value();
171 
172  r.KpXYZ(0) = _ui.doubleSpinBoxIPTXR->value();
173  r.KpXYZ(1) = _ui.doubleSpinBoxIPTYR->value();
174  r.KpXYZ(2) = _ui.doubleSpinBoxIPTZR->value();
175 
176  r.KpRPY(0) = _ui.doubleSpinBoxIPRXR->value();
177  r.KpRPY(1) = _ui.doubleSpinBoxIPRYR->value();
178  r.KpRPY(2) = _ui.doubleSpinBoxIPRZR->value();
179 
180  r.KdXYZ(0) = _ui.doubleSpinBoxIDTXR->value();
181  r.KdXYZ(1) = _ui.doubleSpinBoxIDTYR->value();
182  r.KdXYZ(2) = _ui.doubleSpinBoxIDTZR->value();
183 
184  r.KdRPY(0) = _ui.doubleSpinBoxIDRXR->value();
185  r.KdRPY(1) = _ui.doubleSpinBoxIDRYR->value();
186  r.KdRPY(2) = _ui.doubleSpinBoxIDRZR->value();
187 
188  return {l, r};
189  }
190  std::array<detail::NJBmanCartAdmCtrl::Force, 2>
191  BimanualCartesianAdmittanceControllerGuiWidgetController::readForceCFG() const
192  {
193  detail::NJBmanCartAdmCtrl::Force l;
194  detail::NJBmanCartAdmCtrl::Force r;
195 
196  l.wrenchXYZ(0) = _ui.doubleSpinBoxFWTXL->value();
197  l.wrenchXYZ(1) = _ui.doubleSpinBoxFWTYL->value();
198  l.wrenchXYZ(2) = _ui.doubleSpinBoxFWTZL->value();
199 
200  l.wrenchRPY(0) = _ui.doubleSpinBoxFWRXL->value();
201  l.wrenchRPY(1) = _ui.doubleSpinBoxFWRYL->value();
202  l.wrenchRPY(2) = _ui.doubleSpinBoxFWRZL->value();
203 
204  l.mass = _ui.doubleSpinBoxFML->value();
205 
206  l.offsetForce(0) = _ui.doubleSpinBoxFOFXL->value();
207  l.offsetForce(1) = _ui.doubleSpinBoxFOFYL->value();
208  l.offsetForce(2) = _ui.doubleSpinBoxFOFZL->value();
209 
210  l.offsetTorque(0) = _ui.doubleSpinBoxFOTXL->value();
211  l.offsetTorque(1) = _ui.doubleSpinBoxFOTYL->value();
212  l.offsetTorque(2) = _ui.doubleSpinBoxFOTZL->value();
213 
214  l.forceThreshold(0) = _ui.doubleSpinBoxFFTXL->value();
215  l.forceThreshold(1) = _ui.doubleSpinBoxFFTYL->value();
216  l.forceThreshold(2) = _ui.doubleSpinBoxFFTZL->value();
217 
218  r.wrenchXYZ(0) = _ui.doubleSpinBoxFWTXR->value();
219  r.wrenchXYZ(1) = _ui.doubleSpinBoxFWTYR->value();
220  r.wrenchXYZ(2) = _ui.doubleSpinBoxFWTZR->value();
221 
222  r.wrenchRPY(0) = _ui.doubleSpinBoxFWRXR->value();
223  r.wrenchRPY(1) = _ui.doubleSpinBoxFWRYR->value();
224  r.wrenchRPY(2) = _ui.doubleSpinBoxFWRZR->value();
225 
226  r.mass = _ui.doubleSpinBoxFMR->value();
227 
228  r.offsetForce(0) = _ui.doubleSpinBoxFOFXR->value();
229  r.offsetForce(1) = _ui.doubleSpinBoxFOFYR->value();
230  r.offsetForce(2) = _ui.doubleSpinBoxFOFZR->value();
231 
232  r.offsetTorque(0) = _ui.doubleSpinBoxFOTXR->value();
233  r.offsetTorque(1) = _ui.doubleSpinBoxFOTYR->value();
234  r.offsetTorque(2) = _ui.doubleSpinBoxFOTZR->value();
235 
236  r.forceThreshold(0) = _ui.doubleSpinBoxFFTXR->value();
237  r.forceThreshold(1) = _ui.doubleSpinBoxFFTYR->value();
238  r.forceThreshold(2) = _ui.doubleSpinBoxFFTZR->value();
239  return {l, r};
240  }
241  detail::NJBmanCartAdmCtrl::Admittance
242  BimanualCartesianAdmittanceControllerGuiWidgetController::readAdmittanceCFG() const
243  {
244  detail::NJBmanCartAdmCtrl::Admittance c;
245 
246  c.KpXYZ(0) = _ui.doubleSpinBoxAPTX->value();
247  c.KpXYZ(1) = _ui.doubleSpinBoxAPTY->value();
248  c.KpXYZ(2) = _ui.doubleSpinBoxAPTZ->value();
249 
250  c.KpRPY(0) = _ui.doubleSpinBoxAPRX->value();
251  c.KpRPY(1) = _ui.doubleSpinBoxAPRY->value();
252  c.KpRPY(2) = _ui.doubleSpinBoxAPRZ->value();
253 
254  c.KdXYZ(0) = _ui.doubleSpinBoxADTX->value();
255  c.KdXYZ(1) = _ui.doubleSpinBoxADTY->value();
256  c.KdXYZ(2) = _ui.doubleSpinBoxADTZ->value();
257 
258  c.KdRPY(0) = _ui.doubleSpinBoxADRX->value();
259  c.KdRPY(1) = _ui.doubleSpinBoxADRY->value();
260  c.KdRPY(2) = _ui.doubleSpinBoxADRZ->value();
261 
262  c.KmXYZ(0) = _ui.doubleSpinBoxAMTX->value();
263  c.KmXYZ(1) = _ui.doubleSpinBoxAMTY->value();
264  c.KmXYZ(2) = _ui.doubleSpinBoxAMTZ->value();
265 
266  c.KmRPY(0) = _ui.doubleSpinBoxAMRX->value();
267  c.KmRPY(1) = _ui.doubleSpinBoxAMRY->value();
268  c.KmRPY(2) = _ui.doubleSpinBoxAMRZ->value();
269 
270  return c;
271  }
272  NJointControllerConfigPtr
273  BimanualCartesianAdmittanceControllerGuiWidgetController::readFullCFG() const
274  {
275  NJointBimanualCartesianAdmittanceControllerConfigPtr c =
276  new NJointBimanualCartesianAdmittanceControllerConfig;
277  c->kinematicChainRight = "RightArm";
278  c->kinematicChainLeft = "LeftArm";
279  c->ftSensorRight = "FT R";
280  c->ftSensorLeft = "FT L";
281  c->ftSensorRightFrame = "ArmR_FT";
282  c->ftSensorLeftFrame = "ArmL_FT";
283  c->box.width = _ui.doubleSpinBoxOBoxw->value();
284  c->filterCoeff = _ui.doubleSpinBoxOFiltCoeff->value();
285  c->torqueLimit = _ui.doubleSpinBoxOTorqueLim->value();
286  c->ftCalibrationTime = _ui.doubleSpinBoxOCalibTime->value();
287  c->nullspace = readNullspaceCFG();
288  c->admittanceObject = readAdmittanceCFG();
289  const auto f = readForceCFG();
290  c->forceLeft = f.at(0);
291  c->forceRight = f.at(1);
292  const auto i = readImpedanceCFG();
293  c->impedanceLeft = i.at(0);
294  c->impedanceRight = i.at(1);
295  return NJointControllerConfigPtr::dynamicCast(c);
296  }
297  std::array<Eigen::Vector3f, 2>
298  BimanualCartesianAdmittanceControllerGuiWidgetController::readPosTarg() const
299  {
300  Eigen::Vector3f xyz;
301  Eigen::Vector3f rpy;
302 
303  xyz(0) = _ui.doubleSpinBoxTargTX->value();
304  xyz(1) = _ui.doubleSpinBoxTargTY->value();
305  xyz(2) = _ui.doubleSpinBoxTargTZ->value();
306 
307  rpy(0) = _ui.doubleSpinBoxTargRX->value();
308  rpy(1) = _ui.doubleSpinBoxTargRY->value();
309  rpy(2) = _ui.doubleSpinBoxTargRZ->value();
310 
311  return {xyz, rpy};
312  }
313  std::array<Eigen::Vector3f, 2>
314  BimanualCartesianAdmittanceControllerGuiWidgetController::readVelTarg() const
315  {
316  Eigen::Vector3f xyz;
317  Eigen::Vector3f rpy;
318 
319  xyz(0) = _ui.doubleSpinBoxTargVTX->value();
320  xyz(1) = _ui.doubleSpinBoxTargVTY->value();
321  xyz(2) = _ui.doubleSpinBoxTargVTZ->value();
322 
323  rpy(0) = _ui.doubleSpinBoxTargVRX->value();
324  rpy(1) = _ui.doubleSpinBoxTargVRY->value();
325  rpy(2) = _ui.doubleSpinBoxTargVRZ->value();
326 
327  return {xyz, rpy};
328  }
329 } // namespace armarx
330 //push buttons
331 namespace armarx
332 {
333  void
334  BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendAll_clicked()
335  {
336  std::lock_guard g{_allMutex};
337  if (!_controller)
338  {
339  return;
340  }
341  _controller->setConfig(
342  NJointBimanualCartesianAdmittanceControllerConfigPtr::dynamicCast(readFullCFG()));
343  }
344 
345  void
346  BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendForce_clicked()
347  {
348  std::lock_guard g{_allMutex};
349  if (!_controller)
350  {
351  return;
352  }
353  const auto c = readForceCFG();
354  _controller->setForceConfig(c.at(0), c.at(1));
355  }
356 
357  void
358  BimanualCartesianAdmittanceControllerGuiWidgetController::
359  on_pushButtonCfgSendAdmittance_clicked()
360  {
361  std::lock_guard g{_allMutex};
362  if (!_controller)
363  {
364  return;
365  }
366  _controller->setAdmittanceConfig(readAdmittanceCFG());
367  }
368 
369  void
370  BimanualCartesianAdmittanceControllerGuiWidgetController::
371  on_pushButtonCfgSendImpedance_clicked()
372  {
373  std::lock_guard g{_allMutex};
374  if (!_controller)
375  {
376  return;
377  }
378  const auto c = readImpedanceCFG();
379  _controller->setImpedanceConfig(c.at(0), c.at(1));
380  }
381 
382  void
383  BimanualCartesianAdmittanceControllerGuiWidgetController::
384  on_pushButtonCfgSendNullspace_clicked()
385  {
386  std::lock_guard g{_allMutex};
387  if (!_controller)
388  {
389  return;
390  }
391  _controller->setNullspaceConfig(readNullspaceCFG());
392  }
393 
394  void
395  BimanualCartesianAdmittanceControllerGuiWidgetController::
396  on_pushButtonCfgSendDefaultPose_clicked()
397  {
398  std::lock_guard g{_allMutex};
399  if (!_controller)
400  {
401  return;
402  }
403  const auto c = readDesiredJointCFG();
404  _controller->setDesiredJointValuesLeft(c.at(0));
405  _controller->setDesiredJointValuesRight(c.at(1));
406  }
407 
408  void
409  BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonTargSend_clicked()
410  {
411  std::lock_guard g{_allMutex};
412  if (!_controller)
413  {
414  return;
415  }
416  const auto t = readPosTarg();
417  const auto v = readVelTarg();
418  const Eigen::Matrix4f m = simox::math::pos_rpy_to_mat4f(t.at(0), t.at(1));
419  _controller->setBoxPoseAndVelocity(m, v.at(0), v.at(1));
420  }
421 
422  void
423  BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonTargAdd_clicked()
424  {
425  std::lock_guard g{_allMutex};
426  if (!_controller)
427  {
428  return;
429  }
430  const auto t = readPosTarg();
431  const Eigen::Matrix4f m = simox::math::pos_rpy_to_mat4f(t.at(0), t.at(1));
432  _controller->moveBoxPose(m);
433  }
434 
435  void
436  BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonReadCurrentPose_clicked()
437  {
438  std::lock_guard g{_allMutex};
439  if (!_controller)
440  {
441  return;
442  }
443  const Eigen::Matrix4f m = _controller->getBoxPose();
444  const auto rpy = simox::math::mat4f_to_rpy(m);
445  _ui.doubleSpinBoxTargTX->setValue(m(0, 3));
446  _ui.doubleSpinBoxTargTY->setValue(m(1, 3));
447  _ui.doubleSpinBoxTargTZ->setValue(m(2, 3));
448  _ui.doubleSpinBoxTargRX->setValue(rpy(0));
449  _ui.doubleSpinBoxTargRY->setValue(rpy(1));
450  _ui.doubleSpinBoxTargRZ->setValue(rpy(2));
451  }
452 } // namespace armarx
armarx::BimanualCartesianAdmittanceControllerGuiWidgetController::BimanualCartesianAdmittanceControllerGuiWidgetController
BimanualCartesianAdmittanceControllerGuiWidgetController()
Definition: BimanualCartesianAdmittanceControllerGuiWidgetController.cpp:54
RemoteRobot.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::SpinBoxToVector::get
void get(Eigen::Matrix< Scalar, Rows, Cols > &m) const
Definition: SpinBoxToVector.h:45
lo
#define lo(x)
Definition: AbstractInterface.h:43
armarx::detail::_NJointControllerGuiPluginBase::Base::_allMutex
std::recursive_mutex _allMutex
Definition: NJointControllerGuiPluginBase.h:104
armarx::detail::_NJointControllerGuiPluginBase::Base::connectCreateAcivateDeactivateDelete
void connectCreateAcivateDeactivateDelete(QPushButton *cr, QPushButton *ac, QPushButton *dc, QPushButton *de)
Definition: NJointControllerGuiPluginBase.cpp:41
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
hi
#define hi(x)
Definition: AbstractInterface.h:42
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:123
T
float T
Definition: UnscentedKalmanFilterTest.cpp:35
BimanualCartesianAdmittanceControllerGuiWidgetController.h
armarx::NJointControllerGuiPluginBase
Definition: NJointControllerGuiPluginBase.h:30
armarx::detail::_NJointControllerGuiPluginBase::Base::_robot
VirtualRobot::RobotPtr _robot
Definition: NJointControllerGuiPluginBase.h:105
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::clearLayout
void clearLayout(QLayout *layout)
Clear a layout.
Definition: RemoteGuiWidgetController.cpp:38