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