icub-basic-demos
mainwindow.cpp
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Copyright (C) 2016 iCub Facility - Istituto Italiano di Tecnologia
4  * Author: Marco Randazzo <marco.randazzo@iit.it>
5  * CopyPolicy: Released under the terms of the GPLv2 or later, see GPL.TXT
6  */
7 
8 
9 #include "mainwindow.h"
10 #include "ui_mainwindow.h"
11 
12 #include <QToolBar>
13 #include <QDebug>
14 #include <QHBoxLayout>
15 #include <QScrollArea>
16 #include <QFont>
17 #include <QFontMetrics>
18 #include <QMessageBox>
19 #include <QSettings>
20 #include <QFileDialog>
21 #include <string>
22 #include "robot_interfaces.h"
23 #include <yarp/os/LogStream.h>
24 #include <yarp/dev/all.h>
25 
26 using namespace std;
27 using namespace yarp::dev;
28 
29 MainWindow::MainWindow(QWidget *parent) :
30  QMainWindow(parent), ui(new Ui::MainWindow)
31 {
32  m_robot = 0;
33  ui->setupUi(this);
34 
35  connect(ui->radioButton, SIGNAL(clicked()), this, SLOT(Radio1Selected()));
36  connect(ui->radioButton_2, SIGNAL(clicked()), this, SLOT(Radio2Selected()));
37  connect(ui->radioButton_3, SIGNAL(clicked()), this, SLOT(Radio3Selected()));
38  connect(ui->radioButton_4, SIGNAL(clicked()), this, SLOT(Radio4Selected()));
39  connect(ui->radioButton_5, SIGNAL(clicked()), this, SLOT(Radio5Selected()));
40  connect(ui->radioButton_6, SIGNAL(clicked()), this, SLOT(Radio6Selected()));
41  connect(ui->radioButton_7, SIGNAL(clicked()), this, SLOT(Radio7Selected()));
42  connect(ui->radioButton_8, SIGNAL(clicked()), this, SLOT(Radio8Selected()));
43  connect(ui->radioButton_9, SIGNAL(clicked()), this, SLOT(Radio9Selected()));
44  connect(ui->radioButton_10, SIGNAL(clicked()), this, SLOT(Radio10Selected()));
45  connect(ui->radioButton_11, SIGNAL(clicked()), this, SLOT(Radio11Selected()));
46  connect(ui->radioButton_12, SIGNAL(clicked()), this, SLOT(Radio12Selected()));
47  connect(ui->radioButton_13, SIGNAL(clicked()), this, SLOT(Radio13Selected()));
48  connect(ui->radioButton_14, SIGNAL(clicked()), this, SLOT(Radio14Selected()));
49  connect(ui->radioButton_15, SIGNAL(clicked()), this, SLOT(Radio15Selected()));
50  connect(ui->radioButton_16, SIGNAL(clicked()), this, SLOT(Radio16Selected()));
51  connect(ui->radioButton_17, SIGNAL(clicked()), this, SLOT(Radio17Selected()));
52  connect(ui->radioButton_18, SIGNAL(clicked()), this, SLOT(Radio18Selected()));
53  connect(ui->radioButton_19, SIGNAL(clicked()), this, SLOT(Radio19Selected()));
54  connect(ui->radioButton_20, SIGNAL(clicked()), this, SLOT(Radio20Selected()));
55  connect(ui->radioButton_21, SIGNAL(clicked()), this, SLOT(Radio21Selected()));
56  connect(ui->radioButton_22, SIGNAL(clicked()), this, SLOT(Radio22Selected()));
57  connect(ui->radioButton_23, SIGNAL(clicked()), this, SLOT(Radio23Selected()));
58  connect(ui->radioButton_24, SIGNAL(clicked()), this, SLOT(Radio24Selected()));
59  connect(ui->radioButton_25, SIGNAL(clicked()), this, SLOT(Radio25Selected()));
60 
61  QLocale::setDefault(QLocale::C);
62  setWindowTitle("demoForceControl");
63 }
64 
65 MainWindow::~MainWindow()
66 {
67  delete ui;
68 }
69 
70 void MainWindow::disable_left_arm()
71 {
72  ui->radioButton->setDisabled(true);
73  ui->radioButton_2->setDisabled(true);
74  ui->radioButton_3->setDisabled(true);
75  ui->radioButton_4->setDisabled(true);
76  ui->radioButton_5->setDisabled(true);
77 }
78 
79 void MainWindow::disable_right_arm()
80 {
81  ui->radioButton_6->setDisabled(true);
82  ui->radioButton_7->setDisabled(true);
83  ui->radioButton_8->setDisabled(true);
84  ui->radioButton_9->setDisabled(true);
85  ui->radioButton_10->setDisabled(true);
86 }
87 
88 void MainWindow::disable_left_leg()
89 {
90  ui->radioButton_11->setDisabled(true);
91  ui->radioButton_12->setDisabled(true);
92  ui->radioButton_13->setDisabled(true);
93  ui->radioButton_14->setDisabled(true);
94  ui->radioButton_15->setDisabled(true);
95 }
96 
97 void MainWindow::disable_right_leg()
98 {
99  ui->radioButton_16->setDisabled(true);
100  ui->radioButton_17->setDisabled(true);
101  ui->radioButton_18->setDisabled(true);
102  ui->radioButton_19->setDisabled(true);
103  ui->radioButton_20->setDisabled(true);
104 }
105 
106 void MainWindow::disable_torso()
107 {
108  ui->radioButton_21->setDisabled(true);
109  ui->radioButton_22->setDisabled(true);
110  ui->radioButton_23->setDisabled(true);
111  ui->radioButton_24->setDisabled(true);
112  ui->radioButton_25->setDisabled(true);
113 }
114 
115 bool MainWindow::slot_changed_trq(int kk)
116 {
117  if (kk<0 || kk >= 5 || !m_robot->icmd[kk])
118  {
119  yError() << "Critical fail";
120  return false;
121  }
122 
123  switch (kk)
124  {
125  case LEFT_ARM:
126  case RIGHT_ARM:
127  m_robot->iimp[kk]->setImpedance(0, 0.0, 0.0);
128  m_robot->iimp[kk]->setImpedance(1, 0.0, 0.0);
129  m_robot->iimp[kk]->setImpedance(2, 0.0, 0.0);
130  m_robot->iimp[kk]->setImpedance(3, 0.0, 0.0);
131  m_robot->iimp[kk]->setImpedance(4, 0.0, 0.0);
132  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_TORQUE);
133  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_TORQUE);
134  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_TORQUE);
135  m_robot->icmd[kk]->setControlMode(3,VOCAB_CM_TORQUE);
136  m_robot->icmd[kk]->setControlMode(4,VOCAB_CM_TORQUE);
137  break;
138  case LEFT_LEG:
139  case RIGHT_LEG:
140  m_robot->iimp[kk]->setImpedance(0, 0.0, 0.0);
141  m_robot->iimp[kk]->setImpedance(1, 0.0, 0.0);
142  m_robot->iimp[kk]->setImpedance(2, 0.0, 0.0);
143  m_robot->iimp[kk]->setImpedance(3, 0.0, 0.0);
144  m_robot->iimp[kk]->setImpedance(4, 0.0, 0.0);
145  m_robot->iimp[kk]->setImpedance(5, 0.0, 0.0);
146  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_TORQUE);
147  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_TORQUE);
148  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_TORQUE);
149  m_robot->icmd[kk]->setControlMode(3,VOCAB_CM_TORQUE);
150  m_robot->icmd[kk]->setControlMode(4,VOCAB_CM_TORQUE);
151  m_robot->icmd[kk]->setControlMode(5,VOCAB_CM_TORQUE);
152  break;
153  case TORSO:
154  m_robot->iimp[kk]->setImpedance(0, 0.0, 0.0);
155  m_robot->iimp[kk]->setImpedance(1, 0.0, 0.0);
156  m_robot->iimp[kk]->setImpedance(2, 0.1, 0.0);
157  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_TORQUE);
158  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_TORQUE);
159  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_POSITION);
160  m_robot->iint[kk]->setInteractionMode(2, VOCAB_IM_COMPLIANT);
161  break;
162  }
163  update_labels();
164  return true;
165 }
166 
167 bool MainWindow::slot_changed_imp_soft(int kk)
168 {
169  if (kk<0 || kk >= 5 || !m_robot->icmd[kk])
170  {
171  yError() << "Critical fail";
172  return false;
173  }
174 
175  switch (kk)
176  {
177  case LEFT_ARM:
178  case RIGHT_ARM:
179  m_robot->iimp[kk]->setImpedance(0,0.2,0.0);
180  m_robot->iimp[kk]->setImpedance(1,0.2,0.0);
181  m_robot->iimp[kk]->setImpedance(2,0.2,0.0);
182  m_robot->iimp[kk]->setImpedance(3,0.2,0.0);
183  m_robot->iimp[kk]->setImpedance(4,0.1,0.0);
184  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_POSITION);
185  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_POSITION);
186  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_POSITION);
187  m_robot->icmd[kk]->setControlMode(3,VOCAB_CM_POSITION);
188  m_robot->icmd[kk]->setControlMode(4,VOCAB_CM_POSITION);
189  m_robot->iint[kk]->setInteractionMode(0, VOCAB_IM_COMPLIANT);
190  m_robot->iint[kk]->setInteractionMode(1, VOCAB_IM_COMPLIANT);
191  m_robot->iint[kk]->setInteractionMode(2, VOCAB_IM_COMPLIANT);
192  m_robot->iint[kk]->setInteractionMode(3, VOCAB_IM_COMPLIANT);
193  m_robot->iint[kk]->setInteractionMode(4, VOCAB_IM_COMPLIANT);
194  break;
195  case LEFT_LEG:
196  case RIGHT_LEG:
197  m_robot->iimp[kk]->setImpedance(0,0.3,0.0);
198  m_robot->iimp[kk]->setImpedance(1,0.3,0.0);
199  m_robot->iimp[kk]->setImpedance(2,0.2,0.0);
200  m_robot->iimp[kk]->setImpedance(3,0.2,0.0);
201  m_robot->iimp[kk]->setImpedance(4,0.2,0.0);
202  m_robot->iimp[kk]->setImpedance(5,0.2,0.0);
203  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_POSITION);
204  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_POSITION);
205  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_POSITION);
206  m_robot->icmd[kk]->setControlMode(3,VOCAB_CM_POSITION);
207  m_robot->icmd[kk]->setControlMode(4,VOCAB_CM_POSITION);
208  m_robot->icmd[kk]->setControlMode(5,VOCAB_CM_POSITION);
209  m_robot->iint[kk]->setInteractionMode(0, VOCAB_IM_COMPLIANT);
210  m_robot->iint[kk]->setInteractionMode(1, VOCAB_IM_COMPLIANT);
211  m_robot->iint[kk]->setInteractionMode(2, VOCAB_IM_COMPLIANT);
212  m_robot->iint[kk]->setInteractionMode(3, VOCAB_IM_COMPLIANT);
213  m_robot->iint[kk]->setInteractionMode(4, VOCAB_IM_COMPLIANT);
214  m_robot->iint[kk]->setInteractionMode(5, VOCAB_IM_COMPLIANT);
215  break;
216  case TORSO:
217  m_robot->iimp[kk]->setImpedance(0,0.1,0.0);
218  m_robot->iimp[kk]->setImpedance(1,0.1,0.0);
219  m_robot->iimp[kk]->setImpedance(2,0.1,0.0);
220  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_POSITION);
221  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_POSITION);
222  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_POSITION);
223  m_robot->iint[kk]->setInteractionMode(0, VOCAB_IM_COMPLIANT);
224  m_robot->iint[kk]->setInteractionMode(1, VOCAB_IM_COMPLIANT);
225  m_robot->iint[kk]->setInteractionMode(2, VOCAB_IM_COMPLIANT);
226  break;
227  }
228 
229  update_labels();
230  return true;
231 }
232 
233 
234 bool MainWindow::slot_changed_imp_medium(int kk)
235 {
236  if (kk<0 || kk >= 5 || !m_robot->icmd[kk])
237  {
238  yError() << "Critical fail";
239  return false;
240  }
241 
242  switch (kk)
243  {
244  case LEFT_ARM:
245  case RIGHT_ARM:
246  m_robot->iimp[kk]->setImpedance(0,0.4,0.03);
247  m_robot->iimp[kk]->setImpedance(1,0.4,0.03);
248  m_robot->iimp[kk]->setImpedance(2,0.4,0.03);
249  m_robot->iimp[kk]->setImpedance(3,0.2,0.01);
250  m_robot->iimp[kk]->setImpedance(4,0.2,0.00);
251  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_POSITION);
252  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_POSITION);
253  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_POSITION);
254  m_robot->icmd[kk]->setControlMode(3,VOCAB_CM_POSITION);
255  m_robot->icmd[kk]->setControlMode(4,VOCAB_CM_POSITION);
256  m_robot->iint[kk]->setInteractionMode(0, VOCAB_IM_COMPLIANT);
257  m_robot->iint[kk]->setInteractionMode(1, VOCAB_IM_COMPLIANT);
258  m_robot->iint[kk]->setInteractionMode(2, VOCAB_IM_COMPLIANT);
259  m_robot->iint[kk]->setInteractionMode(3, VOCAB_IM_COMPLIANT);
260  m_robot->iint[kk]->setInteractionMode(4, VOCAB_IM_COMPLIANT);
261  break;
262  case LEFT_LEG:
263  case RIGHT_LEG:
264  m_robot->iimp[kk]->setImpedance(0,0.6,0.01);
265  m_robot->iimp[kk]->setImpedance(1,0.6,0.01);
266  m_robot->iimp[kk]->setImpedance(2,0.4,0.01);
267  m_robot->iimp[kk]->setImpedance(3,0.4,0.01);
268  m_robot->iimp[kk]->setImpedance(4,0.4,0.01);
269  m_robot->iimp[kk]->setImpedance(5,0.4,0.01);
270  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_POSITION);
271  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_POSITION);
272  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_POSITION);
273  m_robot->icmd[kk]->setControlMode(3,VOCAB_CM_POSITION);
274  m_robot->icmd[kk]->setControlMode(4,VOCAB_CM_POSITION);
275  m_robot->icmd[kk]->setControlMode(5,VOCAB_CM_POSITION);
276  m_robot->iint[kk]->setInteractionMode(0, VOCAB_IM_COMPLIANT);
277  m_robot->iint[kk]->setInteractionMode(1, VOCAB_IM_COMPLIANT);
278  m_robot->iint[kk]->setInteractionMode(2, VOCAB_IM_COMPLIANT);
279  m_robot->iint[kk]->setInteractionMode(3, VOCAB_IM_COMPLIANT);
280  m_robot->iint[kk]->setInteractionMode(4, VOCAB_IM_COMPLIANT);
281  m_robot->iint[kk]->setInteractionMode(5, VOCAB_IM_COMPLIANT);
282  break;
283  case TORSO:
284  m_robot->iimp[kk]->setImpedance(0,0.3,0.0);
285  m_robot->iimp[kk]->setImpedance(1,0.3,0.0);
286  m_robot->iimp[kk]->setImpedance(2,0.3,0.0);
287  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_POSITION);
288  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_POSITION);
289  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_POSITION);
290  m_robot->iint[kk]->setInteractionMode(0, VOCAB_IM_COMPLIANT);
291  m_robot->iint[kk]->setInteractionMode(1, VOCAB_IM_COMPLIANT);
292  m_robot->iint[kk]->setInteractionMode(2, VOCAB_IM_COMPLIANT);
293  break;
294  }
295 
296  update_labels();
297  return true;
298 }
299 
300 
301 
302 bool MainWindow::slot_changed_imp_hard(int kk)
303 {
304  if (kk<0 || kk >= 5 || !m_robot->icmd[kk])
305  {
306  yError() << "Critical fail";
307  return false;
308  }
309 
310  switch (kk)
311  {
312  case LEFT_ARM:
313  case RIGHT_ARM:
314  m_robot->iimp[kk]->setImpedance(0,0.6,0.06);
315  m_robot->iimp[kk]->setImpedance(1,0.6,0.06);
316  m_robot->iimp[kk]->setImpedance(2,0.6,0.06);
317  m_robot->iimp[kk]->setImpedance(3,0.3,0.02);
318  m_robot->iimp[kk]->setImpedance(4,0.2,0.00);
319  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_POSITION);
320  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_POSITION);
321  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_POSITION);
322  m_robot->icmd[kk]->setControlMode(3,VOCAB_CM_POSITION);
323  m_robot->icmd[kk]->setControlMode(4,VOCAB_CM_POSITION);
324  m_robot->iint[kk]->setInteractionMode(0, VOCAB_IM_COMPLIANT);
325  m_robot->iint[kk]->setInteractionMode(1, VOCAB_IM_COMPLIANT);
326  m_robot->iint[kk]->setInteractionMode(2, VOCAB_IM_COMPLIANT);
327  m_robot->iint[kk]->setInteractionMode(3, VOCAB_IM_COMPLIANT);
328  m_robot->iint[kk]->setInteractionMode(4, VOCAB_IM_COMPLIANT);
329  break;
330  case LEFT_LEG:
331  case RIGHT_LEG:
332  m_robot->iimp[kk]->setImpedance(0,1.0,0.02);
333  m_robot->iimp[kk]->setImpedance(1,1.0,0.02);
334  m_robot->iimp[kk]->setImpedance(2,0.7,0.02);
335  m_robot->iimp[kk]->setImpedance(3,0.6,0.02);
336  m_robot->iimp[kk]->setImpedance(4,0.6,0.02);
337  m_robot->iimp[kk]->setImpedance(5,0.6,0.02);
338  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_POSITION);
339  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_POSITION);
340  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_POSITION);
341  m_robot->icmd[kk]->setControlMode(3,VOCAB_CM_POSITION);
342  m_robot->icmd[kk]->setControlMode(4,VOCAB_CM_POSITION);
343  m_robot->icmd[kk]->setControlMode(5,VOCAB_CM_POSITION);
344  m_robot->iint[kk]->setInteractionMode(0, VOCAB_IM_COMPLIANT);
345  m_robot->iint[kk]->setInteractionMode(1, VOCAB_IM_COMPLIANT);
346  m_robot->iint[kk]->setInteractionMode(2, VOCAB_IM_COMPLIANT);
347  m_robot->iint[kk]->setInteractionMode(3, VOCAB_IM_COMPLIANT);
348  m_robot->iint[kk]->setInteractionMode(4, VOCAB_IM_COMPLIANT);
349  m_robot->iint[kk]->setInteractionMode(5, VOCAB_IM_COMPLIANT);
350  break;
351  case TORSO:
352  m_robot->iimp[kk]->setImpedance(0,0.7,0.015);
353  m_robot->iimp[kk]->setImpedance(1,0.7,0.015);
354  m_robot->iimp[kk]->setImpedance(2,0.7,0.015);
355  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_POSITION);
356  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_POSITION);
357  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_POSITION);
358  m_robot->iint[kk]->setInteractionMode(0, VOCAB_IM_COMPLIANT);
359  m_robot->iint[kk]->setInteractionMode(1, VOCAB_IM_COMPLIANT);
360  m_robot->iint[kk]->setInteractionMode(2, VOCAB_IM_COMPLIANT);
361  break;
362  }
363 
364  update_labels();
365  return true;
366 }
367 
368 bool MainWindow::slot_changed_pos(int kk)
369 {
370  if (kk<0 || kk>=5 || !m_robot->icmd[kk])
371  {
372  yError() << "Critical fail";
373  return false;
374  }
375 
376  switch (kk)
377  {
378  case LEFT_ARM:
379  case RIGHT_ARM:
380  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_POSITION);
381  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_POSITION);
382  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_POSITION);
383  m_robot->icmd[kk]->setControlMode(3,VOCAB_CM_POSITION);
384  m_robot->icmd[kk]->setControlMode(4,VOCAB_CM_POSITION);
385  m_robot->iint[kk]->setInteractionMode(0, VOCAB_IM_STIFF);
386  m_robot->iint[kk]->setInteractionMode(1, VOCAB_IM_STIFF);
387  m_robot->iint[kk]->setInteractionMode(2, VOCAB_IM_STIFF);
388  m_robot->iint[kk]->setInteractionMode(3, VOCAB_IM_STIFF);
389  m_robot->iint[kk]->setInteractionMode(4, VOCAB_IM_STIFF);
390  break;
391  case LEFT_LEG:
392  case RIGHT_LEG:
393  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_POSITION);
394  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_POSITION);
395  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_POSITION);
396  m_robot->icmd[kk]->setControlMode(3,VOCAB_CM_POSITION);
397  m_robot->icmd[kk]->setControlMode(4,VOCAB_CM_POSITION);
398  m_robot->icmd[kk]->setControlMode(5,VOCAB_CM_POSITION);
399  m_robot->iint[kk]->setInteractionMode(0, VOCAB_IM_STIFF);
400  m_robot->iint[kk]->setInteractionMode(1, VOCAB_IM_STIFF);
401  m_robot->iint[kk]->setInteractionMode(2, VOCAB_IM_STIFF);
402  m_robot->iint[kk]->setInteractionMode(3, VOCAB_IM_STIFF);
403  m_robot->iint[kk]->setInteractionMode(4, VOCAB_IM_STIFF);
404  m_robot->iint[kk]->setInteractionMode(5, VOCAB_IM_STIFF);
405  break;
406  case TORSO:
407  m_robot->icmd[kk]->setControlMode(0,VOCAB_CM_POSITION);
408  m_robot->icmd[kk]->setControlMode(1,VOCAB_CM_POSITION);
409  m_robot->icmd[kk]->setControlMode(2,VOCAB_CM_POSITION);
410  m_robot->iint[kk]->setInteractionMode(0, VOCAB_IM_STIFF);
411  m_robot->iint[kk]->setInteractionMode(1, VOCAB_IM_STIFF);
412  m_robot->iint[kk]->setInteractionMode(2, VOCAB_IM_STIFF);
413  break;
414  default:
415  yError() << "Critical error";
416  return false;
417  break;
418  }
419 
420  update_labels();
421  return true;
422 }
423 
424 bool MainWindow::init(QString robotName, ResourceFinder *finder, robot_interfaces *robot)
425 {
426  m_robot = robot;
427 
428  if (m_robot->dd[LEFT_ARM] == 0)
429  {
430  disable_left_arm();
431  }
432  else
433  {
434  ui->radioButton->setChecked(true);
435  slot_changed_pos(LEFT_ARM);
436  }
437 
438  if (m_robot->dd[RIGHT_ARM] == 0)
439  {
440  disable_right_arm();
441  }
442  else
443  {
444  ui->radioButton_6->setChecked(true);
445  slot_changed_pos(RIGHT_ARM);
446  }
447 
448  if (m_robot->dd[LEFT_LEG] == 0)
449  {
450  disable_left_leg();
451  }
452  else
453  {
454  ui->radioButton_11->setChecked(true);
455  slot_changed_pos(LEFT_LEG);
456  }
457 
458  if (m_robot->dd[RIGHT_LEG] == 0)
459  {
460  disable_right_leg();
461  }
462  else
463  {
464  ui->radioButton_16->setChecked(true);
465  slot_changed_pos(RIGHT_LEG);
466  }
467 
468  if (m_robot->dd[TORSO] == 0)
469  {
470  disable_torso();
471  }
472  else
473  {
474  ui->radioButton_21->setChecked(true);
475  slot_changed_pos(TORSO);
476  }
477 
478  return true;
479 }
480 
481 void MainWindow::closeEvent(QCloseEvent *event)
482 {
483  this->setVisible(false);
484  QMainWindow::closeEvent(event);
485 }
486 
487 void MainWindow::update_labels()
488 {
489  QLabel* ql[5];
490  ql[0] = ui->label1;
491  ql[1] = ui->label2;
492  ql[2] = ui->label3;
493  ql[3] = ui->label4;
494  ql[4] = ui->label5;
495 
496  char buff[255];
497  for (int i = 0; i<5; i++)
498  {
499  int jmax = 0;
500  std::string txt;
501  switch (i)
502  {
503  case LEFT_ARM:
504  case RIGHT_ARM:
505  jmax = 5;
506  break;
507  case LEFT_LEG:
508  case RIGHT_LEG:
509  jmax = 6;
510  break;
511  case TORSO:
512  jmax = 3;
513  break;
514  }
515  for (int j = 0; j<jmax; j++)
516  {
517  double stiff = 0;
518  double damp = 0;
519  sprintf(buff, "\n \nJ%d:\n", j); txt += std::string(buff);
520  if (m_robot->iimp[i]) m_robot->iimp[i]->getImpedance(j, &stiff, &damp);
521  sprintf(buff, "stiff: %3.3f Nm/deg \n", stiff); txt += std::string(buff);
522  sprintf(buff, "damp: %3.3f Nm/(deg/s) \n", damp); txt += std::string(buff);
523  }
524  ql[i]->setText(QString(txt.c_str()));
525  }
526 }
Generated by doxygen 1.9.1