24 #include "VirtualRobot/IK/IKSolver.h"
31 ARMARX_INFO <<
"RobotTrajectoryDesigner: WaypointController on init";
36 initValidator(-10000.000, 10000.000, 3);
39 this->guiWaypointTab->getWaypointTab()->waypointList->setSelectionMode(
40 QAbstractItemView::SingleSelection);
41 this->guiWaypointTab->getWaypointTab()->waypointList->clear();
47 ARMARX_INFO <<
"RobotTrajectoryDesigner: WaypointController on connect";
50 QObject::connect(guiWaypointTab->getWaypointTab()->waypointList,
51 SIGNAL(itemClicked(QListWidgetItem*)),
56 QObject::connect(guiWaypointTab->getWaypointTab()->xPositionLineEdit,
57 SIGNAL(editingFinished()),
59 SLOT(setXCoordinate()));
62 QObject::connect(guiWaypointTab->getWaypointTab()->yPositionLineEdit,
63 SIGNAL(editingFinished()),
65 SLOT(setYCoordinate()));
68 QObject::connect(guiWaypointTab->getWaypointTab()->zPositionLineEdit,
69 SIGNAL(editingFinished()),
71 SLOT(setZCoordinate()));
74 QObject::connect(guiWaypointTab->getWaypointTab()->eulerRLineEdit,
75 SIGNAL(editingFinished()),
77 SLOT(setEulerAngleR()));
80 QObject::connect(guiWaypointTab->getWaypointTab()->eulerPLineEdit,
81 SIGNAL(editingFinished()),
83 SLOT(setEulerAngleP()));
86 QObject::connect(guiWaypointTab->getWaypointTab()->eulerYLineEdit,
87 SIGNAL(editingFinished()),
89 SLOT(setEulerAngleY()));
92 QObject::connect(guiWaypointTab->getWaypointTab()->ikSelectionComboBox,
93 SIGNAL(activated(
int)),
95 SLOT(setCartesianSelection(
int)));
98 QObject::connect(guiWaypointTab->getWaypointTab()->isBreakpointCheckBox,
99 SIGNAL(clicked(
bool)),
101 SLOT(setBreakpoint(
bool)));
104 QObject::connect(guiWaypointTab->getWaypointTab()->insertButton,
110 QObject::connect(guiWaypointTab->getWaypointTab()->deleteButton,
119 ARMARX_INFO <<
"RobotTrajectoryDesigner: WaypointController on disconnect";
125 ARMARX_INFO <<
"RobotTrajectoryDesigner: WaypointController on exit";
129 guiWaypointTab(guiWaypointTab)
138 return this->guiWaypointTab;
144 if (guiWaypointTab != NULL)
146 this->guiWaypointTab = guiWaypointTab;
151 WaypointController::addWaypoint(
int index,
152 std::vector<double>
values,
153 int cartesianSelection,
166 data.setValue(waypoint);
169 QListWidgetItem* item =
171 item->setData(Qt::UserRole,
data);
172 QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
174 waypoints->insertItem(
index, item);
178 if (waypoints->count() == 1)
183 changeTextListWidgetItems();
187 WaypointController::removeWaypoint(
int index)
189 QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
191 if (waypoints->count() >
index)
193 delete waypoints->takeItem(
index);
196 if (waypoints->count() == 0)
202 changeTextListWidgetItems();
211 updateWaypointElements(waypoint);
214 int row = guiWaypointTab->getWaypointTab()->waypointList->row(item);
215 guiWaypointTab->getWaypointTab()->waypointList->setCurrentRow(row);
224 QListWidgetItem* item = guiWaypointTab->getWaypointTab()->waypointList->item(
index);
228 updateWaypointElements(waypoint);
231 guiWaypointTab->getWaypointTab()->waypointList->setCurrentRow(
index);
237 WaypointController::setXCoordinate()
240 if (guiWaypointTab->getWaypointTab()->xPositionLineEdit->hasAcceptableInput())
242 QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
243 QListWidgetItem* item = waypoints->currentItem();
249 double x = guiWaypointTab->getWaypointTab()->xPositionLineEdit->text().toDouble();
252 if ((waypoint.
values.size() == 6) && (
x != waypoint.
values[0]))
262 (
new QErrorMessage(0))->showMessage(
"Invalid input.");
267 WaypointController::setYCoordinate()
270 if (guiWaypointTab->getWaypointTab()->yPositionLineEdit->hasAcceptableInput())
272 QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
273 QListWidgetItem* item = waypoints->currentItem();
279 double y = guiWaypointTab->getWaypointTab()->yPositionLineEdit->text().toDouble();
282 if ((waypoint.values.size() == 6) && (y != waypoint.values[1]))
284 std::vector<double>
values = waypoint.values;
292 (
new QErrorMessage(0))->showMessage(
"Invalid input.");
297 WaypointController::setZCoordinate()
300 if (guiWaypointTab->getWaypointTab()->zPositionLineEdit->hasAcceptableInput())
302 QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
303 QListWidgetItem* item = waypoints->currentItem();
309 double z = guiWaypointTab->getWaypointTab()->xPositionLineEdit->text().toDouble();
312 if ((waypoint.values.size() == 6) && (z != waypoint.values[2]))
314 std::vector<double>
values = waypoint.values;
322 (
new QErrorMessage(0))->showMessage(
"Invalid Input.");
327 WaypointController::setEulerAngleR()
330 if (guiWaypointTab->getWaypointTab()->eulerRLineEdit->hasAcceptableInput())
332 QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
333 QListWidgetItem* item = waypoints->currentItem();
339 double r = guiWaypointTab->getWaypointTab()->eulerRLineEdit->text().toDouble();
342 if ((waypoint.values.size() == 6) && (r != waypoint.values[3]))
344 std::vector<double>
values = waypoint.values;
352 (
new QErrorMessage(0))->showMessage(
"Invalid Input.");
357 WaypointController::setEulerAngleP()
360 if (guiWaypointTab->getWaypointTab()->eulerPLineEdit->hasAcceptableInput())
362 QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
363 QListWidgetItem* item = waypoints->currentItem();
369 double p = guiWaypointTab->getWaypointTab()->eulerPLineEdit->text().toDouble();
372 if ((waypoint.values.size() == 6) && (p != waypoint.values[4]))
374 std::vector<double>
values = waypoint.values;
382 (
new QErrorMessage(0))->showMessage(
"Invalid Input.");
387 WaypointController::setEulerAngleY()
390 if (guiWaypointTab->getWaypointTab()->eulerYLineEdit->hasAcceptableInput())
392 QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
393 QListWidgetItem* item = waypoints->currentItem();
399 double y = guiWaypointTab->getWaypointTab()->eulerYLineEdit->text().toDouble();
402 if ((waypoint.values.size() == 6) && (y != waypoint.values[5]))
404 std::vector<double>
values = waypoint.values;
412 (
new QErrorMessage(0))->showMessage(
"Invalid Input.");
417 WaypointController::setCartesianSelection(
int cs)
419 QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
420 QListWidgetItem* item = waypoints->currentItem();
428 int cartesianSelection = guiWaypointTab->getWaypointTab()
429 ->ikSelectionComboBox->itemData(cs, Qt::UserRole)
431 if (cartesianSelection != waypoint.cartesianSelection)
439 WaypointController::setBreakpoint(
bool isBreakpoint)
441 QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
442 QListWidgetItem* item = waypoints->currentItem();
450 if (isBreakpoint != waypoint.isBreakpoint)
459 std::vector<double>
values,
460 int cartesianSelection,
469 if (waypoint.
values.size() == 6)
473 data.setValue(waypoint);
475 QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
476 QListWidgetItem* item = waypoints->item(
index);
479 item->setData(Qt::UserRole,
data);
481 if (waypoints->currentRow() ==
index)
483 updateWaypointElements(waypoint);
489 WaypointController::addWaypoint()
491 if (guiWaypointTab->getWaypointTab()->waypointList->count() == 0)
493 emit
addedWaypoint(guiWaypointTab->getWaypointTab()->waypointList->count(),
494 guiWaypointTab->getWaypointTab()->insertAfterButton->isChecked());
496 else if (guiWaypointTab->getWaypointTab()->waypointList->currentRow() == -1)
498 emit
addedWaypoint(guiWaypointTab->getWaypointTab()->waypointList->count() - 1,
499 guiWaypointTab->getWaypointTab()->insertAfterButton->isChecked());
503 emit
addedWaypoint(guiWaypointTab->getWaypointTab()->waypointList->currentRow(),
504 guiWaypointTab->getWaypointTab()->insertAfterButton->isChecked());
509 WaypointController::removeWaypoint()
511 QListWidget* waypoints = this->guiWaypointTab->getWaypointTab()->waypointList;
512 if (waypoints->count() > 0)
514 int index = waypoints->currentRow();
529 this->guiWaypointTab->getWaypointTab()->waypointList->clear();
531 this->guiWaypointTab->getWaypointTab()->xPositionLineEdit->clear();
532 this->guiWaypointTab->getWaypointTab()->yPositionLineEdit->clear();
533 this->guiWaypointTab->getWaypointTab()->zPositionLineEdit->clear();
534 this->guiWaypointTab->getWaypointTab()->eulerRLineEdit->clear();
535 this->guiWaypointTab->getWaypointTab()->eulerPLineEdit->clear();
536 this->guiWaypointTab->getWaypointTab()->eulerYLineEdit->clear();
538 this->guiWaypointTab->getWaypointTab()->ikSelectionComboBox->setCurrentIndex(5);
540 this->guiWaypointTab->getWaypointTab()->isBreakpointCheckBox->setChecked(
false);
542 this->guiWaypointTab->getWaypointTab()->insertAfterButton->setChecked(
true);
543 this->guiWaypointTab->getWaypointTab()->insertBeforeButton->setChecked(
false);
551 guiWaypointTab->getWaypointTab()->deleteButton->setEnabled(enable);
552 guiWaypointTab->getWaypointTab()->ikSelectionComboBox->setEnabled(enable);
553 guiWaypointTab->getWaypointTab()->isBreakpointCheckBox->setEnabled(enable);
559 guiWaypointTab->getWaypointTab()->insertButton->setEnabled(enable);
560 guiWaypointTab->getWaypointTab()->insertAfterButton->setEnabled(enable);
561 guiWaypointTab->getWaypointTab()->insertBeforeButton->setEnabled(enable);
567 guiWaypointTab->getWaypointTab()->eulerPLineEdit->setEnabled(enable);
568 guiWaypointTab->getWaypointTab()->eulerRLineEdit->setEnabled(enable);
569 guiWaypointTab->getWaypointTab()->eulerYLineEdit->setEnabled(enable);
570 guiWaypointTab->getWaypointTab()->waypointList->setEnabled(enable);
571 guiWaypointTab->getWaypointTab()->xPositionLineEdit->setEnabled(enable);
572 guiWaypointTab->getWaypointTab()->yPositionLineEdit->setEnabled(enable);
573 guiWaypointTab->getWaypointTab()->zPositionLineEdit->setEnabled(enable);
580 WaypointController::initCSComboBox()
582 QComboBox* cs = guiWaypointTab->getWaypointTab()->ikSelectionComboBox;
585 cs->setFocusPolicy(Qt::StrongFocus);
590 QVariant xpos(1), ypos(2), zpos(4), position(7), orientation(8),
all(15);
591 cs->addItem(QString::fromStdString(
"X Position"), xpos);
592 cs->addItem(QString::fromStdString(
"Y Position"), ypos);
593 cs->addItem(QString::fromStdString(
"Z Position"), zpos);
594 cs->addItem(QString::fromStdString(
"Position"), position);
595 cs->addItem(QString::fromStdString(
"Orientation"), orientation);
596 cs->addItem(QString::fromStdString(
"Position and Orientation"),
all);
597 cs->setEnabled(
true);
598 cs->setCurrentIndex(5);
602 WaypointController::initValidator(
double bottom,
double top,
int decimals)
604 QDoubleValidator* validator =
605 new QDoubleValidator(bottom, top, decimals, guiWaypointTab.get());
606 validator->setNotation(QDoubleValidator::StandardNotation);
608 this->guiWaypointTab->getWaypointTab()->xPositionLineEdit->setValidator(validator);
609 this->guiWaypointTab->getWaypointTab()->yPositionLineEdit->setValidator(validator);
610 this->guiWaypointTab->getWaypointTab()->zPositionLineEdit->setValidator(validator);
611 this->guiWaypointTab->getWaypointTab()->eulerRLineEdit->setValidator(validator);
612 this->guiWaypointTab->getWaypointTab()->eulerPLineEdit->setValidator(validator);
613 this->guiWaypointTab->getWaypointTab()->eulerYLineEdit->setValidator(validator);
617 WaypointController::updateWaypointElements(
GuiWaypoint waypoint)
619 if (waypoint.values.size() == 6)
622 guiWaypointTab->getWaypointTab()->xPositionLineEdit->setText(
623 QString::number(waypoint.values[0]));
625 guiWaypointTab->getWaypointTab()->yPositionLineEdit->setText(
626 QString::number(waypoint.values[1]));
628 guiWaypointTab->getWaypointTab()->zPositionLineEdit->setText(
629 QString::number(waypoint.values[2]));
631 guiWaypointTab->getWaypointTab()->eulerRLineEdit->setText(
632 QString::number(waypoint.values[3]));
634 guiWaypointTab->getWaypointTab()->eulerPLineEdit->setText(
635 QString::number(waypoint.values[4]));
637 guiWaypointTab->getWaypointTab()->eulerYLineEdit->setText(
638 QString::number(waypoint.values[5]));
640 guiWaypointTab->getWaypointTab()->ikSelectionComboBox->setCurrentIndex(
641 guiWaypointTab->getWaypointTab()->ikSelectionComboBox->findData(
642 QVariant(waypoint.cartesianSelection)));
644 guiWaypointTab->getWaypointTab()->isBreakpointCheckBox->setChecked(
645 waypoint.isBreakpoint);
650 WaypointController::changeTextListWidgetItems()
652 QListWidget* waypoints = this->guiWaypointTab->getWaypointTab()->waypointList;
653 for (
int i = 0; i < waypoints->count(); i++)
655 waypoints->item(i)->setText(QString::fromStdString((
"Waypoint: " +
std::to_string(i))));