TrajectoryController.cpp
Go to the documentation of this file.
1 #include "TrajectoryController.h"
2 #include "../Util/OrientationConversion.h"
3 #include <iostream>
4 #include <QMessageBox>
5 using namespace std;
6 
7 namespace armarx
8 {
9  void TrajectoryController::onInitComponent()
10  {
11  ARMARX_INFO << "RobotTrajectoryDesigner: TrajectoryController on init";
12  }
13 
14  void TrajectoryController::onConnectComponent()
15  {
16  ARMARX_INFO << "RobotTrajectoryDesigner: TrajectoryController on connect";
17  }
18 
19  void TrajectoryController::onDisconnectComponent()
20  {
21  ARMARX_INFO << "RobotTrajectoryDesigner: TrajectoryController on disconnect";
22  }
23 
24  void TrajectoryController::onExitComponent()
25  {
26  ARMARX_INFO << "RobotTrajectoryDesigner: TrajectoryController on exit";
27  }
28 
29  //=============================================================================================
30 
31  TrajectoryController::TrajectoryController()
32  {
33  onInitComponent();
34  onConnectComponent();
35  }
36 
37  void TrajectoryController::updateTCP(QString tcp)
38  {
39  if (environment->getRobot() == NULL || holder == NULL)
40  {
41  ARMARX_INFO << "Robot or holder is NULL";
42  return;
43  }
44  VirtualRobot::RobotNodeSetPtr helpRns = environment->getRobot()->getRobotNodeSet(tcp.toStdString());
45  if (manager != NULL && !manager->getIsInitialized())
46  {
47  holder->deleteDesignerTrajectoryManager(rns->getName());
48  emit removeTransitionWaypointGui();
49  emit removeTrajectory(QString::fromStdString(rns->getName()));
50  }
51  if (manager == NULL)
52  {
53  rns = helpRns;
54  helpCreateDesignerTrajectoryManager();
55  }
56  else if (rns == helpRns)
57  {
58  return;
59  }
60  else if (holder->rnsIsPartOfExistingRns(helpRns->getName()))
61  {
62  if (holder->rnsExists(helpRns->getName()))
63  {
64  emit removeTransitionWaypointGui();
65  manager = holder->getTrajectoryManager(helpRns->getName());
66  rns = helpRns;
67  emit rnsChanged(rns);
68  emit showTrajectory(manager->getDesignerTrajectory());
69  addTransitionWaypointGui(manager->getDesignerTrajectory());
70  emit updateSelectedTCP(QString::fromStdString(rns->getName()));
71  }
72  else
73  {
74  QMessageBox* rnsIsPartOfExistingRns = new QMessageBox;
75  rnsIsPartOfExistingRns->setWindowTitle(QString::fromStdString("Error Message"));
76  rnsIsPartOfExistingRns->setText(QString::fromStdString("Selected TCP is part of an existing TCP."));
77  rnsIsPartOfExistingRns->exec();
78  emit updateSelectedTCP(QString::fromStdString(rns->getName()));
79  }
80  }
81  else
82  {
83  emit showTrajectory(NULL);
84  emit removeTransitionWaypointGui();
85  rns = helpRns;
86  helpCreateDesignerTrajectoryManager();
87  }
88  helpEnableButtons();
89  helpEnableUndoRedo();
90  }
91 
92  void TrajectoryController::updateTransition(int transition, int it)
93  {
94  if (manager)
95  {
96  try
97  {
98  manager->setTransitionInterpolation(transition, (InterpolationType) it);
99  emit showTrajectory(manager->getDesignerTrajectory());
100  changeTransitionWaypointGui(manager->getDesignerTrajectory());
101  helpEnableUndoRedo();
102  helpCollision();
103  }
104  catch (LocalException& e)
105  {
106  helpExceptionMessageBox(e.getReason());
107  TransitionPtr transitionPtr = manager->getDesignerTrajectory()->getTransition(transition);
108  helpChangeTransitionGui(transitionPtr, transition);
109  }
110  catch (InvalidArgumentException& e)
111  {
112  helpExceptionMessageBox(e.reason);
113  TransitionPtr transitionPtr = manager->getDesignerTrajectory()->getTransition(transition);
114  helpChangeTransitionGui(transitionPtr, transition);
115  }
116  }
117  }
118 
119  void TrajectoryController::updateTransition(int transition, double duration)
120  {
121  if (manager)
122  {
123  try
124  {
125  manager->setTransitionUserDuration(transition, duration);
126  emit showTrajectory(manager->getDesignerTrajectory());
127  changeTransitionWaypointGui(manager->getDesignerTrajectory());
128  helpEnableUndoRedo();
129  helpCollision();
130  }
131  catch (InvalidArgumentException& e)
132  {
133  TransitionPtr transitionPtr = manager->getDesignerTrajectory()->getTransition(transition);
134  helpChangeTransitionGui(transitionPtr, transition);
135  }
136  }
137  }
138 
139  void TrajectoryController::updateWaypoint(int waypoint, std::vector<double> values)
140  {
141  if (manager)
142  {
143  try
144  {
145  manager->editWaypointPoseBase(waypoint, parseToPoseBasePtr(values));
146  emit showTrajectory(manager->getDesignerTrajectory());
147  changeTransitionWaypointGui(manager->getDesignerTrajectory());
148  helpEnableUndoRedo();
149  helpCollision();
150  }
151  catch (LocalException& e)
152  {
153  UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
154  helpChangeWaypointGui(waypointPtr, waypoint);
155  }
156  catch (InvalidArgumentException& e)
157  {
158  UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
159  helpChangeWaypointGui(waypointPtr, waypoint);
160  }
161  }
162  }
163 
164  void TrajectoryController::updateWaypoint(int waypoint)
165  {
166  if (manager)
167  {
168  try
169  {
170  manager->editWaypointPoseBase(waypoint, *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(rns->getTCP()->getGlobalPose()))));
171  emit showTrajectory(manager->getDesignerTrajectory());
172  changeTransitionWaypointGui(manager->getDesignerTrajectory());
173  helpEnableUndoRedo();
174  helpCollision();
175  }
176  catch (LocalException& e)
177  {
178  helpExceptionMessageBox(e.getReason());
179 
180  }
181  catch (InvalidArgumentException& e)
182  {
183  helpExceptionMessageBox(e.reason);
184  }
185  }
186  }
187 
188  void TrajectoryController::updateWaypoint(int waypoint, PoseBasePtr newPoseBase)
189  {
190  if (manager)
191  {
192  try
193  {
194  manager->editWaypointPoseBase(waypoint, newPoseBase);
195  emit showTrajectory(manager->getDesignerTrajectory());
196  changeTransitionWaypointGui(manager->getDesignerTrajectory());
197  helpEnableUndoRedo();
198  helpCollision();
199  }
200  catch (LocalException& e)
201  {
202  helpExceptionMessageBox(e.getReason());
203  UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
204  helpChangeWaypointGui(waypointPtr, waypoint);
205  }
206  catch (InvalidArgumentException& e)
207  {
208  helpExceptionMessageBox(e.reason);
209  UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
210  helpChangeWaypointGui(waypointPtr, waypoint);
211  }
212  }
213  }
214 
215  void TrajectoryController::updateWaypoint(int waypoint, int cartesianSelection)
216  {
217  if (manager)
218  {
219  try
220  {
221  manager->editWaypointIKSelection(waypoint, (VirtualRobot::IKSolver::CartesianSelection) cartesianSelection);
222  emit showTrajectory(manager->getDesignerTrajectory());
223  changeTransitionWaypointGui(manager->getDesignerTrajectory());
224  emit cartesianSelectionChanged(manager->getDesignerTrajectory()->getUserWaypoint(waypoint)->getIKSelection());
225  helpEnableUndoRedo();
226  helpCollision();
227  }
228  catch (LocalException& e)
229  {
230  helpExceptionMessageBox(e.getReason());
231  UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
232  helpChangeWaypointGui(waypointPtr, waypoint);
233  }
234  catch (InvalidArgumentException& e)
235  {
236  helpExceptionMessageBox(e.reason);
237  UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
238  helpChangeWaypointGui(waypointPtr, waypoint);
239  }
240  }
241  }
242 
243  void TrajectoryController::updateWaypoint(int waypoint, bool isBreakpoint)
244  {
245  if (manager == NULL)
246  {
247  ARMARX_INFO << "Manager is NULL";
248  return;
249  }
250  try
251  {
252  manager->setWaypointAsBreakpoint(waypoint, isBreakpoint);
253  emit showTrajectory(manager->getDesignerTrajectory());
254  changeTransitionWaypointGui(manager->getDesignerTrajectory());
255  helpEnableUndoRedo();
256  }
257  catch (LocalException& e)
258  {
259  helpExceptionMessageBox(e.getReason());
260  UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
261  helpChangeWaypointGui(waypointPtr, waypoint);
262  }
263  catch (InvalidArgumentException& e)
264  {
265  helpExceptionMessageBox(e.reason);
266  UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
267  helpChangeWaypointGui(waypointPtr, waypoint);
268  }
269  }
270 
271  void TrajectoryController::addWaypoint(int waypoint, bool insertAfter)
272  {
273  if (manager == NULL)
274  {
275  ARMARX_WARNING << "Manager is NULL";
276  return;
277  }
278  unsigned int helpWaypoint = static_cast<unsigned>(waypoint);
279  if (!manager->getIsInitialized())
280  {
281  std::vector<double> jointValues;
282  for (double newValue : rns->getJointValues())
283  {
284  jointValues.push_back((double) newValue);
285  }
286  manager->initializeDesignerTrajectory(jointValues);
287  emit showTrajectory(manager->getDesignerTrajectory());
288  helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint), waypoint);
289  emit newTrajectory(QString::fromStdString(rns->getName()));
290  emit updateSelectedTCP(QString::fromStdString(rns->getName()));
291  }
292  else if (helpWaypoint == manager->getDesignerTrajectory()->getNrOfUserWaypoints() - static_cast<unsigned>(1) && insertAfter)
293  {
294  try
295  {
296  manager->addWaypoint(*new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(rns->getTCP()->getGlobalPose()))));
297  emit showTrajectory(manager->getDesignerTrajectory());
298  helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint + 1), waypoint + 1);
299  helpAddTransitionGui(manager->getDesignerTrajectory()->getTransition(waypoint), waypoint);
300  changeTransitionWaypointGui(manager->getDesignerTrajectory());
301  helpCollision();
302  }
303  catch (LocalException& e)
304  {
305  helpExceptionMessageBox(e.getReason());
306  }
307  catch (InvalidArgumentException& e)
308  {
309  helpExceptionMessageBox(e.reason);
310  }
311  }
312  else if (helpWaypoint < manager->getDesignerTrajectory()->getNrOfUserWaypoints() - static_cast<unsigned>(1) && waypoint >= 0 && insertAfter)
313  {
314  try
315  {
316  manager->insertWaypoint(waypoint + 1, *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(rns->getTCP()->getGlobalPose()))));
317  emit showTrajectory(manager->getDesignerTrajectory());
318  helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint + 1), waypoint + 1);
319  helpAddTransitionGui(manager->getDesignerTrajectory()->getTransition(waypoint + 1), waypoint + 1);
320  changeTransitionWaypointGui(manager->getDesignerTrajectory());
321  helpCollision();
322  }
323  catch (LocalException& e)
324  {
325  helpExceptionMessageBox(e.getReason());
326  }
327  }
328  else if (helpWaypoint < manager->getDesignerTrajectory()->getNrOfUserWaypoints() && waypoint >= 0 && !insertAfter)
329  {
330  try
331  {
332  manager->insertWaypoint(waypoint, *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(rns->getTCP()->getGlobalPose()))));
333  emit showTrajectory(manager->getDesignerTrajectory());
334  helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint), waypoint);
335  helpAddTransitionGui(manager->getDesignerTrajectory()->getTransition(waypoint), waypoint);
336  changeTransitionWaypointGui(manager->getDesignerTrajectory());
337  helpCollision();
338  }
339  catch (LocalException& e)
340  {
341  helpExceptionMessageBox(e.getReason());
342  }
343  catch (InvalidArgumentException& e)
344  {
345  helpExceptionMessageBox(e.reason);
346  }
347  }
348  helpEnableButtons();
349  helpEnableUndoRedo();
350  }
351 
352  void TrajectoryController::deleteWaypoint(int waypoint)
353  {
354  if (manager == NULL)
355  {
356  ARMARX_WARNING << "Manager is NULL";
357  return;
358  }
359  if (!manager->getIsInitialized())
360  {
361  ARMARX_WARNING << "There is no designer trajectory";
362  return;
363  }
364  unsigned int helpWaypoint = static_cast<unsigned>(waypoint);
365  if (waypoint == 0 && manager->getDesignerTrajectory()->getNrOfUserWaypoints() == 1)
366  {
367  try
368  {
369  manager->deleteWaypoint(waypoint);
370  emit removeTransitionWaypointGui();
371  }
372  catch (LocalException& e)
373  {
374  helpExceptionMessageBox(e.getReason());
375  }
376  catch (InvalidArgumentException& e)
377  {
378  helpExceptionMessageBox(e.reason);
379  }
380  }
381  else if (helpWaypoint == manager->getDesignerTrajectory()->getNrOfUserWaypoints() - static_cast<unsigned>(1))
382  {
383  try
384  {
385  manager->deleteWaypoint(waypoint);
386  emit showTrajectory(manager->getDesignerTrajectory());
387  emit removeWaypointGui(waypoint);
388  emit removeTransitionGui(waypoint - 1);
389  changeTransitionWaypointGui(manager->getDesignerTrajectory());
390  helpCollision();
391  }
392  catch (LocalException& e)
393  {
394  helpExceptionMessageBox(e.getReason());
395  }
396  catch (InvalidArgumentException& e)
397  {
398  helpExceptionMessageBox(e.reason);
399  }
400  }
401  else if (waypoint >= 0 && helpWaypoint < manager->getDesignerTrajectory()->getNrOfUserWaypoints() - static_cast<unsigned>(1))
402  {
403  try
404  {
405  manager->deleteWaypoint(waypoint);
406  emit showTrajectory(manager->getDesignerTrajectory());
407  emit removeWaypointGui(waypoint);
408  emit removeTransitionGui(waypoint);
409  changeTransitionWaypointGui(manager->getDesignerTrajectory());
410  helpCollision();
411  }
412  catch (LocalException& e)
413  {
414  helpExceptionMessageBox(e.getReason());
415  }
416  catch (InvalidArgumentException& e)
417  {
418  helpExceptionMessageBox(e.reason);
419  }
420  }
421  helpEnableUndoRedo();
422  helpEnableButtons();
423  }
424 
425  void TrajectoryController::import(DesignerTrajectoryPtr designerTrajectory)
426  {
427  if (holder)
428  {
429  if (holder->rnsIsPartOfExistingRns(designerTrajectory->getRns()->getName()))
430  {
431  QMessageBox::StandardButton reply;
432  reply = QMessageBox::question(0, "Import", "Do you want to override the existing Trajectory?", QMessageBox::Yes | QMessageBox::No);
433  if (reply == QMessageBox::Yes)
434  {
435  emit removeTrajectory(QString::fromStdString(rns->getName()));
436  helpImportDesignerTrajectory(designerTrajectory);
437  }
438  else
439  {
440  QMessageBox* importFailed = new QMessageBox;
441  importFailed->setWindowTitle(QString::fromStdString("Error Message"));
442  importFailed->setText(QString::fromStdString("Import failed."));
443  importFailed->exec();
444  }
445  }
446  else
447  {
448  helpImportDesignerTrajectory(designerTrajectory);
449  }
450  //emit showTrajectory(manager->getDesignerTrajectory());
451  helpEnableButtons();
452  helpEnableUndoRedo();
453  helpCollision();
454  }
455  }
456 
457  void TrajectoryController::exportTrajectory(int fps)
458  {
459  if (manager)
460  {
461  std::vector<DesignerTrajectoryPtr> designerTrajectories;
462  for (VirtualRobot::RobotNodeSetPtr s : environment->getRobot()->getRobotNodeSets())
463  {
464  if (holder->rnsExists(s->getName()) && holder->getTrajectoryManager(s->getName())->getIsInitialized())
465  {
466  //holder->getTrajectoryManager(s)->setFPS(fps);
467  designerTrajectories.push_back(holder->getTrajectoryManager(s->getName())->getDesignerTrajectory());
468  }
469  }
470  emit exportTrajectory(designerTrajectories);
471  }
472  }
473 
474  void TrajectoryController::exportTrajectory()
475  {
476  if (manager)
477  {
478  std::vector<DesignerTrajectoryPtr> designerTrajectories;
479  for (VirtualRobot::RobotNodeSetPtr s : environment->getRobot()->getRobotNodeSets())
480  {
481  if (holder->rnsExists(s->getName()) && holder->getTrajectoryManager(s->getName())->getIsInitialized())
482  {
483  designerTrajectories.push_back(holder->getTrajectoryManager(s->getName())->getDesignerTrajectory());
484  }
485  }
486  emit exportTrajectory(designerTrajectories);
487  }
488  }
489 
490  void TrajectoryController::undo()
491  {
492  if (manager)
493  {
494  if (manager->undo())
495  {
496  emit removeTransitionWaypointGui();
497  emit showTrajectory(manager->getDesignerTrajectory());
498  addTransitionWaypointGui(manager->getDesignerTrajectory());
499  }
500  }
501  helpEnableUndoRedo();
502  helpEnableButtons();
503  helpCollision();
504  }
505 
506  void TrajectoryController::redo()
507  {
508  if (manager)
509  {
510  if (manager->redo())
511  {
512  emit removeTransitionWaypointGui();
513  emit showTrajectory(manager->getDesignerTrajectory());
514  addTransitionWaypointGui(manager->getDesignerTrajectory());
515  }
516  }
517  helpEnableUndoRedo();
518  helpEnableButtons();
519  helpCollision();
520  }
521 
522  void TrajectoryController::playTrajectories()
523  {
524  std::vector<DesignerTrajectoryPtr> designerTrajectories;
525  for (VirtualRobot::RobotNodeSetPtr s : environment->getRobot()->getRobotNodeSets())
526  {
527  if (holder->rnsExists(s->getName()))
528  {
529  designerTrajectories.push_back(holder->getTrajectoryManager(s->getName())->getDesignerTrajectory());
530  }
531  }
532  emit playTrajectories(designerTrajectories);
533  }
534 
535  void TrajectoryController::environmentChanged(EnvironmentPtr environment)
536  {
537  this->environment = environment;
538  holder = std::make_shared<DesignerTrajectoryHolder>(environment);
539  manager = NULL;
540  rns = NULL;
541  emit removeTransitionWaypointGui();
542  emit removeAllTrajectories();
543  helpEnableUndoRedo();
544  helpEnableButtons();
545  }
546 
547  void TrajectoryController::setActiveColModelName(QString activeColModelName)
548  {
549  this->activeColModelName = activeColModelName.toStdString();
550  helpCollision();
551  }
552 
553  void TrajectoryController::setBodyColModelsNames(QStringList bodyColModelsNames)
554  {
555  this->bodyColModelsNames.clear();
556  for (QString name : bodyColModelsNames)
557  {
558  this->bodyColModelsNames.push_back(name.toStdString());
559  }
560  helpCollision();
561  }
562 
563  //=============================================================================================
564  //private methods
565  //=============================================================================================
566 
567  std::vector<double> TrajectoryController::parseToCoordinate(PoseBasePtr pose)
568  {
569  float x = pose->position->x;
570  float y = pose->position->y;
571  float z = pose->position->z;
572  QuaternionBasePtr orientation = *new QuaternionBasePtr(new FramedOrientation());
573  orientation->qw = pose->orientation->qw;
574  orientation->qx = pose->orientation->qx;
575  orientation->qy = pose->orientation->qy;
576  orientation->qz = pose->orientation->qz;
577  std::vector<double> eulerAngle = OrientationConversion::toEulerAngle(orientation);
578  return {x, y, z, eulerAngle[0], eulerAngle[1], eulerAngle[2]};
579  }
580 
581  PoseBasePtr TrajectoryController::parseToPoseBasePtr(std::vector<double> values)
582  {
583  Vector3Ptr position = new Vector3(values[0], values[1], values[2]);
584  QuaternionBasePtr orientation = OrientationConversion::toQuaternion(values[3], values[4], values[5]);
585  return new FramedPose(position, orientation, "", "");
586  }
587 
588  //=============================================================================================
589 
590  void TrajectoryController::helpAddWaypointGui(UserWaypointPtr waypoint, int waypointIndex)
591  {
592  std::vector<double> values = parseToCoordinate(waypoint->getPose());
593  bool isBreakpoint = waypoint->getIsTimeOptimalBreakpoint();
594  emit addWaypointGui(waypointIndex, values, waypoint->getIKSelection(), isBreakpoint);
595  }
596 
597  void TrajectoryController::helpAddTransitionGui(TransitionPtr transition, int transitionIndex)
598  {
599  double duration = transition->getUserDuration();
600  double startTime = transition->getStart()->getUserTimestamp();
601  InterpolationType interpolation = transition->getInterpolationType();
602  emit addTransitionGui(transitionIndex, duration, startTime, interpolation);
603  }
604 
605  void TrajectoryController::addTransitionWaypointGui(DesignerTrajectoryPtr trajectory)
606  {
607  if (trajectory)
608  {
609  for (unsigned int i = 0; i < trajectory->getNrOfUserWaypoints(); i++)
610  {
611  UserWaypointPtr waypoint = trajectory->getUserWaypoint(i);
612  helpAddWaypointGui(waypoint, i);
613  }
614 
615  for (unsigned int i = 0; i < trajectory->getNrOfUserWaypoints() - 1; i++)
616  {
617  TransitionPtr transition = trajectory->getTransition(i);
618  helpAddTransitionGui(transition, i);
619  }
620  }
621  else
622  {
623  return;
624  }
625 
626  }
627 
628  //=============================================================================================
629 
630  void TrajectoryController::helpChangeTransitionGui(TransitionPtr transition, int transitionIndex)
631  {
632  double duration = transition->getUserDuration();
633  double startTime = transition->getStart()->getUserTimestamp();
634  InterpolationType interpolation = transition->getInterpolationType();
635  emit changeTransitionGui(transitionIndex, duration, startTime, interpolation);
636  }
637 
638  void TrajectoryController::helpChangeWaypointGui(UserWaypointPtr waypoint, int waypointIndex)
639  {
640  std::vector<double> values = parseToCoordinate(waypoint->getPose());
641  bool isBreakpoint = waypoint->getIsTimeOptimalBreakpoint();
642  emit changeWaypointGui(waypointIndex, values, waypoint->getIKSelection(), isBreakpoint);
643  }
644 
645  void TrajectoryController::changeTransitionWaypointGui(DesignerTrajectoryPtr trajectory)
646  {
647  for (unsigned int i = 0; i < trajectory->getNrOfUserWaypoints(); i++)
648  {
649  UserWaypointPtr waypoint = trajectory->getUserWaypoint(i);
650  helpChangeWaypointGui(waypoint, i);
651  }
652 
653  for (unsigned int i = 0; i < trajectory->getNrOfUserWaypoints() - 1; i++)
654  {
655  TransitionPtr transition = trajectory->getTransition(i);
656  helpChangeTransitionGui(transition, i);
657  }
658  }
659 
660  //=============================================================================================
661 
662  void TrajectoryController::helpImportDesignerTrajectory(DesignerTrajectoryPtr designerTrajectory)
663  {
664  removeTransitionWaypointGui();
665  holder->import(designerTrajectory, true);
666  manager = holder->getTrajectoryManager(designerTrajectory->getRns()->getName());
667  rns = manager->getDesignerTrajectory()->getRns();
668  emit rnsChanged(rns);
669  emit showTrajectory(manager->getDesignerTrajectory());
670  emit newTrajectory(QString::fromStdString(rns->getName()));
671  emit updateSelectedTCP(QString::fromStdString(rns->getName()));
672  addTransitionWaypointGui(manager->getDesignerTrajectory());
673  }
674 
675  //=============================================================================================
676 
677  void TrajectoryController::helpCreateDesignerTrajectoryManager()
678  {
679  holder->createDesignerTrajectoryManager(rns->getName());
680  manager = holder->getTrajectoryManager(rns->getName());
681  emit rnsChanged(rns);
682  }
683 
684  //=============================================================================================
685 
686  bool TrajectoryController::helpEnableExportPreviewAll()
687  {
688  bool help = false;
689  for (VirtualRobot::RobotNodeSetPtr s : environment->getRobot()->getRobotNodeSets())
690  {
691  if (holder->rnsExists(s->getName()) && holder->getTrajectoryManager(s->getName())->getIsInitialized())
692  {
693  if (holder->getTrajectoryManager(s->getName())->getDesignerTrajectory()->getNrOfUserWaypoints() > 1)
694  {
695  help = true;
696  }
697  else
698  {
699  return false;
700  }
701  }
702  }
703  return help;
704  }
705 
706  void TrajectoryController::helpEnableUndoRedo()
707  {
708  if (manager != NULL)
709  {
710  emit enableRedo(manager->redoPossible());
711  emit enableUndo(manager->undoPossible());
712  }
713  else
714  {
715  emit enableRedo(false);
716  emit enableUndo(false);
717  }
718  }
719 
720  void TrajectoryController::helpEnableButtons()
721  {
722  emit enableExportButtons(helpEnableExportPreviewAll());
723  emit enablePreviewAll(helpEnableExportPreviewAll());
724  if (manager != NULL)
725  {
726  emit enableAdd(true);
727  if (manager->getIsInitialized())
728  {
729  unsigned int nrWaypoints = manager->getDesignerTrajectory()->getNrOfUserWaypoints();
730  emit enableIKSolutionButton(false);
731  if (nrWaypoints >= 1)
732  {
733  emit enableDeleteChange(true);
734  if (nrWaypoints >= 2)
735  {
736  emit enablePreview(true);
737  }
738  else
739  {
740  emit enablePreview(false);
741  }
742  }
743  else
744  {
745  emit enableDeleteChange(false);
746  emit enablePreview(false);
747  }
748  }
749  else
750  {
751  emit enableDeleteChange(false);
752  emit enablePreview(false);
753  emit enableIKSolutionButton(true);
754  }
755  }
756  else
757  {
758  emit enableAdd(false);
759  emit enablePreview(false);
760  emit enableDeleteChange(false);
761  emit enableIKSolutionButton(false);
762  }
763  }
764 
765  void TrajectoryController::helpExceptionMessageBox(std::string errorMessage)
766  {
767  QMessageBox* errorMessageBox = new QMessageBox;
768  errorMessageBox->setWindowTitle(QString::fromStdString("Error Message"));
769  errorMessageBox->setText(QString::fromStdString(errorMessage));
770  errorMessageBox->exec();
771  }
772 
773  //=============================================================================================
774 
775  void TrajectoryController::helpCollision()
776  {
777  if (holder == NULL)
778  {
779  return;
780  }
781 
782  if (holder->isInCollision(activeColModelName, bodyColModelsNames, 100))
783  {
784  QMessageBox* changeInterpolationFailed = new QMessageBox;
785  changeInterpolationFailed->setWindowTitle(QString::fromStdString("Information"));
786  changeInterpolationFailed->setText(QString::fromStdString("A collision is detected."));
787  changeInterpolationFailed->exec();
788  }
789  }
790 }
armarx::UserWaypointPtr
std::shared_ptr< UserWaypoint > UserWaypointPtr
Definition: UserWaypoint.h:137
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:37
armarx::statechartmodel::TransitionPtr
std::shared_ptr< Transition > TransitionPtr
Definition: Transition.h:93
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
armarx::EnvironmentPtr
std::shared_ptr< Environment > EnvironmentPtr
Definition: Environment.h:29
armarx::InterpolationType
InterpolationType
The InterpolationType enum lists all available interpolation types eLinearInterpolation: represents l...
Definition: InterpolationType.h:32
armarx::NJointTaskSpaceDMPControllerMode::CartesianSelection
CartesianSelection
Definition: ControllerInterface.ice:34
armarx::TransitionPtr
std::shared_ptr< Transition > TransitionPtr
Definition: Transition.h:141
TrajectoryController.h
No
Introduction Thank you for taking interest in our work and downloading this software This library implements the algorithm described in the paper R R R Klein Efficient RANSAC for Point Cloud Shape in Computer Graphics No
Definition: ReadMe.txt:21
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:40
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::DesignerTrajectoryPtr
std::shared_ptr< DesignerTrajectory > DesignerTrajectoryPtr
Definition: DesignerTrajectory.h:165
std
Definition: Application.h:66
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
armarx::Vector3Ptr
IceInternal::Handle< Vector3 > Vector3Ptr
Definition: Pose.h:165
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
GfxTL::Yes
OnOff< true > Yes
Definition: OnOff.h:12