TCPMover.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 ArmarX::
17 * @author Mirko Waechter ( mirko.waechter at kit dot edu)
18 * @date 2012
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 #include "TCPMover.h"
24 
25 
26 //VirtualRobot
27 #include <VirtualRobot/IK/DifferentialIK.h>
28 #include <VirtualRobot/LinkedCoordinate.h>
29 #include <VirtualRobot/Nodes/RobotNode.h>
30 #include <VirtualRobot/Robot.h>
31 #include <VirtualRobot/RobotNodeSet.h>
32 #include <VirtualRobot/XML/RobotIO.h>
33 
34 #include <RobotAPI/libraries/core/FramedPose.h>
35 
36 // C++ includes
37 #include <sstream>
38 
39 // Qt includes
40 #include <QDialogButtonBox>
41 #include <QLineEdit>
42 #include <QScrollBar>
43 
44 using namespace armarx;
45 using namespace VirtualRobot;
46 
47 TCPMover::TCPMover() : robotRequested(false)
48 
49 {
50 
51  // handData[selectedTCP][0] = handData[selectedTCP][1] = handData[selectedTCP][2] = 0.0f;
52  // handData[selectedTCP][3] = handData[selectedTCP][4] = handData[selectedTCP][5] = 0.0f;
53  ui.setupUi(getWidget());
54  ui.gridLayout->setEnabled(false);
55  // SIGNALS AND SLOTS CONNECTIONS
56  connect(ui.cbselectedTCP, SIGNAL(currentIndexChanged(int)), this, SLOT(selectHand(int)));
57  connect(ui.BtnRequestControl, SIGNAL(clicked(bool)), this, SLOT(robotControl(bool)));
58  connect(ui.btnUp, SIGNAL(clicked()), this, SLOT(moveUp()));
59  connect(ui.btnDown, SIGNAL(clicked()), this, SLOT(moveDown()));
60  connect(ui.btnZUp, SIGNAL(clicked()), this, SLOT(moveZUp()));
61  connect(ui.btnZDown, SIGNAL(clicked()), this, SLOT(moveZDown()));
62  connect(ui.btnLeft, SIGNAL(clicked()), this, SLOT(moveRight()));
63  connect(ui.btnRight, SIGNAL(clicked()), this, SLOT(moveLeft()));
64  connect(ui.btnIncreaseAlpha, SIGNAL(clicked()), this, SLOT(increaseAlpha()));
65  connect(ui.btnDecreaseAlpha, SIGNAL(clicked()), this, SLOT(decreaseAlpha()));
66  connect(ui.btnIncreaseBeta, SIGNAL(clicked()), this, SLOT(increaseBeta()));
67  connect(ui.btnDecreaseBeta, SIGNAL(clicked()), this, SLOT(decreaseBeta()));
68  connect(ui.btnIncreaseGamma, SIGNAL(clicked()), this, SLOT(increaseGamma()));
69  connect(ui.btnDecreaseGamma, SIGNAL(clicked()), this, SLOT(decreaseGamma()));
70 
71  connect(ui.btnRight, SIGNAL(clicked()), this, SLOT(moveLeft()));
72  connect(ui.btnStop, SIGNAL(clicked()), this, SLOT(stopMoving()));
73  connect(ui.btnResetJoinAngles, SIGNAL(clicked()), this, SLOT(reset()));
74 }
75 
76 TCPMover::~TCPMover()
77 {
78 }
79 
80 void
81 armarx::TCPMover::loadSettings(QSettings* settings)
82 {
83  tcpMoverUnitName =
84  settings->value("TCPControlUnitName", "TCPControlUnit").toString().toStdString();
85 }
86 
87 void
88 armarx::TCPMover::saveSettings(QSettings* settings)
89 {
90  settings->setValue("TCPControlUnitName", tcpMoverUnitName.c_str());
91 }
92 
93 QPointer<QDialog>
94 TCPMover::getConfigDialog(QWidget* parent)
95 {
96  if (!configDialog)
97  {
98  configDialog = new TCPMoverConfigDialog(parent);
99  }
100 
101  return qobject_cast<TCPMoverConfigDialog*>(configDialog);
102 }
103 
104 void
105 TCPMover::configured()
106 {
107  tcpMoverUnitName = qobject_cast<TCPMoverConfigDialog*>(getConfigDialog())
108  ->proxyFinder->getSelectedProxyName()
109  .toStdString();
110 }
111 
112 void
113 armarx::TCPMover::onInitComponent()
114 {
115  // kinematicUnitFile = getProperty<std::string>("RobotFileName", KINEMATIC_UNIT_FILE_DEFAULT).getValueOrDefault();
116  // kinematicUnitName = getProperty<std::string>("RobotNodeSetName",KINEMATIC_UNIT_NAME_DEFAULT).getValueOrDefault();
117 
118  // // Load Robot
119  // ARMARX_VERBOSE << ": Loading KinematicUnit " << kinematicUnitName << " from '" << kinematicUnitFile << "' ..." << flush;
120  // robot = RobotIO::loadRobot(kinematicUnitFile);
121  // if (!robot)
122  // {
123  // ARMARX_ERROR << "Could not find Robot XML file with name '" << kinematicUnitFile << "'" << flush;
124  // }
125 
126 
127  // usingProxy(kinematicUnitName);
128  usingProxy(tcpMoverUnitName);
129  usingProxy("RobotStateComponent");
130 }
131 
132 void
133 armarx::TCPMover::onConnectComponent()
134 {
135 
136  tcpMoverUnitPrx = getProxy<TCPControlUnitInterfacePrx>(tcpMoverUnitName);
137  robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
138 
139  robotPrx = robotStateComponentPrx->getSynchronizedRobot();
140  NameList nodeSets = robotPrx->getRobotNodeSets();
141  QStringList nodeSetsQStr;
142 
143  for (unsigned int i = 0; i < nodeSets.size(); i++)
144  {
145  if (!robotPrx->getRobotNodeSet(nodeSets.at(i))->tcpName.empty())
146  {
147  tcpData[nodeSets.at(i)].resize(6, 0.f);
148  nodeSetsQStr << QString::fromStdString(nodeSets.at(i));
149  }
150  }
151 
152  QString selected = ui.cbselectedTCP->currentText();
153  ui.cbselectedTCP->clear();
154  ui.cbselectedTCP->addItems(nodeSetsQStr);
155  int index = ui.cbselectedTCP->findText(selected);
156 
157  if (index != -1)
158  {
159  ui.cbselectedTCP->setCurrentIndex(index);
160  }
161 
162  refFrame = robotStateComponentPrx->getSynchronizedRobot()->getRootNode()->getName();
163 
164  // kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
165 
166  // tcpNodeSetName = "LARM";
167  // tcpNodeSet = robot->getRobotNodeSet(tcpNodeSetName);
168  // NameControlModeMap modeMap;
169  // for(unsigned int i = 0; i < tcpNodeSet->getSize(); i++)
170  // {
171  // modeMap[tcpNodeSet->getNode(i)->getName()] = eVelocityControl;
172  // }
173  // kinematicUnitPrx->switchControlMode(modeMap);
174  // ik = DifferentialIKPtr(new DifferentialIK(tcpNodeSet));
175  ui.gridLayout->setEnabled(false);
176 }
177 
178 void
179 TCPMover::onExitComponent()
180 {
181 
182  // if(robotRequested)
183  // kinematicUnitPrx->release();
184 }
185 
186 void
187 TCPMover::moveUp()
188 {
189  // moveRelative(-1,0,0);
190  tcpData[selectedTCP][0]++;
191  execMove();
192 }
193 
194 void
195 TCPMover::moveDown()
196 {
197  // moveRelative(1,0,0);
198  tcpData[selectedTCP][0]--;
199  execMove();
200 }
201 
202 void
203 TCPMover::moveZUp()
204 {
205  tcpData[selectedTCP][2]++;
206  execMove();
207 }
208 
209 void
210 TCPMover::moveZDown()
211 {
212  tcpData[selectedTCP][2]--;
213  execMove();
214 }
215 
216 void
217 TCPMover::moveLeft()
218 {
219  // moveRelative(0,1,0);
220  tcpData[selectedTCP][1]++;
221  execMove();
222 }
223 
224 void
225 TCPMover::moveRight()
226 {
227  tcpData[selectedTCP][1]--;
228  execMove();
229 
230  // moveRelative(0,-1,0);
231 }
232 
233 void
234 TCPMover::increaseAlpha()
235 {
236  tcpData[selectedTCP].at(3) += 2.f;
237  execMove();
238 }
239 
240 void
241 TCPMover::decreaseAlpha()
242 {
243  tcpData[selectedTCP].at(3) -= 2.f;
244  execMove();
245 }
246 
247 void
248 TCPMover::increaseBeta()
249 {
250  tcpData[selectedTCP].at(4) += 2.f;
251  execMove();
252 }
253 
254 void
255 TCPMover::decreaseBeta()
256 {
257  tcpData[selectedTCP].at(4) -= 2.f;
258  execMove();
259 }
260 
261 void
262 TCPMover::increaseGamma()
263 {
264  tcpData[selectedTCP].at(5) += 2.f;
265  execMove();
266 }
267 
268 void
269 TCPMover::decreaseGamma()
270 {
271  tcpData[selectedTCP].at(5) -= 2.f;
272  execMove();
273 }
274 
275 void
276 TCPMover::stopMoving()
277 {
278  tcpData[selectedTCP][0] = tcpData[selectedTCP][1] = tcpData[selectedTCP][2] = 0.0f;
279 
280  execMove();
281 }
282 
283 void
284 TCPMover::reset()
285 {
286  tcpData[selectedTCP][0] = tcpData[selectedTCP][1] = tcpData[selectedTCP][2] = 0.0f;
287  tcpData[selectedTCP].at(3) = tcpData[selectedTCP].at(4) = tcpData[selectedTCP].at(5) = 0.0f;
288  execMove();
289  tcpMoverUnitPrx->release();
290  // tcpMoverUnitPrx->resetArmToHomePosition(eLeftHand);
291 }
292 
293 void
294 TCPMover::robotControl(bool request)
295 {
296  tcpMoverUnitPrx->request();
297  // try{
298  // if(request)
299  // kinematicUnitPrx->request();
300  // else
301  // kinematicUnitPrx->release();
302  // }catch(ResourceUnavailableException &e){
303  // ui.BtnRequestControl->setChecked(false);
304 
305  // }catch(ResourceNotOwnedException &e){
306  // ui.BtnRequestControl->setChecked(true);
307  // }
308  // robotRequested = ui.BtnRequestControl->isChecked();
309 }
310 
311 void
312 TCPMover::selectHand(int index)
313 {
314  selectedTCP = ui.cbselectedTCP->itemText(index).toStdString();
315 }
316 
317 void
318 TCPMover::execMove()
319 {
320  ARMARX_INFO << "Setting new velos and orientation";
321  ui.lblXValue->setText(QString::number(tcpData[selectedTCP][0]));
322  ui.lblYValue->setText(QString::number(tcpData[selectedTCP][1]));
323  ui.lblZValue->setText(QString::number(tcpData[selectedTCP][2]));
324  ui.lblAlphaValue->setText(QString::number(tcpData[selectedTCP].at(3)));
325  ui.lblBetaValue->setText(QString::number(tcpData[selectedTCP].at(4)));
326  ui.lblGammaValue->setText(QString::number(tcpData[selectedTCP].at(5)));
327 
328  Eigen::Vector3f vec;
329  vec << tcpData[selectedTCP][0], tcpData[selectedTCP][1], tcpData[selectedTCP][2];
330  vec *= ui.sbFactor->value();
331  const auto agentName = robotPrx->getName();
332  FramedDirectionPtr tcpVel = new FramedDirection(vec, refFrame, agentName);
333  Eigen::Vector3f vecOri;
334  vecOri << tcpData[selectedTCP].at(3) / 180.f * 3.145f,
335  tcpData[selectedTCP].at(4) / 180.f * 3.145f, tcpData[selectedTCP].at(5) / 180.f * 3.145f;
336  vecOri *= ui.sbFactor->value();
337  FramedDirectionPtr tcpOri = new FramedDirection(vecOri, refFrame, agentName);
338 
339  if (!ui.cbTranslation->isChecked())
340  {
341  tcpVel = NULL;
342  }
343 
344  if (!ui.cbOrientation->isChecked())
345  {
346  tcpOri = NULL;
347  }
348 
349  tcpMoverUnitPrx->begin_setTCPVelocity(
350  selectedTCP, ui.edtTCPName->text().toStdString(), tcpVel, tcpOri);
351 
352  // tcpMoverUnitPrx->setCartesianTCPVelocity(selectedTCP, handData[selectedTCP][0], handData[selectedTCP][1], handData[selectedTCP][2], ui.sbFactor->value());
353  // tcpMoverUnitPrx->setTCPOrientation(selectedTCP, handData[selectedTCP].at(3)/180.f*3.145f, handData[selectedTCP].at(4)/180.f*3.145f, handData[selectedTCP].at(5)/180.f*3.145f);
354 }
355 
356 void
357 TCPMover::moveRelative(float x, float y, float z)
358 {
359  // RobotNodePtr tcpNode = tcpNodeSet->getTCP();
360  // // calculate cartesian error
361  // Eigen::Matrix4f newPosRelative;
362  // newPosRelative
363  // << 1, 0, 0, 10*ui.sbFactor->value(),
364  // 0, 1, 0, 0,
365  // 0, 0, 1, 0,
366  // 0, 0, 0, 1;
367  // Eigen::VectorXf errorCartVec(3);
368  // errorCartVec << x*ui.sbFactor->value(),y*ui.sbFactor->value(),z*ui.sbFactor->value();
369  //// errorCartVec.segment(0,3) = newPosRelative.block(0,3,3,1);
370 
371 
372  // Eigen::MatrixXf Ji = ik->getPseudoInverseJacobianMatrix(tcpNode, IKSolver::Position);
373  // // calculat joint error
374  // Eigen::VectorXf errorJoint(tcpNodeSet->getSize());
375  // errorJoint = Ji * errorCartVec;
376  // std::vector<float> angles = tcpNodeSet->getJointValues();
377  // std::vector<float> targetAngles(tcpNodeSet->getSize());
378  // NameValueMap targetAnglesMap;
379 
380  // for(unsigned int i = 0; i < tcpNodeSet->getSize(); i++)
381  // {
382  //// float newAngle = angles[i] + errorJoint(i);
383  // float newAngle = errorJoint(i);
384  // targetAngles[i] = newAngle;
385  // targetAnglesMap[tcpNodeSet->getNode(i)->getName()] = newAngle;
386  // //ARMARX_VERBOSE << tcpNodeSet->getNode(i)->getName() << ": " << newAngle;
387  // }
388  // tcpNodeSet->setJointValues(targetAngles);
389  // kinematicUnitPrx->setJointVelocities(targetAnglesMap);
390 }
391 
392 /*void TCPMoverConfigDialog::setupUI(QWidget *parent)
393 {
394  this->setWindowTitle("Enter name of TCPMoverUnit");
395 
396  layout = new QGridLayout(parent);
397 
398 
399  proxyFinder->setSearchMask("*Unit");
400  layout->addWidget(proxyFinder, 0, 0, 1, 2);
401 
402  //label = new QLabel("TCPControlUnit name:", parent);
403  //layout->addWidget(label, 0,0);
404 
405  //editTCPMoverUnitName = new QLineEdit("TCPControlUnit", parent);
406  //layout->addWidget(editTCPMoverUnitName, 0, 1);
407 
408  buttonBox = new QDialogButtonBox(parent);
409  buttonBox->setObjectName(QString::fromUtf8("buttonBox"));
410  buttonBox->setOrientation(Qt::Horizontal);
411  buttonBox->setStandardButtons(QDialogButtonBox::Cancel|QDialogButtonBox::Ok);
412 
413  layout->addWidget(buttonBox, 1, 0, 1, 2);
414 
415 
416  QObject::connect(buttonBox, SIGNAL(accepted()), this, SLOT(accept()));
417  QObject::connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
418 
419 
420 
421 }
422 
423 
424 TCPMoverConfigDialog::TCPMoverConfigDialog(QWidget *parent) :
425  QDialog(parent)
426 {
427  proxyFinder = new IceProxyFinder<TCPControlUnitInterfacePrx>(this);
428  setupUI(this);
429 }*/
armarx::TCPMover::increaseGamma
void increaseGamma()
Definition: TCPMover.cpp:262
armarx::TCPMover::robotPrx
SharedRobotInterfacePrx robotPrx
Definition: TCPMover.h:156
armarx::TCPMover::increaseBeta
void increaseBeta()
Definition: TCPMover.cpp:248
armarx::TCPMover::robotControl
void robotControl(bool request)
Definition: TCPMover.cpp:294
TCPMover.h
armarx::TCPMover::execMove
void execMove()
Definition: TCPMover.cpp:318
armarx::TCPMover::decreaseGamma
void decreaseGamma()
Definition: TCPMover.cpp:269
index
uint8_t index
Definition: EtherCATFrame.h:59
VirtualRobot
Definition: FramedPose.h:42
armarx::TCPMover::configured
void configured() override
This function must be implemented by the user, if he supplies a config dialog.
Definition: TCPMover.cpp:105
armarx::TCPMover::increaseAlpha
void increaseAlpha()
Definition: TCPMover.cpp:234
armarx::TCPMover::stopMoving
void stopMoving()
Definition: TCPMover.cpp:276
armarx::TCPMover::TCPMover
TCPMover()
Definition: TCPMover.cpp:47
armarx::TCPMover::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: TCPMover.cpp:133
armarx::TCPMover::tcpMoverUnitName
std::string tcpMoverUnitName
Definition: TCPMover.h:152
armarx::TCPMover::getConfigDialog
QPointer< QDialog > getConfigDialog(QWidget *parent=0) override
getConfigDialog returns a pointer to the a configuration widget of this controller.
Definition: TCPMover.cpp:94
armarx::TCPMover::moveZUp
void moveZUp()
Definition: TCPMover.cpp:203
armarx::TCPMover::decreaseAlpha
void decreaseAlpha()
Definition: TCPMover.cpp:241
armarx::TCPMover::moveRelative
void moveRelative(float x, float y, float z)
Definition: TCPMover.cpp:357
IceInternal::Handle< FramedDirection >
armarx::TCPMover::selectHand
void selectHand(int index)
Definition: TCPMover.cpp:312
armarx::TCPMover::ui
Ui::TCPMover ui
Definition: TCPMover.h:149
FramedPose.h
armarx::TCPMover::tcpMoverUnitPrx
TCPControlUnitInterfacePrx tcpMoverUnitPrx
Definition: TCPMover.h:154
armarx::TCPMover::moveRight
void moveRight()
Definition: TCPMover.cpp:225
armarx::TCPMover::moveDown
void moveDown()
Definition: TCPMover.cpp:195
armarx::TCPMover::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: TCPMover.cpp:113
armarx::TCPMover::moveUp
void moveUp()
Definition: TCPMover.cpp:187
armarx::TCPMover::moveZDown
void moveZDown()
Definition: TCPMover.cpp:210
armarx::TCPMover::~TCPMover
~TCPMover() override
Definition: TCPMover.cpp:76
armarx::TCPMover::reset
void reset()
Definition: TCPMover.cpp:284
armarx::TCPMover::selectedTCP
std::string selectedTCP
Definition: TCPMover.h:140
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::VariantType::FramedDirection
const VariantTypeId FramedDirection
Definition: FramedPose.h:37
armarx::TCPMover::onExitComponent
void onExitComponent() override
Hook for subclass.
Definition: TCPMover.cpp:179
armarx::TCPMoverConfigDialog
Definition: TCPMoverConfigDialog.h:39
armarx::TCPMover::tcpData
std::map< std::string, std::vector< float > > tcpData
Definition: TCPMover.h:139
armarx::TCPMover::moveLeft
void moveLeft()
Definition: TCPMover.cpp:217
armarx::ArmarXWidgetController::getWidget
virtual QPointer< QWidget > getWidget()
getWidget returns a pointer to the a widget of this controller.
Definition: ArmarXWidgetController.cpp:54
armarx::TCPMover::decreaseBeta
void decreaseBeta()
Definition: TCPMover.cpp:255
armarx::TCPMover::loadSettings
void loadSettings(QSettings *settings) override
Implement to load the settings that are part of the GUI configuration.
Definition: TCPMover.cpp:81
armarx::TCPMover::saveSettings
void saveSettings(QSettings *settings) override
Implement to save the settings as part of the GUI configuration.
Definition: TCPMover.cpp:88
armarx::TCPMover::configDialog
QPointer< TCPMoverConfigDialog > configDialog
Definition: TCPMover.h:157
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::TCPMover::refFrame
std::string refFrame
Definition: TCPMover.h:141