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