From 193d3fef67cb3ffa3fa4756bdcca15a12f319adb Mon Sep 17 00:00:00 2001 From: janEbert Date: Tue, 6 Feb 2018 17:57:45 +0100 Subject: [PATCH 01/16] TaskPanel: separate widget classes TaskPanel, TaskView, TaskSettings, ... --- .../motion_planning_tasks/src/CMakeLists.txt | 6 +- .../motion_planning_tasks/src/task_panel.cpp | 129 ++++++----- .../motion_planning_tasks/src/task_panel.h | 27 ++- .../motion_planning_tasks/src/task_panel.ui | 213 +++--------------- .../motion_planning_tasks/src/task_panel_p.h | 29 ++- .../src/task_settings.ui | 69 ++++++ .../motion_planning_tasks/src/task_view.ui | 150 ++++++++++++ 7 files changed, 379 insertions(+), 244 deletions(-) create mode 100644 visualization/motion_planning_tasks/src/task_settings.ui create mode 100644 visualization/motion_planning_tasks/src/task_view.ui diff --git a/visualization/motion_planning_tasks/src/CMakeLists.txt b/visualization/motion_planning_tasks/src/CMakeLists.txt index 830b58c1..e0032d77 100644 --- a/visualization/motion_planning_tasks/src/CMakeLists.txt +++ b/visualization/motion_planning_tasks/src/CMakeLists.txt @@ -1,6 +1,10 @@ set(MOVEIT_LIB_NAME motion_planning_tasks_rviz_plugin) -qt_wrap_ui(UIC_FILES task_panel.ui) +qt_wrap_ui(UIC_FILES + task_panel.ui + task_view.ui + task_settings.ui +) add_library(${MOVEIT_LIB_NAME} factory_model.cpp diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 18160db4..31ff4b5c 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -79,13 +79,15 @@ TaskPanel::TaskPanel(QWidget* parent) : rviz::Panel(parent), d_ptr(new TaskPanelPrivate(this)) { Q_D(TaskPanel); - // connect signals - connect(d->actionRemoveTaskTreeRows, SIGNAL(triggered()), this, SLOT(removeSelectedStages())); - connect(d->actionAddLocalTask, SIGNAL(triggered()), this, SLOT(addTask())); - connect(d->button_show_stage_dock_widget, SIGNAL(clicked()), this, SLOT(showStageDockWidget())); - connect(d->tasks_view->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)), - this, SLOT(onCurrentStageChanged(QModelIndex,QModelIndex))); + d->tasks_widget = new TaskView(this); + d->settings_widget = new TaskSettings(this); + layout()->addWidget(d->tasks_widget); + layout()->addWidget(d->settings_widget); + + connect(d->button_show_stage_dock_widget, SIGNAL(clicked()), this, SLOT(showStageDockWidget())); + connect(d->button_show_settings, SIGNAL(toggled(bool)), d->settings_widget, SLOT(setVisible(bool))); + d->settings_widget->setVisible(d->button_show_settings->isChecked()); // if still undefined, this becomes the global instance if (TaskPanelPrivate::global_instance_.isNull()) { @@ -127,14 +129,41 @@ void TaskPanel::decUseCount() TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr) : q_ptr(q_ptr) - , settings(new rviz::PropertyTreeModel(new rviz::Property, q_ptr)) { setupUi(q_ptr); - // init tasks view + button_show_stage_dock_widget->setEnabled(bool(getStageFactory())); +} + +void TaskPanel::onInitialize() +{ + d_ptr->window_manager_ = vis_manager_->getWindowManager(); +} + +void TaskPanel::save(rviz::Config config) const +{ + rviz::Panel::save(config); +} + +void TaskPanel::load(const rviz::Config& config) +{ + rviz::Panel::load(config); +} + +void TaskPanel::showStageDockWidget() +{ + rviz::PanelDockWidget *dock = getStageDockWidget(d_ptr->window_manager_); + if (dock) dock->show(); +} + + +TaskViewPrivate::TaskViewPrivate(TaskView *q_ptr) + : q_ptr(q_ptr) +{ + setupUi(q_ptr); + MetaTaskListModel *meta_model = &MetaTaskListModel::instance(); StageFactoryPtr factory = getStageFactory(); if (factory) meta_model->setMimeTypes( { factory->mimeType() } ); - else button_show_stage_dock_widget->setEnabled(false); tasks_view->setModel(meta_model); tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection); @@ -148,30 +177,23 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr) // init actions tasks_view->addActions({actionRemoveTaskTreeRows, actionAddLocalTask}); - - initSettings(settings->getRoot()); - settings_view->setModel(settings); -} - -void TaskPanelPrivate::initSettings(rviz::Property* root) -{ } std::pair -TaskPanelPrivate::getTaskListModel(const QModelIndex &index) const +TaskViewPrivate::getTaskListModel(const QModelIndex &index) const { auto *meta_model = static_cast(tasks_view->model()); return meta_model->getTaskListModel(index); } std::pair -TaskPanelPrivate::getTaskModel(const QModelIndex &index) const +TaskViewPrivate::getTaskModel(const QModelIndex &index) const { auto *meta_model = static_cast(tasks_view->model()); return meta_model->getTaskModel(index); } -void TaskPanelPrivate::unlock(TaskDisplay* display) +void TaskViewPrivate::unlock(TaskDisplay* display) { if (locked_display_ && locked_display_ != display) { locked_display_->clearMarkers(); @@ -180,56 +202,38 @@ void TaskPanelPrivate::unlock(TaskDisplay* display) } } -void TaskPanel::onInitialize() +TaskView::TaskView(QWidget *parent) + : QWidget(parent), d_ptr(new TaskViewPrivate(this)) { - d_ptr->window_manager_ = vis_manager_->getWindowManager(); + Q_D(TaskView); + + // connect signals + connect(d->actionRemoveTaskTreeRows, SIGNAL(triggered()), this, SLOT(removeSelectedStages())); + connect(d->actionAddLocalTask, SIGNAL(triggered()), this, SLOT(addTask())); + + connect(d->tasks_view->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)), + this, SLOT(onCurrentStageChanged(QModelIndex,QModelIndex))); + } -void TaskPanel::save(rviz::Config config) const -{ - rviz::Panel::save(config); - d_ptr->settings->getRoot()->save(config); -} - -void TaskPanel::load(const rviz::Config& config) -{ - rviz::Panel::load(config); - d_ptr->settings->getRoot()->load(config); -} - -void TaskPanel::addTask() +void TaskView::addTask() { TaskListModel* task_list_model = nullptr; QModelIndex current = d_ptr->tasks_view->currentIndex(); - if (!current.isValid()) { - // create new TaskDisplay - TaskDisplay *display = new TaskDisplay(); - display->setName("Motion Planning Task"); - vis_manager_->getRootDisplayGroup()->addDisplay(display); - display->initialize(vis_manager_); - display->setEnabled(true); - - task_list_model = &display->getTaskListModel(); - } else + if (current.isValid()) { task_list_model = d_ptr->getTaskListModel(current).first; - - task_list_model->insertModel(new LocalTaskModel(task_list_model), current.row()); + task_list_model->insertModel(new LocalTaskModel(task_list_model), current.row()); + } } -void TaskPanel::showStageDockWidget() -{ - rviz::PanelDockWidget *dock = getStageDockWidget(d_ptr->window_manager_); - if (dock) dock->show(); -} - -void TaskPanel::removeSelectedStages() +void TaskView::removeSelectedStages() { auto *m = d_ptr->tasks_view->model(); for (const auto &range : d_ptr->tasks_view->selectionModel()->selection()) m->removeRows(range.top(), range.bottom()-range.top()+1, range.parent()); } -void TaskPanel::onCurrentStageChanged(const QModelIndex ¤t, const QModelIndex &previous) +void TaskView::onCurrentStageChanged(const QModelIndex ¤t, const QModelIndex &previous) { BaseTaskModel *task; QModelIndex task_index; @@ -255,7 +259,7 @@ void TaskPanel::onCurrentStageChanged(const QModelIndex ¤t, const QModelIn } } -void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QModelIndex &previous) +void TaskView::onCurrentSolutionChanged(const QModelIndex ¤t, const QModelIndex &previous) { TaskDisplay *display = d_ptr->getTaskListModel(d_ptr->tasks_view->currentIndex()).second; d_ptr->unlock(display); @@ -274,7 +278,7 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QMode vis->showTrajectory(solution, true); } -void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected) +void TaskView::onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected) { QItemSelectionModel *sm = d_ptr->solutions_view->selectionModel(); const QModelIndexList& selected_rows = sm->selectedRows(); @@ -292,6 +296,19 @@ void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const } } + +TaskSettingsPrivate::TaskSettingsPrivate(TaskSettings *q_ptr) + : q_ptr(q_ptr) +{ + setupUi(q_ptr); +} + +TaskSettings::TaskSettings(QWidget *parent) + : QWidget(parent), d_ptr(new TaskSettingsPrivate(this)) +{ + Q_D(TaskSettings); +} + } #include "moc_task_panel.cpp" diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index ace98e0d..845712f9 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -82,9 +82,23 @@ public: void load(const rviz::Config& config) override; void save(rviz::Config config) const override; +protected Q_SLOTS: + void showStageDockWidget(); +}; + + +class MetaTaskListModel; +class TaskViewPrivate; +class TaskView : public QWidget { + Q_OBJECT + Q_DECLARE_PRIVATE(TaskView) + TaskViewPrivate *d_ptr; + +public: + TaskView(QWidget* parent = 0); + public Q_SLOTS: void addTask(); - void showStageDockWidget(); protected Q_SLOTS: void removeSelectedStages(); @@ -93,4 +107,15 @@ protected Q_SLOTS: void onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected); }; + +class TaskSettingsPrivate; +class TaskSettings : public QWidget { + Q_OBJECT + Q_DECLARE_PRIVATE(TaskSettings) + TaskSettingsPrivate *d_ptr; + +public: + TaskSettings(QWidget* parent = 0); +}; + } diff --git a/visualization/motion_planning_tasks/src/task_panel.ui b/visualization/motion_planning_tasks/src/task_panel.ui index 7fa9a4d6..8d178229 100644 --- a/visualization/motion_planning_tasks/src/task_panel.ui +++ b/visualization/motion_planning_tasks/src/task_panel.ui @@ -14,9 +14,6 @@ Tasks Panel - - 0 - 0 @@ -30,193 +27,49 @@ 0 - - - 0 - - - - - :/icons/tasks.png:/icons/tasks.png - - - - - - - 0 + + + + + ... - - 0 + + + :/icons/settings.png:/icons/settings.png - - 0 + + true - - 0 + + + + + + Qt::Horizontal - - 0 + + + 40 + 20 + - - - - 6 - - - - - Task Tree - - - - - - - - 18 - 18 - - - - - - - - :/icons/new-stage.png:/icons/new-stage.png - - - - - - - - - - 0 - 0 - - - - Qt::Horizontal - - - - - 2 - 0 - - - - Qt::ActionsContextMenu - - - false - - - - - - 1 - 0 - - - - QAbstractItemView::ExtendedSelection - - - false - - - true - - - true - - - false - - - - - - - - - - :/icons/settings.png:/icons/settings.png - - - - - - - 0 + + + + + + ... - - 0 + + + :/icons/new-stage.png:/icons/new-stage.png - - 0 - - - 0 - - - 0 - - - - - Settings - - - - - - - - - + + + - - - Remove - - - Remove selected rows - - - Del - - - Qt::WidgetShortcut - - - - - Add task - - - - - rviz::PropertyTreeWidget - QTreeView -
rviz/properties/property_tree_widget.h
-
- - moveit_rviz_plugin::TaskListView - QTreeView -
task_list_model.h
-
- - moveit_rviz_plugin::AutoAdjustingTreeView - QTreeView -
task_list_model.h
-
-
diff --git a/visualization/motion_planning_tasks/src/task_panel_p.h b/visualization/motion_planning_tasks/src/task_panel_p.h index af56ae55..0dcd8a85 100644 --- a/visualization/motion_planning_tasks/src/task_panel_p.h +++ b/visualization/motion_planning_tasks/src/task_panel_p.h @@ -40,6 +40,8 @@ #include "task_panel.h" #include "ui_task_panel.h" +#include "ui_task_view.h" +#include "ui_task_settings.h" #include #include @@ -55,7 +57,19 @@ class TaskPanelPrivate : public Ui_TaskPanel { public: TaskPanelPrivate(TaskPanel *q_ptr); - void initSettings(rviz::Property *root); + TaskPanel* q_ptr; + TaskView* tasks_widget; + TaskSettings* settings_widget; + + rviz::WindowManagerInterface* window_manager_; + static QPointer global_instance_; + static uint global_use_count_; +}; + + +class TaskViewPrivate : public Ui_TaskView { +public: + TaskViewPrivate(TaskView *q_ptr); /// retrieve TaskListModel corresponding to given index inline std::pair @@ -68,13 +82,16 @@ public: /// unlock locked_display_ if given display is different void unlock(TaskDisplay *display); - TaskPanel* q_ptr; - rviz::PropertyTreeModel* settings; + TaskView *q_ptr; QPointer locked_display_; - rviz::WindowManagerInterface* window_manager_; +}; - static QPointer global_instance_; - static uint global_use_count_; + +class TaskSettingsPrivate : public Ui_TaskSettings { +public: + TaskSettingsPrivate(TaskSettings *q_ptr); + + TaskSettings *q_ptr; }; } diff --git a/visualization/motion_planning_tasks/src/task_settings.ui b/visualization/motion_planning_tasks/src/task_settings.ui new file mode 100644 index 00000000..3a76384c --- /dev/null +++ b/visualization/motion_planning_tasks/src/task_settings.ui @@ -0,0 +1,69 @@ + + + moveit_rviz_plugin::TaskSettings + + + + 0 + 0 + 400 + 300 + + + + Settings + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + 0 + + + 0 + + + 0 + + + 0 + + + 0 + + + + + Settings + + + + + + + + + + + + + rviz::PropertyTreeWidget + QTreeView +
rviz/properties/property_tree_widget.h
+
+
+ + +
diff --git a/visualization/motion_planning_tasks/src/task_view.ui b/visualization/motion_planning_tasks/src/task_view.ui new file mode 100644 index 00000000..fe25c1d4 --- /dev/null +++ b/visualization/motion_planning_tasks/src/task_view.ui @@ -0,0 +1,150 @@ + + + moveit_rviz_plugin::TaskView + + + + 0 + 0 + 400 + 300 + + + + Tasks Panel + + + + 0 + + + 0 + + + 0 + + + 0 + + + 0 + + + + + 0 + + + 0 + + + 0 + + + 0 + + + 0 + + + + + 6 + + + + + Task Tree + + + + + + + + + + 0 + 0 + + + + Qt::Horizontal + + + + + 2 + 0 + + + + Qt::ActionsContextMenu + + + false + + + + + + 1 + 0 + + + + QAbstractItemView::ExtendedSelection + + + false + + + true + + + true + + + false + + + + + + + + + + Remove + + + Remove selected rows + + + Del + + + Qt::WidgetShortcut + + + + + Add task + + + + + + moveit_rviz_plugin::TaskListView + QTreeView +
task_list_model.h
+
+ + moveit_rviz_plugin::AutoAdjustingTreeView + QTreeView +
task_list_model.h
+
+
+ + +
From 52904f65a2500b219c901414d4edd2d7106a322e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 6 Feb 2018 22:13:56 +0100 Subject: [PATCH 02/16] bug fixes - correctly keep display's Tasks property in sync with panel (insertion position didn't match to panel) - disable actions depending on context --- .../src/task_display.cpp | 2 +- .../motion_planning_tasks/src/task_panel.cpp | 36 ++++++++++++------- .../motion_planning_tasks/src/task_panel_p.h | 2 +- 3 files changed, 25 insertions(+), 15 deletions(-) diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 73149d6c..ac8e63fb 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -264,7 +264,7 @@ void TaskDisplay::onTasksInserted(const QModelIndex &parent, int first, int last TaskListModel* m = static_cast(sender()); for (; first <= last; ++first) { QModelIndex idx = m->index(first, 0, parent); - tasks_property_->addChild(new rviz::Property(idx.data().toString(), idx.sibling(idx.row(), 1).data())); + tasks_property_->addChild(new rviz::Property(idx.data().toString(), idx.sibling(idx.row(), 1).data()), first); } } diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 31ff4b5c..5a2e6a3b 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -176,7 +176,7 @@ TaskViewPrivate::TaskViewPrivate(TaskView *q_ptr) solutions_view->setStretchSection(2); // init actions - tasks_view->addActions({actionRemoveTaskTreeRows, actionAddLocalTask}); + tasks_view->addActions({actionAddLocalTask, actionRemoveTaskTreeRows}); } std::pair @@ -193,13 +193,13 @@ TaskViewPrivate::getTaskModel(const QModelIndex &index) const return meta_model->getTaskModel(index); } -void TaskViewPrivate::unlock(TaskDisplay* display) +void TaskViewPrivate::lock(TaskDisplay* display) { if (locked_display_ && locked_display_ != display) { locked_display_->clearMarkers(); locked_display_->visualization()->unlock(); - locked_display_ = display; } + locked_display_ = display; } TaskView::TaskView(QWidget *parent) @@ -214,16 +214,23 @@ TaskView::TaskView(QWidget *parent) connect(d->tasks_view->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)), this, SLOT(onCurrentStageChanged(QModelIndex,QModelIndex))); + onCurrentStageChanged(d->tasks_view->currentIndex(), QModelIndex()); } void TaskView::addTask() { - TaskListModel* task_list_model = nullptr; QModelIndex current = d_ptr->tasks_view->currentIndex(); - if (current.isValid()) { - task_list_model = d_ptr->getTaskListModel(current).first; - task_list_model->insertModel(new LocalTaskModel(task_list_model), current.row()); - } + if (!current.isValid()) return; + bool is_top_level = !current.parent().isValid(); + + TaskListModel* task_list_model = d_ptr->getTaskListModel(current).first; + task_list_model->insertModel(new LocalTaskModel(task_list_model), is_top_level ? -1 : current.row()); + + // select and edit newly inserted model + if (is_top_level) current = current.model()->index(task_list_model->rowCount()-1, 0, current); + d_ptr->tasks_view->scrollTo(current); + d_ptr->tasks_view->setCurrentIndex(current); + d_ptr->tasks_view->edit(current); } void TaskView::removeSelectedStages() @@ -235,13 +242,17 @@ void TaskView::removeSelectedStages() void TaskView::onCurrentStageChanged(const QModelIndex ¤t, const QModelIndex &previous) { + // adding task is allowed on top-level items and sub-top-level items + d_ptr->actionAddLocalTask->setEnabled(current.isValid() && + (!current.parent().isValid() || !current.parent().parent().isValid())); + // removing stuff is allowed if there is any selection / any curren item + d_ptr->actionRemoveTaskTreeRows->setEnabled(current.isValid()); + BaseTaskModel *task; QModelIndex task_index; std::tie(task, task_index) = d_ptr->getTaskModel(current); - d_ptr->actionRemoveTaskTreeRows->setEnabled(task != nullptr); - // TaskDisplay *display = d_ptr->getTaskListModel(d_ptr->tasks_view->currentIndex()).second; - d_ptr->unlock(nullptr); + d_ptr->lock(nullptr); // unlocks any locked_display_ auto *view = d_ptr->solutions_view; QItemSelectionModel *sm = view->selectionModel(); @@ -262,7 +273,7 @@ void TaskView::onCurrentStageChanged(const QModelIndex ¤t, const QModelInd void TaskView::onCurrentSolutionChanged(const QModelIndex ¤t, const QModelIndex &previous) { TaskDisplay *display = d_ptr->getTaskListModel(d_ptr->tasks_view->currentIndex()).second; - d_ptr->unlock(display); + d_ptr->lock(display); if (!display || !current.isValid()) return; @@ -270,7 +281,6 @@ void TaskView::onCurrentSolutionChanged(const QModelIndex ¤t, const QModel BaseTaskModel *task = d_ptr->getTaskModel(d_ptr->tasks_view->currentIndex()).first; Q_ASSERT(task); - d_ptr->locked_display_ = display; TaskSolutionVisualization* vis = display->visualization(); const DisplaySolutionPtr& solution = task->getSolution(current); display->setSolutionStatus(bool(solution)); diff --git a/visualization/motion_planning_tasks/src/task_panel_p.h b/visualization/motion_planning_tasks/src/task_panel_p.h index 0dcd8a85..3cba49b6 100644 --- a/visualization/motion_planning_tasks/src/task_panel_p.h +++ b/visualization/motion_planning_tasks/src/task_panel_p.h @@ -80,7 +80,7 @@ public: getTaskModel(const QModelIndex& index) const; /// unlock locked_display_ if given display is different - void unlock(TaskDisplay *display); + void lock(TaskDisplay *display); TaskView *q_ptr; QPointer locked_display_; From 050a4c3f861e1b4b39b3abe010d198cc007ac286 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 6 Feb 2018 23:12:52 +0100 Subject: [PATCH 03/16] basic framework to display stage properties --- .../motion_planning_tasks/CMakeLists.txt | 1 + .../properties/CMakeLists.txt | 17 ++ .../properties/property_factory.cpp | 100 ++++++++++++ .../properties/property_factory.h | 84 ++++++++++ .../motion_planning_tasks/src/CMakeLists.txt | 2 +- .../src/local_task_model.cpp | 11 ++ .../src/local_task_model.h | 3 + .../src/remote_task_model.cpp | 11 ++ .../src/remote_task_model.h | 2 + .../src/task_list_model.h | 6 + .../motion_planning_tasks/src/task_panel.cpp | 10 +- .../motion_planning_tasks/src/task_view.ui | 151 +++++++++--------- 12 files changed, 324 insertions(+), 74 deletions(-) create mode 100644 visualization/motion_planning_tasks/properties/CMakeLists.txt create mode 100644 visualization/motion_planning_tasks/properties/property_factory.cpp create mode 100644 visualization/motion_planning_tasks/properties/property_factory.h diff --git a/visualization/motion_planning_tasks/CMakeLists.txt b/visualization/motion_planning_tasks/CMakeLists.txt index b64a811b..633ca748 100644 --- a/visualization/motion_planning_tasks/CMakeLists.txt +++ b/visualization/motion_planning_tasks/CMakeLists.txt @@ -1,3 +1,4 @@ add_subdirectory(utils) +add_subdirectory(properties) add_subdirectory(src) add_subdirectory(test) diff --git a/visualization/motion_planning_tasks/properties/CMakeLists.txt b/visualization/motion_planning_tasks/properties/CMakeLists.txt new file mode 100644 index 00000000..4986363c --- /dev/null +++ b/visualization/motion_planning_tasks/properties/CMakeLists.txt @@ -0,0 +1,17 @@ +set(MOVEIT_LIB_NAME motion_planning_tasks_properties) + +set(SOURCES + property_factory.cpp +) +add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES}) + +target_link_libraries(${MOVEIT_LIB_NAME} + ${QT_LIBRARIES} +) +target_include_directories(${MOVEIT_LIB_NAME} + PUBLIC $ +) + +install(TARGETS ${MOVEIT_LIB_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp new file mode 100644 index 00000000..ed5bc290 --- /dev/null +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -0,0 +1,100 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Robert Haschke */ + +#include "property_factory.h" +#include +#include + +#include +#include +#include + +using namespace moveit::task_constructor; + +namespace moveit_rviz_plugin { + +/// TODO: We also need to provide methods to sync both properties in both directions. +/// In our Property we could store a callback function to update the rviz::Property. +/// The rviz::Property can simply use a lambda-function slot to update our Property. +/// However, we need to avoid infinite loops in doing so. Our Property cannot compare values... +template +RVIZProp* helper(const QString& name, Property* prop) { + T value = prop->defined() ? boost::any_cast(prop->value()) : T(); + return new RVIZProp(name, value, QString::fromStdString(prop->description())); +} + +PropertyFactory::PropertyFactory() +{ + // registe some standard types + registerType(&helper); + registerType(&helper); +} + +PropertyFactory& PropertyFactory::instance() +{ + static PropertyFactory instance_; + return instance_; +} + +void PropertyFactory::registerType(const std::string &type_name, const FactoryFunction &f) +{ + registry_.insert(std::make_pair(type_name, f)); +} + +rviz::Property* PropertyFactory::create(const std::string& prop_name, Property* prop) const +{ + auto it = registry_.find(prop->typeName()); + if (it == registry_.end()) return nullptr; + return it->second(QString::fromStdString(prop_name), prop); +} + +rviz::PropertyTreeModel* createPropertyTreeModel(PropertyMap& properties, QObject* parent) { + PropertyFactory& factory = PropertyFactory::instance(); + + rviz::Property* root = new rviz::Property(); + rviz::PropertyTreeModel *model = new rviz::PropertyTreeModel(root, parent); + for (auto& prop : properties) { + rviz::Property* rviz_prop = factory.create(prop.first, &prop.second); + if (!rviz_prop) rviz_prop = new rviz::Property(QString::fromStdString(prop.first)); + rviz_prop->setParent(root); + } + // just to see something, when no properties are defined + if (model->rowCount() == 0) + new rviz::Property("no properties", QVariant(), QString(), root); + return model; +} + +} diff --git a/visualization/motion_planning_tasks/properties/property_factory.h b/visualization/motion_planning_tasks/properties/property_factory.h new file mode 100644 index 00000000..6176445b --- /dev/null +++ b/visualization/motion_planning_tasks/properties/property_factory.h @@ -0,0 +1,84 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Robert Haschke */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace rviz { +class Property; +class PropertyTreeModel; +} +namespace moveit { namespace task_constructor { +class Property; +class PropertyMap; +} } + +namespace moveit_rviz_plugin { + +class PropertyFactory +{ +public: + static PropertyFactory& instance(); + + typedef std::function FactoryFunction; + + /// register a new factory function for type T + template + void registerType(const FactoryFunction& f) { registerType(std::type_index(typeid(T)).name(), f); } + + /// retrieve rviz property for given task_constructor property + rviz::Property* create(const std::string &prop_name, moveit::task_constructor::Property *prop) const; + +private: + std::map registry_; + + /// class is singleton + PropertyFactory(); + PropertyFactory(const PropertyFactory&) = delete; + void operator=(const PropertyFactory&) = delete; + + void registerType(const std::string& type_name, const FactoryFunction& f); +}; + +/// turn a PropertyMap into an rviz::PropertyTreeModel +rviz::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::PropertyMap &properties, QObject *parent = nullptr); + +} diff --git a/visualization/motion_planning_tasks/src/CMakeLists.txt b/visualization/motion_planning_tasks/src/CMakeLists.txt index e0032d77..c64d6114 100644 --- a/visualization/motion_planning_tasks/src/CMakeLists.txt +++ b/visualization/motion_planning_tasks/src/CMakeLists.txt @@ -26,7 +26,7 @@ add_library(${MOVEIT_LIB_NAME} set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} - motion_planning_tasks_utils moveit_task_visualization_tools + motion_planning_tasks_utils motion_planning_tasks_properties moveit_task_visualization_tools ${catkin_LIBRARIES} ${QT_LIBRARIES} ) target_include_directories(${MOVEIT_LIB_NAME} diff --git a/visualization/motion_planning_tasks/src/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index a5821631..b756d1c6 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.cpp +++ b/visualization/motion_planning_tasks/src/local_task_model.cpp @@ -36,6 +36,7 @@ #include "local_task_model.h" #include "factory_model.h" +#include "properties/property_factory.h" #include #include @@ -232,4 +233,14 @@ DisplaySolutionPtr LocalTaskModel::getSolution(const QModelIndex &index) return DisplaySolutionPtr(); } +rviz::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex &index) +{ + Node *n = node(index); + if (!n) return nullptr; + auto it_inserted = properties_.insert(std::make_pair(n, nullptr)); + if (it_inserted.second) // newly inserted, create new model + it_inserted.first->second = createPropertyTreeModel(n->me()->properties(), this); + return it_inserted.first->second; +} + } diff --git a/visualization/motion_planning_tasks/src/local_task_model.h b/visualization/motion_planning_tasks/src/local_task_model.h index 9e9290c3..b9873a80 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.h +++ b/visualization/motion_planning_tasks/src/local_task_model.h @@ -49,6 +49,7 @@ class LocalTaskModel typedef moveit::task_constructor::StagePrivate Node; Node *root_; StageFactoryPtr stage_factory_; + std::map properties_; inline Node* node(const QModelIndex &index) const; QModelIndex index(Node *n) const; @@ -73,6 +74,8 @@ public: QAbstractItemModel* getSolutionModel(const QModelIndex& index) override; DisplaySolutionPtr getSolution(const QModelIndex &index) override; + + rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; }; } diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 0b200282..59a14801 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -40,6 +40,8 @@ #include #include #include +#include +#include #include #include @@ -64,9 +66,11 @@ struct RemoteTaskModel::Node { InterfaceFlags interface_flags_; NodeFlags node_flags_; std::unique_ptr solutions_; + std::unique_ptr properties_; inline Node(Node *parent) : parent_(parent) { solutions_.reset(new RemoteSolutionModel()); + properties_.reset(new rviz::PropertyTreeModel(new rviz::Property())); } bool setName(const QString& name) { @@ -369,6 +373,13 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index) return it->second; } +rviz::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex &index) +{ + Node *n = node(index); + if (!n) return nullptr; + return n->properties_.get(); +} + namespace detail { // method used by sorted_ container (requiring an additional dereference to access id) template diff --git a/visualization/motion_planning_tasks/src/remote_task_model.h b/visualization/motion_planning_tasks/src/remote_task_model.h index aa5ed3df..dd770cec 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.h +++ b/visualization/motion_planning_tasks/src/remote_task_model.h @@ -87,6 +87,8 @@ public: QAbstractItemModel* getSolutionModel(const QModelIndex& index) override; DisplaySolutionPtr getSolution(const QModelIndex &index) override; + + rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; }; diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index b6b6f6f4..29950221 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -50,6 +50,7 @@ #include namespace ros { class ServiceClient; } +namespace rviz { class PropertyTreeModel; } namespace moveit_rviz_plugin { @@ -84,8 +85,13 @@ public: Qt::ItemFlags flags(const QModelIndex &index) const override; unsigned int taskFlags() const { return flags_; } + /// get solution model for given stage index virtual QAbstractItemModel* getSolutionModel(const QModelIndex& index) = 0; + /// get solution for given solution index virtual DisplaySolutionPtr getSolution(const QModelIndex &index) = 0; + + /// get property model for given stage index + virtual rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) = 0; }; diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 5a2e6a3b..ae6072c6 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -254,7 +254,8 @@ void TaskView::onCurrentStageChanged(const QModelIndex ¤t, const QModelInd d_ptr->lock(nullptr); // unlocks any locked_display_ - auto *view = d_ptr->solutions_view; + // update the SolutionModel + QTreeView *view = d_ptr->solutions_view; QItemSelectionModel *sm = view->selectionModel(); QAbstractItemModel *m = task ? task->getSolutionModel(task_index) : nullptr; view->sortByColumn(-1); @@ -268,6 +269,13 @@ void TaskView::onCurrentStageChanged(const QModelIndex ¤t, const QModelInd connect(sm, SIGNAL(selectionChanged(QItemSelection, QItemSelection)), this, SLOT(onSolutionSelectionChanged(QItemSelection, QItemSelection))); } + + // update the PropertyModel + view = d_ptr->property_view; + sm = view->selectionModel(); + m = task ? task->getPropertyModel(task_index) : nullptr; + view->setModel(m); + delete sm; // we don't store the selection model } void TaskView::onCurrentSolutionChanged(const QModelIndex ¤t, const QModelIndex &previous) diff --git a/visualization/motion_planning_tasks/src/task_view.ui b/visualization/motion_planning_tasks/src/task_view.ui index fe25c1d4..8ae8aa59 100644 --- a/visualization/motion_planning_tasks/src/task_view.ui +++ b/visualization/motion_planning_tasks/src/task_view.ui @@ -14,9 +14,6 @@ Tasks Panel - - 0 - 0 @@ -30,26 +27,14 @@ 0 - - - 0 + + + Qt::Vertical - - 0 - - - 0 - - - 0 - - - 0 - - - + + - 6 + 0 @@ -58,59 +43,76 @@ + + + + + 0 + 0 + + + + Qt::Horizontal + + + + + 2 + 0 + + + + Qt::ActionsContextMenu + + + false + + + + + + 1 + 0 + + + + QAbstractItemView::ExtendedSelection + + + false + + + true + + + true + + + false + + + + - - - - - - 0 - 0 - + + + + + 0 - - Qt::Horizontal - - - - - 2 - 0 - - - - Qt::ActionsContextMenu - - - false - - - - - - 1 - 0 - - - - QAbstractItemView::ExtendedSelection - - - false - - - true - - - true - - - false - - - - - + + + + Properties + + + + + + + + + @@ -144,6 +146,11 @@ QTreeView
task_list_model.h
+ + rviz::PropertyTreeWidget + QTreeView +
rviz/properties/property_tree_widget.h
+
From cf71d2f0d25e531faf05d409cefbd14b5f42a6b8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 31 Jan 2018 10:44:02 +0100 Subject: [PATCH 04/16] unify use of buffer interfaces in containers All containers need to buffer their children's sendBackward/sendForward states. --- .../moveit/task_constructor/container.h | 2 +- .../moveit/task_constructor/container_p.h | 50 +++++++++---------- core/src/container.cpp | 37 ++++++-------- 3 files changed, 38 insertions(+), 51 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 22178bc3..5cfcc150 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -150,7 +150,7 @@ protected: /** Wrap an existing solution - for use in containers. * * This essentially wraps a solution of a child and thus allows - * for new new clones of start / end states, which in turn will + * for new clones of start / end states, which in turn will * have separate incoming/outgoing trajectories */ class WrappedSolution : public SolutionBase { public: diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 66fadb12..306cefcb 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -47,13 +47,16 @@ namespace moveit { namespace task_constructor { -/* A container needs to decouple its interfaces from those of its children. - * Both, the container and the children have their own starts_ and ends_. - * The container needs to forward states received in its interfaces to - * the interfaces of the children. - * Solutions found by the children, then need to be connected to the - * container's interface states. To this end, we remember the mapping - * from internal to external states. +/* A container needs to decouple its own (external) interfaces + * from those (internal) of its children. + * Both, the container and the children have their own pull interfaces: starts_ and ends_. + * The container forwards states received in its pull interfaces to the + * corresponding interfaces of the children. + * States pushed by children are temporarily stored in pending_backward_ and pending_forward_. + * + * Solutions found by the children need to be lifted from the internal level to the + * external level. To this end, we remember the mapping from internal to external states. + * * Note, that there might be many solutions connecting a single start-end pair. * These solutions might origin from different children (ParallelContainer) * or from different solution paths in a SerialContainer. @@ -93,9 +96,7 @@ public: bool compute() override; protected: - ContainerBasePrivate(ContainerBase *me, const std::string &name) - : StagePrivate(me, name) - {} + ContainerBasePrivate(ContainerBase *me, const std::string &name); /// copy external_state to a child's interface and remember the link in internal_to map void copyState(Interface::iterator external, Stage &child, bool to_start, bool updated); @@ -105,6 +106,11 @@ protected: // map start/end states of children (internal) to corresponding states in our external interfaces std::map internal_to_external_; + + // interface to receive children's sendBackward() states + InterfacePtr pending_backward_; + // interface to receive children's sendForward() states + InterfacePtr pending_forward_; }; PIMPL_FUNCTIONS(ContainerBase) @@ -134,18 +140,14 @@ private: * The solution of a single child stage is usually disconnected to the container's start or end. * Only if all the children in the chain have found a coherent solution from start to end, * this solution can be announced as a solution of the SerialContainer. - * - * Particularly, the first/last stage's sendBackward()/sendForward() call - * cannot directly propagate their associated state to the previous/next stage of this container, - * because we cannot provide a full solution yet. Hence, the first/last stage - * propagate to the pending_backward_/pending_forward_ interface first. - * If eventually a full solution is found, it is propagated to prevEnds()/nextStarts() - - * together with the solution. */ + */ class SerialContainerPrivate : public ContainerBasePrivate { friend class SerialContainer; public: - SerialContainerPrivate(SerialContainer* me, const std::string &name); + SerialContainerPrivate(SerialContainer* me, const std::string &name) + : ContainerBasePrivate(me, name) + {} void storeNewSolution(SerialSolution&& s); const ordered& solutions() const { return solutions_; } @@ -153,11 +155,6 @@ public: private: void connect(StagePrivate *prev, StagePrivate *next); - // interface to buffer first child's sendBackward() states - InterfacePtr pending_backward_; - // interface to buffer last child's sendForward() states - InterfacePtr pending_forward_; - // set of all solutions ordered solutions_; }; @@ -177,11 +174,10 @@ class WrapperBasePrivate : public ContainerBasePrivate { friend class WrapperBase; public: - WrapperBasePrivate(WrapperBase* me, const std::string& name); +WrapperBasePrivate(WrapperBase* me, const std::string& name) + : ContainerBasePrivate(me, name) +{} -private: - InterfacePtr dummy_starts_; - InterfacePtr dummy_ends_; }; PIMPL_FUNCTIONS(WrapperBase) diff --git a/core/src/container.cpp b/core/src/container.cpp index 0fd19e25..246c3553 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -49,6 +49,13 @@ using namespace std::placeholders; namespace moveit { namespace task_constructor { +ContainerBasePrivate::ContainerBasePrivate(ContainerBase *me, const std::string &name) + : StagePrivate(me, name) +{ + pending_backward_.reset(new Interface(Interface::NotifyFunction())); + pending_forward_.reset(new Interface(Interface::NotifyFunction())); +} + ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int index) const { const_iterator position = children_.begin(); if (index > 0) { @@ -156,7 +163,10 @@ void ContainerBase::reset() for (auto& child: impl->children()) child->reset(); - // clear mapping + // clear buffer interfaces + impl->pending_backward_->clear(); + impl->pending_forward_->clear(); + // ... and state mapping impl->internal_to_external_.clear(); Stage::reset(); @@ -190,15 +200,6 @@ void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) } -SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name) - : ContainerBasePrivate(me, name) -{ - // these lists don't need a notify function, connections are handled by onNewSolution() - pending_backward_.reset(new Interface(Interface::NotifyFunction())); - pending_forward_.reset(new Interface(Interface::NotifyFunction())); -} - - struct SolutionCollector { SolutionCollector(size_t max_depth) : max_depth(max_depth) {} @@ -320,8 +321,6 @@ void SerialContainer::reset() // clear queues impl->solutions_.clear(); - impl->pending_backward_->clear(); - impl->pending_forward_->clear(); // recursively reset children ContainerBase::reset(); @@ -521,13 +520,6 @@ void ParallelContainerBase::onNewSolution(const SolutionBase &s) } -WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name) - : ContainerBasePrivate(me, name) -{ - dummy_starts_.reset(new Interface(Interface::NotifyFunction())); - dummy_ends_.reset(new Interface(Interface::NotifyFunction())); -} - WrapperBase::WrapperBase(const std::string &name, Stage::pointer &&child) : WrapperBase(new WrapperBasePrivate(this, name), std::move(child)) {} @@ -548,8 +540,7 @@ bool WrapperBase::insert(Stage::pointer &&stage, int before) void WrapperBase::reset() { - pimpl()->dummy_starts_->clear(); - pimpl()->dummy_ends_->clear(); + ContainerBase::reset(); } void WrapperBase::init(const planning_scene::PlanningSceneConstPtr &scene) @@ -564,8 +555,8 @@ void WrapperBase::init(const planning_scene::PlanningSceneConstPtr &scene) assert(!impl->ends()); // provide a dummy interface to receive interface states of wrapped child - wrapped()->pimpl()->setPrevEnds(impl->dummy_ends_); - wrapped()->pimpl()->setNextStarts(impl->dummy_starts_); + wrapped()->pimpl()->setPrevEnds(impl->pending_backward_); + wrapped()->pimpl()->setNextStarts(impl->pending_forward_); // init + validate children ContainerBase::init(scene); From eaaaf072145030a6e7c964405254009bdd7eb243 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 31 Jan 2018 10:45:22 +0100 Subject: [PATCH 05/16] exploit default argument for Interface constructor --- core/src/container.cpp | 4 ++-- core/test/test_container.cpp | 2 +- core/test/test_stage.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 246c3553..16c94fb1 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -52,8 +52,8 @@ namespace moveit { namespace task_constructor { ContainerBasePrivate::ContainerBasePrivate(ContainerBase *me, const std::string &name) : StagePrivate(me, name) { - pending_backward_.reset(new Interface(Interface::NotifyFunction())); - pending_forward_.reset(new Interface(Interface::NotifyFunction())); + pending_backward_.reset(new Interface); + pending_forward_.reset(new Interface); } ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int index) const { diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index d179c136..29b5d2f5 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -55,7 +55,7 @@ TEST_F(BaseTest, serialContainer) { SerialContainer c("serial"); SerialContainerPrivate *cp = c.pimpl(); // pretend, that the container is connected - InterfacePtr dummy(new Interface(Interface::NotifyFunction())); + InterfacePtr dummy(new Interface); cp->setNextStarts(dummy); cp->setPrevEnds(dummy); planning_scene::PlanningScenePtr scene; diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index 928ea53d..bac4a247 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -18,8 +18,8 @@ public: robot_model_loader::RobotModelLoader loader; ps.reset(new planning_scene::PlanningScene(loader.getModel())); - prev.reset(new Interface(Interface::NotifyFunction())); - next.reset(new Interface(Interface::NotifyFunction())); + prev.reset(new Interface); + next.reset(new Interface); pimpl()->setPrevEnds(prev); pimpl()->setNextStarts(next); } From 73f509c3ba5ad1cedb29558835cd05389f6d461d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 31 Jan 2018 12:39:55 +0100 Subject: [PATCH 06/16] simplify usage of ContainerBasePrivate::copyState() --- .../moveit/task_constructor/container_p.h | 2 +- core/src/container.cpp | 33 +++++++------------ 2 files changed, 12 insertions(+), 23 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 306cefcb..e9c98568 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -99,7 +99,7 @@ protected: ContainerBasePrivate(ContainerBase *me, const std::string &name); /// copy external_state to a child's interface and remember the link in internal_to map - void copyState(Interface::iterator external, Stage &child, bool to_start, bool updated); + void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); protected: container_type children_; diff --git a/core/src/container.cpp b/core/src/container.cpp index 16c94fb1..eac0c58e 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -97,15 +97,13 @@ bool ContainerBasePrivate::compute() return static_cast(me_)->compute(); } -void ContainerBasePrivate::copyState(Interface::iterator external, - Stage &child, bool to_start, bool updated) { - if (to_start) { - InterfaceState& internal = *child.pimpl()->starts()->clone(*external); - internal_to_external_.insert(std::make_pair(&internal, external)); - } else { - InterfaceState& internal = *child.pimpl()->ends()->clone(*external); - internal_to_external_.insert(std::make_pair(&internal, external)); - } +void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { + // TODO need to update existing mapping? + + // create a clone of external state within target interface (child's starts() or ends()) + InterfaceState& internal = *target->clone(*external); + // and remember the mapping between them + internal_to_external_.insert(std::make_pair(&internal, external)); } ContainerBase::ContainerBase(ContainerBasePrivate *impl) @@ -364,19 +362,10 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) ContainerBase::init(scene); // initialize starts_ and ends_ interfaces - Stage* child = start->get(); - if (child->pimpl()->starts()) - impl->starts_.reset(new Interface([impl, child](Interface::iterator external, bool updated){ - // new external state in our starts_ interface is copied to first child - impl->copyState(external, *child, true, updated); - })); - - child = last->get(); - if (child->pimpl()->ends()) - impl->ends_.reset(new Interface([impl, child](Interface::iterator external, bool updated){ - // new external state in our ends_ interface is copied to last child - impl->copyState(external, *child, false, updated); - })); + if (const InterfacePtr& target = (*start)->pimpl()->starts()) + impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2))); + if (const InterfacePtr& target = (*last)->pimpl()->ends()) + impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2))); // validate connectivity of this if (!impl->nextStarts()) From a3a8f1a9d221b8b4b60febb619f94bae23faaa8d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 31 Jan 2018 15:37:44 +0100 Subject: [PATCH 07/16] improved directional (forward/backward) access to interfaces - TraverseDirection -> Interface::Direction - StagePrivate: pullInterface(), pushInterface() - trajectories(SolutionBase) -> SolutionBase::trajectories() --- .../moveit/task_constructor/container.h | 2 +- .../include/moveit/task_constructor/stage_p.h | 6 +++++ .../include/moveit/task_constructor/storage.h | 16 +++++------ core/src/container.cpp | 27 ++++++++++++++++--- 4 files changed, 38 insertions(+), 13 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 5cfcc150..b39721f5 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -108,7 +108,7 @@ protected: /// until the end, i.e. until there are no more subsolutions in the given direction /// For each solution path, callback the given processor passing /// the full trace (from start to end, but not including start) and its accumulated costs - template + template void traverse(const SolutionBase &start, const SolutionProcessor &cb, solution_container &trace, double trace_cost = 0); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 7791d041..a9d2ab80 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -80,6 +80,12 @@ public: inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); } inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); } + /// templated access to pull/push interfaces + inline InterfacePtr& pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; } + inline InterfacePtr pushInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); } + inline InterfaceConstPtr pullInterface(Interface::Direction dir) const { return dir == Interface::FORWARD ? starts_ : ends_; } + inline InterfaceConstPtr pushInterface(Interface::Direction dir) const { return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); } + /// the following methods should be called only by a container /// to setup the connection structure of their children inline void setHierarchy(ContainerBase* parent, container_type::iterator it) { diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 9a3a47a3..3ae3bf4a 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -142,6 +142,7 @@ private: /** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */ class Interface : public ordered { public: + enum Direction { FORWARD, BACKWARD, START=FORWARD, END=BACKWARD }; typedef std::function NotifyFunction; Interface(const NotifyFunction ¬ify = NotifyFunction()); @@ -170,6 +171,9 @@ public: inline const InterfaceState* start() const { return start_; } inline const InterfaceState* end() const { return end_; } + template + inline const InterfaceState::Solutions& trajectories() const; + inline void setStartState(const InterfaceState& state){ assert(start_ == NULL); // only allow setting once (by Stage) start_ = &state; @@ -244,17 +248,13 @@ private: }; -enum TraverseDirection { FORWARD, BACKWARD }; -template -const InterfaceState::Solutions& trajectories(const SolutionBase &start); - template <> inline -const InterfaceState::Solutions& trajectories(const SolutionBase &solution) { - return solution.end()->outgoingTrajectories(); +const InterfaceState::Solutions& SolutionBase::trajectories() const { + return end_->outgoingTrajectories(); } template <> inline -const InterfaceState::Solutions& trajectories(const SolutionBase &solution) { - return solution.start()->incomingTrajectories(); +const InterfaceState::Solutions& SolutionBase::trajectories() const { + return start_->incomingTrajectories(); } } } diff --git a/core/src/container.cpp b/core/src/container.cpp index eac0c58e..7d48342a 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -231,11 +231,11 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) // find all incoming solution pathes ending at current solution SolutionCollector incoming(num_before); - traverse(current, std::ref(incoming), trace); + traverse(current, std::ref(incoming), trace); // find all outgoing solution pathes starting at current solution SolutionCollector outgoing(num_after); - traverse(current, std::ref(outgoing), trace); + traverse(current, std::ref(outgoing), trace); // collect (and sort) all solutions spanning from start to end of this container ordered sorted; @@ -414,11 +414,11 @@ void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &p break; } -template +template void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb, solution_container &trace, double trace_cost) { - const InterfaceState::Solutions& solutions = trajectories(start); + const InterfaceState::Solutions& solutions = start.trajectories(); if (solutions.empty()) // if we reached the end, call the callback cb(trace, trace_cost); else for (SolutionBase* successor : solutions) { @@ -492,6 +492,25 @@ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &sc // this needs to be done *after* initializing the connections ContainerBase::init(scene); + bool pulls[2]; + for (const Stage::pointer& stage : impl->children()) { + StagePrivate *child = stage->pimpl(); + // is there any child reading from starts() or ends() ? + pulls[Interface::START] |= bool(child->pullInterface(Interface::START)); + pulls[Interface::END] |= bool(child->pullInterface(Interface::END)); + } + + // initialize this' pull connections + for (Interface::Direction dir : { Interface::START, Interface::END }) { + if (pulls[dir]) + impl->pullInterface(dir).reset(new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, impl, dir, _1, _2))); + else + impl->pullInterface(dir).reset(); + } + + if (impl->children().empty()) + errors.push_back(*this, "no children"); + if (errors) throw errors; } From 919be24d16d5cc01bf8706bae4ba5b98f6f9e2b3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 31 Jan 2018 17:16:53 +0100 Subject: [PATCH 08/16] implement ParallelContainerBase --- .../moveit/task_constructor/container.h | 39 +++++++-- .../moveit/task_constructor/container_p.h | 15 ++++ core/src/container.cpp | 79 ++++++++++++++++--- 3 files changed, 117 insertions(+), 16 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index b39721f5..eabfcde6 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -139,11 +139,6 @@ protected: ParallelContainerBase(ParallelContainerBasePrivate* impl); virtual void onNewSolution(const SolutionBase& s) override; - - /// callback for new start states (received externally) - virtual void onNewStartState(Interface::iterator external, bool updated) = 0; - /// callback for new end states (received externally) - virtual void onNewEndState(Interface::iterator external, bool updated) = 0; }; @@ -167,6 +162,40 @@ private: }; +/** Plan for different alternatives in parallel. + * + * Solution of all children are reported - sorted by cost. + */ +class Alternatives : public ParallelContainerBase +{ +public: + Alternatives(const std::string &name) : ParallelContainerBase(name) {} + + bool canCompute() const override; + bool compute() override; +}; + + +/** Plan for different alternatives in sequence. + * + * Try to find feasible solutions using first child. Only if this fails, + * proceed to the next child trying an alternative planning strategy. + * All solutions of the last active child are reported. + */ +class Fallbacks : public ParallelContainerBase +{ + mutable Stage* active_child_ = nullptr; + +public: + Fallbacks(const std::string &name) : ParallelContainerBase(name) {} + + void reset() override; + void init(const planning_scene::PlanningSceneConstPtr &scene) override; + bool canCompute() const override; + bool compute() override; +}; + + class WrapperBasePrivate; /** Base class for Wrapper and Task * diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index e9c98568..6ec57cd7 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -98,6 +98,16 @@ public: protected: ContainerBasePrivate(ContainerBase *me, const std::string &name); + // Get push interface to be used for children: If our own push interface is not set, + // don't set children's interface either: pushing is not supported. + // Otherwise return pending_* buffer. + InterfacePtr getPushBackwardInterface() { + return prevEnds() ? pending_backward_ : InterfacePtr(); + } + InterfacePtr getPushForwardInterface() { + return nextStarts() ? pending_forward_ : InterfacePtr(); + } + /// copy external_state to a child's interface and remember the link in internal_to map void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); @@ -107,6 +117,8 @@ protected: // map start/end states of children (internal) to corresponding states in our external interfaces std::map internal_to_external_; + /* TODO: these interfaces don't need to be priority-sorted. + * Introduce base class UnsortedInterface (which is a plain list) for this use case. */ // interface to receive children's sendBackward() states InterfacePtr pending_backward_; // interface to receive children's sendForward() states @@ -166,6 +178,9 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate { public: ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string &name); + + /// callback for new externally received states + void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); }; PIMPL_FUNCTIONS(ParallelContainerBase) diff --git a/core/src/container.cpp b/core/src/container.cpp index 7d48342a..8a11e608 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -144,6 +144,7 @@ bool ContainerBase::insert(Stage::pointer &&stage, int before) bool ContainerBase::remove(int pos) { ContainerBasePrivate::const_iterator it = pimpl()->position(pos); + (*it)->pimpl()->setHierarchy(nullptr, ContainerBasePrivate::iterator()); pimpl()->children_.erase(it); return true; } @@ -339,11 +340,11 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) /*** connect children ***/ // first stage sends backward to pending_backward_ auto start = impl->children().begin(); - (*start)->pimpl()->setPrevEnds(impl->pending_backward_); + (*start)->pimpl()->setPrevEnds(impl->getPushBackwardInterface()); // last stage sends forward to pending_forward_ auto last = --impl->children().end(); - (*last)->pimpl()->setNextStarts(impl->pending_forward_); + (*last)->pimpl()->setNextStarts(impl->getPushForwardInterface()); auto cur = start; auto prev = cur; ++cur; // prev points to 1st, cur points to 2nd stage @@ -458,8 +459,11 @@ void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg, ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name) : ContainerBasePrivate(me, name) { - starts_.reset(new Interface(std::bind(&ParallelContainerBase::onNewStartState, me, _1, _2))); - ends_.reset(new Interface(std::bind(&ParallelContainerBase::onNewEndState, me, _1, _2))); +} + +void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated) { + for (const Stage::pointer& stage : children()) + copyState(external, stage->pimpl()->pullInterface(dir), updated); } @@ -476,20 +480,24 @@ void ParallelContainerBase::reset() ContainerBase::reset(); } +/* States received by the container need to be copied to all children's pull interfaces. + * States generated by children can be directly forwarded into the container's push interfaces. + */ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) { InitStageException errors; auto impl = pimpl(); - // connect children such that they directly send to this' prevEnds() / nextStarts() + // initialize push connections of children + InterfacePtr push_prev = impl->getPushBackwardInterface(); + InterfacePtr push_next = impl->getPushForwardInterface(); for (const Stage::pointer& stage : impl->children()) { StagePrivate *child = stage->pimpl(); - child->setPrevEnds(impl->prevEnds()); - child->setNextStarts(impl->nextStarts()); + child->setPrevEnds(push_prev); + child->setNextStarts(push_next); } - // recursively init + validate all children - // this needs to be done *after* initializing the connections + // this needs to be done *after* initializing push connections ContainerBase::init(scene); bool pulls[2]; @@ -563,8 +571,8 @@ void WrapperBase::init(const planning_scene::PlanningSceneConstPtr &scene) assert(!impl->ends()); // provide a dummy interface to receive interface states of wrapped child - wrapped()->pimpl()->setPrevEnds(impl->pending_backward_); - wrapped()->pimpl()->setNextStarts(impl->pending_forward_); + wrapped()->pimpl()->setPrevEnds(impl->getPushBackwardInterface()); + wrapped()->pimpl()->setNextStarts(impl->getPushForwardInterface()); // init + validate children ContainerBase::init(scene); @@ -665,4 +673,53 @@ void Wrapper::spawn(InterfaceState &&state, std::unique_ptr&& s) impl->newSolution(*solution); } + +bool Alternatives::canCompute() const +{ + for (const auto& stage : pimpl()->children()) + if (stage->pimpl()->canCompute()) + return true; + return false; +} + +bool Alternatives::compute() +{ + bool success = false; + for (const auto& stage : pimpl()->children()) + success |= stage->pimpl()->compute(); + return success; +} + + +void Fallbacks::reset() +{ + active_child_ = nullptr; + ParallelContainerBase::reset(); +} + +void Fallbacks::init(const planning_scene::PlanningSceneConstPtr &scene) +{ + ParallelContainerBase::init(scene); + active_child_ = pimpl()->children().front().get(); +} + +bool Fallbacks::canCompute() const +{ + while (active_child_) { + StagePrivate* child = active_child_->pimpl(); + if (child->canCompute()) return true; + + // active child failed, continue with next + auto next = child->it(); ++next; + active_child_ = next->get(); + } + return false; +} + +bool Fallbacks::compute() +{ + if (!active_child_) return false; + return active_child_->pimpl()->compute(); +} + } } From 1ab2d596fbab198a5e5eaf96da22bfd13402c004 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 31 Jan 2018 21:32:42 +0100 Subject: [PATCH 09/16] ContainerBasePrivate::liftSolution() --- .../moveit/task_constructor/container_p.h | 14 +-- core/src/container.cpp | 86 +++++++++---------- 2 files changed, 50 insertions(+), 50 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 6ec57cd7..c94f791a 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -110,6 +110,9 @@ protected: /// copy external_state to a child's interface and remember the link in internal_to map void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); + /// lift solution from internal to external level + void liftSolution(SolutionBase& solution, + const InterfaceState *internal_from, const InterfaceState *internal_to); protected: container_type children_; @@ -157,12 +160,7 @@ class SerialContainerPrivate : public ContainerBasePrivate { friend class SerialContainer; public: - SerialContainerPrivate(SerialContainer* me, const std::string &name) - : ContainerBasePrivate(me, name) - {} - - void storeNewSolution(SerialSolution&& s); - const ordered& solutions() const { return solutions_; } + SerialContainerPrivate(SerialContainer* me, const std::string &name); private: void connect(StagePrivate *prev, StagePrivate *next); @@ -179,8 +177,12 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate { public: ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string &name); +private: /// callback for new externally received states void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); + + // set of all solutions + ordered solutions_; }; PIMPL_FUNCTIONS(ParallelContainerBase) diff --git a/core/src/container.cpp b/core/src/container.cpp index 8a11e608..597cc557 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -106,6 +106,36 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa internal_to_external_.insert(std::make_pair(&internal, external)); } +void ContainerBasePrivate::liftSolution(SolutionBase& solution, + const InterfaceState *internal_from, const InterfaceState *internal_to) +{ + // add solution to existing or new start state + auto it = internal_to_external_.find(internal_from); + if (it != internal_to_external_.end()) { + // connect solution to existing start state + solution.setStartState(*it->second); + } else { + // spawn a new state in previous stage + Interface::iterator external = prevEnds()->add(InterfaceState(*internal_from), NULL, &solution); + internal_to_external_.insert(std::make_pair(internal_from, external)); + } + + // add solution to existing or new end state + it = internal_to_external_.find(internal_to); + if (it != internal_to_external_.end()) { + // connect solution to existing start state + solution.setEndState(*it->second); + } else { + // spawn a new state in next stage + Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL); + internal_to_external_.insert(std::make_pair(internal_to, external)); + } + + // perform default stage action on new solution + StagePrivate::newSolution(solution); +} + + ContainerBase::ContainerBase(ContainerBasePrivate *impl) : Stage(impl) { @@ -268,42 +298,10 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) } // store new solutions (in sorted) - for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) - impl->storeNewSolution(std::move(*it)); -} - -void SerialContainerPrivate::storeNewSolution(SerialSolution &&s) -{ - const InterfaceState *internal_from = s.internalStart(); - const InterfaceState *internal_to = s.internalEnd(); - - // create new SerialSolution and get a reference to it - SerialSolution& solution = *solutions_.insert(std::move(s)); - - // add solution to existing or new start state - auto it = internal_to_external_.find(internal_from); - if (it != internal_to_external_.end()) { - // connect solution to existing start state - solution.setStartState(*it->second); - } else { - // spawn a new state in previous stage - Interface::iterator external = prevEnds()->add(InterfaceState(*internal_from), NULL, &solution); - internal_to_external_.insert(std::make_pair(internal_from, external)); + for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) { + auto inserted = impl->solutions_.insert(std::move(*it)); + impl->liftSolution(*inserted, inserted->internalStart(), inserted->internalEnd()); } - - // add solution to existing or new end state - it = internal_to_external_.find(internal_to); - if (it != internal_to_external_.end()) { - // connect solution to existing start state - solution.setEndState(*it->second); - } else { - // spawn a new state in next stage - Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL); - internal_to_external_.insert(std::make_pair(internal_to, external)); - } - - // perform default stage action on new solution - newSolution(solution); } @@ -325,6 +323,10 @@ void SerialContainer::reset() ContainerBase::reset(); } +SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name) + : ContainerBasePrivate(me, name) +{} + void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) { prev->setNextStarts(next->starts()); next->setPrevEnds(prev->ends()); @@ -410,7 +412,7 @@ size_t SerialContainer::numSolutions() const void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &processor) const { - for(const SolutionBase& s : pimpl()->solutions()) + for(const SolutionBase& s : pimpl()->solutions_) if (!processor(s)) break; } @@ -525,14 +527,9 @@ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &sc void ParallelContainerBase::onNewSolution(const SolutionBase &s) { - // update state priorities - InterfaceState::Priority prio(1, s.cost()); - InterfaceState* start = const_cast(s.start()); - start->owner()->updatePriority(*start, prio); - InterfaceState* end = const_cast(s.end()); - end->owner()->updatePriority(*end, prio); - - pimpl()->newSolution(s); + auto impl = pimpl(); + auto it = impl->solutions_.insert(WrappedSolution(impl, &s)); + impl->liftSolution(*it, s.start(), s.end()); } @@ -591,6 +588,7 @@ Stage* WrapperBase::wrapped() void WrapperBase::onNewSolution(const SolutionBase &s) { + // TODO replace with liftSolution() // update state priorities InterfaceState::Priority prio(1, s.cost()); InterfaceState* start = const_cast(s.start()); From 032a4e23c1cc4074f86cba2f02c6dcc853e360bf Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 31 Jan 2018 23:25:41 +0100 Subject: [PATCH 10/16] derive WrapperBase from ParallelContainerBase --- .../moveit/task_constructor/container.h | 12 ++-- .../moveit/task_constructor/container_p.h | 7 ++- core/include/moveit/task_constructor/task.h | 6 +- core/src/container.cpp | 60 +++++-------------- core/src/task.cpp | 21 ++++--- 5 files changed, 41 insertions(+), 65 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index eabfcde6..0e3d1859 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -135,6 +135,9 @@ public: void reset() override; void init(const planning_scene::PlanningSceneConstPtr &scene) override; + size_t numSolutions() const override; + void processSolutions(const SolutionProcessor &processor) const override; + protected: ParallelContainerBase(ParallelContainerBasePrivate* impl); @@ -202,17 +205,12 @@ class WrapperBasePrivate; * WrapperBase ensures that only a single child is wrapped in a container. * This child can be accessed via wrapped(). */ -class WrapperBase : public ContainerBase +class WrapperBase : public ParallelContainerBase { public: PRIVATE_CLASS(WrapperBase) WrapperBase(const std::string &name, pointer &&child = Stage::pointer()); - void reset() override; - void init(const planning_scene::PlanningSceneConstPtr &scene) override; - - size_t numSolutions() const override; - /// insertion is only allowed if children() is empty bool insert(Stage::pointer&& stage, int before = -1) override; @@ -223,8 +221,6 @@ public: } protected: - virtual void onNewSolution(const SolutionBase& s) = 0; - WrapperBase(WrapperBasePrivate *impl, pointer &&child = Stage::pointer()); }; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index c94f791a..d4da7e2b 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -95,6 +95,9 @@ public: bool canCompute() const override; bool compute() override; + InterfacePtr pendingBackward() const { return pending_backward_; } + InterfacePtr pendingForward() const { return pending_forward_; } + protected: ContainerBasePrivate(ContainerBase *me, const std::string &name); @@ -187,12 +190,12 @@ private: PIMPL_FUNCTIONS(ParallelContainerBase) -class WrapperBasePrivate : public ContainerBasePrivate { +class WrapperBasePrivate : public ParallelContainerBasePrivate { friend class WrapperBase; public: WrapperBasePrivate(WrapperBase* me, const std::string& name) - : ContainerBasePrivate(me, name) + : ParallelContainerBasePrivate(me, name) {} }; diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index b0affc3f..b38c2aeb 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -62,9 +62,9 @@ MOVEIT_CLASS_FORWARD(Task) /** A Task is the root of a tree of stages. * - * Actually a tasks wraps a single container, which serves as the root of all stages. - * The wrapped container spawns its solutions into the prevEnds(), nextStarts() interfaces, - * which are provided by the wrappers end_ and start_ and interfaces respectively. */ + * Actually a tasks wraps a single container (by default a SerialContainer), + * which serves as the root of all stages. + */ class Task : protected WrapperBase { public: Task(const std::string& id = "", diff --git a/core/src/container.cpp b/core/src/container.cpp index 597cc557..8213520a 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -525,6 +525,18 @@ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &sc throw errors; } +size_t ParallelContainerBase::numSolutions() const +{ + return pimpl()->solutions_.size(); +} + +void ParallelContainerBase::processSolutions(const Stage::SolutionProcessor &processor) const +{ + for(const SolutionBase& s : pimpl()->solutions_) + if (!processor(s)) + break; +} + void ParallelContainerBase::onNewSolution(const SolutionBase &s) { auto impl = pimpl(); @@ -538,7 +550,7 @@ WrapperBase::WrapperBase(const std::string &name, Stage::pointer &&child) {} WrapperBase::WrapperBase(WrapperBasePrivate *impl, Stage::pointer &&child) - : ContainerBase(impl) + : ParallelContainerBase(impl) { if (child) insert(std::move(child)); } @@ -548,37 +560,7 @@ bool WrapperBase::insert(Stage::pointer &&stage, int before) // restrict num of children to one if (numChildren() > 0) return false; - return ContainerBase::insert(std::move(stage), before); -} - -void WrapperBase::reset() -{ - ContainerBase::reset(); -} - -void WrapperBase::init(const planning_scene::PlanningSceneConstPtr &scene) -{ - auto impl = pimpl(); - - if (numChildren() != 1) - throw InitStageException(*this, "no wrapped child"); - - // as a generator-like stage, we don't accept inputs - assert(!impl->starts()); - assert(!impl->ends()); - - // provide a dummy interface to receive interface states of wrapped child - wrapped()->pimpl()->setPrevEnds(impl->getPushBackwardInterface()); - wrapped()->pimpl()->setNextStarts(impl->getPushForwardInterface()); - - // init + validate children - ContainerBase::init(scene); -} - -size_t WrapperBase::numSolutions() const -{ - // dummy implementation needed to allow insert() in constructor - return 0; + return ParallelContainerBase::insert(std::move(stage), before); } Stage* WrapperBase::wrapped() @@ -586,24 +568,12 @@ Stage* WrapperBase::wrapped() return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get(); } -void WrapperBase::onNewSolution(const SolutionBase &s) -{ - // TODO replace with liftSolution() - // update state priorities - InterfaceState::Priority prio(1, s.cost()); - InterfaceState* start = const_cast(s.start()); - start->owner()->updatePriority(*start, prio); - InterfaceState* end = const_cast(s.end()); - end->owner()->updatePriority(*end, prio); - - pimpl()->newSolution(s); -} - WrapperPrivate::WrapperPrivate(Wrapper *me, const std::string &name) : WrapperBasePrivate(me, name) {} + Wrapper::Wrapper(WrapperPrivate *impl, Stage::pointer &&child) : WrapperBase(impl, std::move(child)) {} diff --git a/core/src/task.cpp b/core/src/task.cpp index 2d19755e..1000865d 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -180,10 +180,19 @@ void Task::reset() void Task::init(const planning_scene::PlanningSceneConstPtr &scene) { - WrapperBase::init(scene); + auto impl = pimpl(); + // initialize push connections of wrapped child + StagePrivate *child = wrapped()->pimpl(); + child->setPrevEnds(impl->pendingBackward()); + child->setNextStarts(impl->pendingForward()); + + // explicitly call ContainerBase::init(), not WrapperBase::init() + // to keep the just set push interface for the wrapped child + ContainerBase::init(scene); + // provide introspection instance to all stages - pimpl()->setIntrospection(introspection_.get()); - pimpl()->traverseStages([this](Stage& stage, int) { + impl->setIntrospection(introspection_.get()); + impl->traverseStages([this](Stage& stage, int) { stage.pimpl()->setIntrospection(introspection_.get()); return true; }, 1, UINT_MAX); @@ -222,9 +231,7 @@ bool Task::plan() size_t Task::numSolutions() const { - auto w = stages(); - // during initial insert() we call numSolutions(), but wrapped() is not yet defined - return w ? w->numSolutions() : 0; + return stages()->numSolutions(); } void Task::processSolutions(const ContainerBase::SolutionProcessor &processor) const @@ -240,7 +247,7 @@ void Task::publishAllSolutions(bool wait) void Task::onNewSolution(const SolutionBase &s) { - WrapperBase::onNewSolution(s); + // no need to call WrapperBase::onNewSolution! if (introspection_) introspection_->publishSolution(s); } From 449ccf55c6bca3469981f8d84d27fa43bed1ab81 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 1 Feb 2018 01:16:07 +0100 Subject: [PATCH 11/16] merge WrapperBase + Wrapper into WrapperBase Wrapper is not restricted to generator-type stage anymore. --- .../moveit/task_constructor/container.h | 80 ++++------ .../moveit/task_constructor/container_p.h | 54 ++++--- .../task_constructor/stages/compute_ik.h | 2 +- .../include/moveit/task_constructor/storage.h | 10 ++ core/include/moveit/task_constructor/utils.h | 9 -- core/src/container.cpp | 149 +++++++++--------- core/src/stages/compute_ik.cpp | 14 +- 7 files changed, 155 insertions(+), 163 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 0e3d1859..f884e026 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -137,31 +137,30 @@ public: size_t numSolutions() const override; void processSolutions(const SolutionProcessor &processor) const override; + size_t numFailures() const override; + void processFailures(const SolutionProcessor &processor) const override; protected: ParallelContainerBase(ParallelContainerBasePrivate* impl); + /// method called if any child found a new (internal) solution virtual void onNewSolution(const SolutionBase& s) override; -}; - -/** Wrap an existing solution - for use in containers. - * - * This essentially wraps a solution of a child and thus allows - * for new clones of start / end states, which in turn will - * have separate incoming/outgoing trajectories */ -class WrappedSolution : public SolutionBase { -public: - explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped) - : SolutionBase(creator, wrapped->cost()), wrapped_(wrapped) - {} - void fillMessage(moveit_task_constructor_msgs::Solution &solution, - Introspection* introspection = nullptr) const override { - wrapped_->fillMessage(solution, introspection); + /// lift unmodified child solution (useful for simple filtering) + void liftSolution(const SolutionBase* solution) { + liftSolution(solution, solution->cost()); } + /// lift child solution, but allow for modifying costs + void liftSolution(const SolutionBase* solution, double cost); -private: - const SolutionBase* wrapped_; + /// spawn a new solution with given state as start and end + void spawn(InterfaceState &&state, SubTrajectory&& trajectory); + /// convience method spawning an empty SubTrajectory with given cost + void spawn(InterfaceState &&state, double cost) { + SubTrajectory trajectory; + trajectory.setCost(cost); + spawn(std::move(state), std::move(trajectory)); + } }; @@ -200,10 +199,16 @@ public: class WrapperBasePrivate; -/** Base class for Wrapper and Task +/** A wrapper wraps a single child stage, which can be accessed via wrapped(). * - * WrapperBase ensures that only a single child is wrapped in a container. - * This child can be accessed via wrapped(). + * Implementations of this interface need to implement onNewSolution(), which is + * called when the child has generated a new solution. + * The wrapper may reject the solution or create one or multiple derived solutions, + * potentially adapting the cost, as well as its start and end states. + * + * Care needs to be taken to not modify pulled states, but only pushed ones! + * liftFor each new solution, liftSolution() should be called, which comes in various + * flavours to allow for handing in new states (or not). */ class WrapperBase : public ParallelContainerBase { @@ -214,48 +219,17 @@ public: /// insertion is only allowed if children() is empty bool insert(Stage::pointer&& stage, int before = -1) override; - /// access the single wrapped child + /// access the single wrapped child, NULL if still empty Stage* wrapped(); inline const Stage* wrapped() const { return const_cast(this)->wrapped(); } -protected: - WrapperBase(WrapperBasePrivate *impl, pointer &&child = Stage::pointer()); -}; - - -class WrapperPrivate; -/** A wrapper wraps a single generator-style stage (and acts itself as a generator). - * - * The wrapped stage must act as a generator, i.e. only spawn new states - * in its external interfaces. It's intended, e.g. to filter or clone - * a generated solution of its wrapped generator. - */ -class Wrapper : public WrapperBase -{ -public: - PRIVATE_CLASS(Wrapper) - Wrapper(const std::string &name, pointer &&child = Stage::pointer()); - - void reset() override; bool canCompute() const override; bool compute() override; - size_t numSolutions() const override; - size_t numFailures() const override; - void processSolutions(const SolutionProcessor &processor) const override; - void processFailures(const SolutionProcessor &processor) const override; - - void spawn(InterfaceState &&state, std::unique_ptr&& s); - void spawn(InterfaceState &&state, double cost) { - std::unique_ptr s(new SubTrajectory()); - s->setCost(cost); - spawn(std::move(state), std::move(s)); - } - protected: - Wrapper(WrapperPrivate* impl, pointer &&child = Stage::pointer()); + WrapperBase(WrapperBasePrivate* impl, pointer &&child = Stage::pointer()); /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase &s) override = 0; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index d4da7e2b..9fbd339a 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -39,7 +39,6 @@ #pragma once #include -#include "utils.h" #include "stage_p.h" #include @@ -174,6 +173,27 @@ private: PIMPL_FUNCTIONS(SerialContainer) +/** Wrap an existing solution - for use in parallel containers and wrappers. + * + * This essentially wraps a solution of a child and thus allows + * for new clones of start / end states, which in turn will + * have separate incoming/outgoing trajectories */ +class WrappedSolution : public SolutionBase { +public: + explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped, double cost) + : SolutionBase(creator, cost), wrapped_(wrapped) + {} + explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped) + : WrappedSolution(creator, wrapped, wrapped->cost()) + {} + void fillMessage(moveit_task_constructor_msgs::Solution &solution, + Introspection* introspection = nullptr) const override; + +private: + const SolutionBase* wrapped_; +}; + + class ParallelContainerBasePrivate : public ContainerBasePrivate { friend class ParallelContainerBase; @@ -184,8 +204,17 @@ private: /// callback for new externally received states void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); - // set of all solutions - ordered solutions_; + // buffer for wrapped solutions + std::list wrapped_solutions_; + // buffer for newly created (not wrapped) solutions + std::list created_solutions_; + // buffer of created states (for use in created solutions) + std::list states_; + + // cost-ordered set of solutions (pointers into wrapped or created) + ordered solutions_; + // buffer for failures (pointers into wrapped or created) + std::list failures_; }; PIMPL_FUNCTIONS(ParallelContainerBase) @@ -194,25 +223,8 @@ class WrapperBasePrivate : public ParallelContainerBasePrivate { friend class WrapperBase; public: -WrapperBasePrivate(WrapperBase* me, const std::string& name) - : ParallelContainerBasePrivate(me, name) -{} - + WrapperBasePrivate(WrapperBase* me, const std::string& name); }; PIMPL_FUNCTIONS(WrapperBase) - -class WrapperPrivate : public WrapperBasePrivate { - friend class Wrapper; - -public: - WrapperPrivate(Wrapper* me, const std::string& name); - -private: - ordered, pointerLessThan>> solutions_; - std::list> failures_; - std::list failure_states_; -}; -PIMPL_FUNCTIONS(Wrapper) - } } diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index 4a480a78..f2448c3e 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -15,7 +15,7 @@ namespace core { MOVEIT_CLASS_FORWARD(RobotState) } namespace moveit { namespace task_constructor { namespace stages { -class ComputeIK : public Wrapper { +class ComputeIK : public WrapperBase { public: ComputeIK(const std::string &name, pointer &&child = Stage::pointer()); diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 3ae3bf4a..43182dce 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -258,3 +258,13 @@ const InterfaceState::Solutions& SolutionBase::trajectories } } } + +namespace std { +// comparison for pointers to SolutionBase: compare based on value +template<> struct less { + bool operator()(const moveit::task_constructor::SolutionBase* x, + const moveit::task_constructor::SolutionBase* y) { + return *x < *y; + } +}; +} diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index 657f4e5c..667da900 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -94,12 +94,3 @@ private: }; #define DECLARE_FLAGS(Flags, Enum) typedef QFlags Flags; - - -// compare two pointers by comparing their referenced values -template -struct pointerLessThan : std::enable_if::value> { - inline bool operator()(const T& x, const T& y) const { - return *x < *y; - } -}; diff --git a/core/src/container.cpp b/core/src/container.cpp index 8213520a..19373791 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -130,9 +130,6 @@ void ContainerBasePrivate::liftSolution(SolutionBase& solution, Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL); internal_to_external_.insert(std::make_pair(internal_to, external)); } - - // perform default stage action on new solution - StagePrivate::newSolution(solution); } @@ -301,6 +298,7 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) { auto inserted = impl->solutions_.insert(std::move(*it)); impl->liftSolution(*inserted, inserted->internalStart(), inserted->internalEnd()); + impl->newSolution(*inserted); } } @@ -436,7 +434,7 @@ void SerialContainer::traverse(const SolutionBase &start, const SolutionProcesso } void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg, - Introspection* introspection = nullptr) const + Introspection* introspection) const { moveit_task_constructor_msgs::SubSolution sub_msg; sub_msg.id = introspection ? introspection->solutionId(*this) : 0; @@ -458,6 +456,12 @@ void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg, } +void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution &solution, + Introspection *introspection) const +{ + wrapped_->fillMessage(solution, introspection); +} + ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name) : ContainerBasePrivate(me, name) { @@ -480,6 +484,13 @@ void ParallelContainerBase::reset() { // recursively reset children ContainerBase::reset(); + // clear buffers + auto impl = pimpl(); + impl->solutions_.clear(); + impl->failures_.clear(); + impl->wrapped_solutions_.clear(); + impl->created_solutions_.clear(); + impl->states_.clear(); } /* States received by the container need to be copied to all children's pull interfaces. @@ -532,18 +543,73 @@ size_t ParallelContainerBase::numSolutions() const void ParallelContainerBase::processSolutions(const Stage::SolutionProcessor &processor) const { - for(const SolutionBase& s : pimpl()->solutions_) - if (!processor(s)) + for(const SolutionBase* s : pimpl()->solutions_) + if (!processor(*s)) break; } -void ParallelContainerBase::onNewSolution(const SolutionBase &s) +size_t ParallelContainerBase::numFailures() const +{ + return pimpl()->failures_.size(); +} + +void ParallelContainerBase::processFailures(const Stage::SolutionProcessor &processor) const +{ + for(const SolutionBase* f : pimpl()->failures_) + if (!processor(*f)) + break; +} + +void ParallelContainerBase::onNewSolution(const SolutionBase& s) +{ + liftSolution(&s); +} + +void ParallelContainerBase::liftSolution(const SolutionBase* solution, double cost) { auto impl = pimpl(); - auto it = impl->solutions_.insert(WrappedSolution(impl, &s)); - impl->liftSolution(*it, s.start(), s.end()); + // create new WrappedSolution instance + auto wit = impl->wrapped_solutions_.insert(impl->wrapped_solutions_.end(), WrappedSolution(impl, solution, cost)); + + if (wit->isFailure()) { + wit->setStartState(*solution->start()); + wit->setEndState(*solution->end()); + impl->failures_.push_back(&*wit); + } else { + impl->solutions_.insert(&*wit); + impl->liftSolution(*wit, solution->start(), solution->end()); + } + impl->newSolution(*wit); } +void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t) +{ + auto impl = pimpl(); + assert(impl->prevEnds() && impl->nextStarts()); + + t.setCreator(impl); + // store newly created solution (otherwise it's lost) + auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t)); + + if (it->isFailure()) { + auto state_it = impl->states_.insert(impl->states_.end(), std::move(state)); + it->setStartState(*state_it); + it->setEndState(*state_it); + impl->failures_.push_back(&*it); + } else { + // directly spawn states in push interfaces + impl->prevEnds()->add(InterfaceState(state), NULL, &*it); + impl->nextStarts()->add(std::move(state), &*it, NULL); + impl->solutions_.insert(&*it); + } + impl->newSolution(*it); +} + + +WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name) + : ParallelContainerBasePrivate(me, name) +{} + WrapperBase::WrapperBase(const std::string &name, Stage::pointer &&child) : WrapperBase(new WrapperBasePrivate(this, name), std::move(child)) @@ -568,79 +634,18 @@ Stage* WrapperBase::wrapped() return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get(); } - -WrapperPrivate::WrapperPrivate(Wrapper *me, const std::string &name) - : WrapperBasePrivate(me, name) -{} - - -Wrapper::Wrapper(WrapperPrivate *impl, Stage::pointer &&child) - : WrapperBase(impl, std::move(child)) -{} -Wrapper::Wrapper(const std::string &name, Stage::pointer &&child) - : Wrapper(new WrapperPrivate(this, name), std::move(child)) -{} - -void Wrapper::reset() -{ - WrapperBase::reset(); - pimpl()->solutions_.clear(); -} - -bool Wrapper::canCompute() const +bool WrapperBase::canCompute() const { return wrapped()->pimpl()->canCompute(); } -bool Wrapper::compute() +bool WrapperBase::compute() { size_t num_before = numSolutions(); wrapped()->pimpl()->compute(); return numSolutions() > num_before; } -size_t Wrapper::numSolutions() const -{ - return pimpl()->solutions_.size(); -} - -size_t Wrapper::numFailures() const -{ - return pimpl()->failures_.size(); -} - -void Wrapper::processSolutions(const Stage::SolutionProcessor &processor) const -{ - for(const auto& s : pimpl()->solutions_) - if (!processor(*s)) - break; -} - -void Wrapper::processFailures(const Stage::SolutionProcessor &processor) const -{ - for(const auto& s : pimpl()->failures_) - if (!processor(*s)) - break; -} - -void Wrapper::spawn(InterfaceState &&state, std::unique_ptr&& s) -{ - auto impl = pimpl(); - s->setCreator(impl); - SolutionBase* solution = s.get(); - if (s->isFailure()) { - impl->failure_states_.emplace_back(std::move(state)); - s->setStartState(impl->failure_states_.back()); - s->setEndState(impl->failure_states_.back()); - impl->failures_.emplace_back(std::move(s)); - } else { - impl->solutions_.insert(std::move(s)); - impl->prevEnds()->add(InterfaceState(state), NULL, solution); - impl->nextStarts()->add(std::move(state), solution, NULL); - } - impl->newSolution(*solution); -} - bool Alternatives::canCompute() const { diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 8ae8fbda..f5f2e0d4 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -20,7 +20,7 @@ namespace moveit { namespace task_constructor { namespace stages { ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) - : Wrapper(name, std::move(child)) + : WrapperBase(name, std::move(child)) { auto& p = properties(); p.declare("timeout", 1.0); @@ -218,17 +218,17 @@ void ComputeIK::onNewSolution(const SolutionBase &s) start_time = now; planning_scene::PlanningSceneConstPtr scene = s.start()->scene(); - std::unique_ptr solution(new SubTrajectory()); + SubTrajectory solution; // include markers from original solution - std::copy(s.markers().begin(), s.markers().end(), std::back_inserter(solution->markers())); + std::copy(s.markers().begin(), s.markers().end(), std::back_inserter(solution.markers())); // frame at target pose target_pose_msg.header.frame_id = scene->getPlanningFrame(); - rviz_marker_tools::appendFrame(solution->markers(), target_pose_msg, 0.1, "ik frame"); + rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame"); if (succeeded) { - solution->setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); + solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); // create a new scene for each solution as they will have different robot states planning_scene::PlanningScenePtr new_scene = s.start()->scene()->diff(); robot_state::RobotState& robot_state = new_scene->getCurrentStateNonConst(); @@ -240,11 +240,11 @@ void ComputeIK::onNewSolution(const SolutionBase &s) auto appender = [&solution](visualization_msgs::Marker& marker, const std::string& name) { marker.ns = "ik solution"; marker.color.a *= 0.5; - solution->markers().push_back(marker); + solution.markers().push_back(marker); }; generateVisualMarkers(robot_state, appender, jmg->getLinkModelNames()); } else { - solution->setCost(std::numeric_limits::infinity()); + solution.setCost(std::numeric_limits::infinity()); } spawn(InterfaceState(scene), std::move(solution)); From 381edf2d22de820b3de3e44514e081a1f087ef08 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 16 Oct 2017 14:45:44 +0200 Subject: [PATCH 12/16] ModifyPlanningScene stage - attach / detach objects to robot - enable / disable collision pairs - works in either direction (FORWARD + BACKWARD) --- .../stages/modify_planning_scene.h | 173 ++++++++++++++++++ core/include/moveit/task_constructor/task.h | 1 + core/src/stages/CMakeLists.txt | 2 + core/src/stages/modify_planning_scene.cpp | 77 ++++++++ core/src/task.cpp | 4 + 5 files changed, 257 insertions(+) create mode 100644 core/include/moveit/task_constructor/stages/modify_planning_scene.h create mode 100644 core/src/stages/modify_planning_scene.cpp diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h new file mode 100644 index 00000000..1681479a --- /dev/null +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -0,0 +1,173 @@ +#pragma once + +#include +#include +#include + +namespace moveit { namespace planning_interface { +MOVEIT_CLASS_FORWARD(MoveGroupInterface) +} } + +namespace moveit { namespace task_constructor { namespace stages { + +class ModifyPlanningScene : public PropagatingEitherWay { +public: + typedef std::vector Names; + ModifyPlanningScene(std::string name); + + bool computeForward(const InterfaceState& from) override; + bool computeBackward(const InterfaceState& to) override; + + /// methods to attach objects to a robot link + template + void attachObjects(const C& objects, const std::string& attach_link); + template + void detachObjects(const C& objects, const std::string& attach_link); + /// attachObjects() and detachObjects() forward to this method, setting attach = true resp. false + void attachObjects(const Names& objects, const std::string& attach_link, bool attach); + + /// method to enable / disable collision between object pairs + template + void enableCollisions(const T1& first, const T2& second, bool enable_collision); + /// enable / disable all collisions for given object + void enableCollisions(const std::string& object, bool enable_collision); + +protected: + // list of objects to attach (true) / detach (false) to a given link + std::map > attach_objects_; + + // list of objects to mutually + struct CollisionMatrixPairs { + Names first; + Names second; + bool enable; + }; + std::list collision_matrix_edits_; + +protected: + // apply stored modifications to scene + void attachObjects(planning_scene::PlanningScene &scene, const std::pair >& pair, bool invert); + void setCollisions(planning_scene::PlanningScene &scene, const CollisionMatrixPairs& pairs, bool invert); + planning_scene::PlanningScenePtr apply(const planning_scene::PlanningSceneConstPtr &scene, bool invert); +}; + + +// As C++ doesn't allow for partial specialization of functions / methods, we need to use a helper class +namespace detail { + +// generic implementation +template struct AttachHelper { + static void impl(ModifyPlanningScene* stage, const T& objects, const std::string& link, bool attach) { + static_assert(std::is_base_of::value, "T must be a container of std::strings"); + stage->attachObjects(ModifyPlanningScene::Names(objects.cbegin(), objects.cend()), link, attach); + } +}; + +// specialization for Names +template<> +struct AttachHelper { + static void impl(ModifyPlanningScene* stage, const ModifyPlanningScene::Names& objects, const std::string& link, bool attach) { + stage->attachObjects(objects, link, attach); + } +}; + +// specialization for std::string +template<> +struct AttachHelper { + static void impl(ModifyPlanningScene* stage, const std::string& object, const std::string& link, bool attach) { + stage->attachObjects(ModifyPlanningScene::Names({object}), link, attach); + } +}; +// specialization for string literal +template +struct AttachHelper { + static void impl(ModifyPlanningScene* stage, const char object[N], const std::string& link, bool attach) { + stage->attachObjects(ModifyPlanningScene::Names({object}), link, attach); + } +}; + +} // namespace detail + +// implementation of methods forwards to AttachHelper::impl() +template +void ModifyPlanningScene::attachObjects(const T& first, const std::string& attach_link) { + detail::AttachHelper::impl(this, first, attach_link, true); +} +template +void ModifyPlanningScene::detachObjects(const T& first, const std::string& attach_link) { + detail::AttachHelper::impl(this, first, attach_link, false); +} + + + +// specialization for Names, implemented in .cpp +template <> +void ModifyPlanningScene::enableCollisions +(const ModifyPlanningScene::Names& first, const ModifyPlanningScene::Names& second, bool enable_collision); + +// As C++ doesn't allow for partial specialization of functions / methods, we need to use a helper class +namespace detail { + +// generic implementation +template struct CollisionHelper { + static void impl(ModifyPlanningScene* stage, const T1& first, const T2& second, bool enable) { + static_assert(std::is_base_of::value, "T1 must be a container of std::strings"); + static_assert(std::is_base_of::value, "T2 must be a container of std::strings"); + stage->enableCollisions(ModifyPlanningScene::Names(first.cbegin(), first.cend()), + ModifyPlanningScene::Names(second.cbegin(), second.cend()), enable); + } +}; + +// specialization for Names +template<> +struct CollisionHelper { + static void impl(ModifyPlanningScene* stage, const ModifyPlanningScene::Names& first, const ModifyPlanningScene::Names& second, bool enable) { + stage->enableCollisions(first, second, enable); + } +}; + +// specialization for pair of std::string +template<> +struct CollisionHelper { + static void impl(ModifyPlanningScene* stage, const std::string& first, const std::string& second, bool enable) { + stage->enableCollisions(ModifyPlanningScene::Names({first}), ModifyPlanningScene::Names({second}), enable); + } +}; +// specialization for pair of string literals +template +struct CollisionHelper { + static void impl(ModifyPlanningScene* stage, const char first[N1], const char second[N2], bool enable) { + stage->enableCollisions(ModifyPlanningScene::Names({std::string(first)}), + ModifyPlanningScene::Names({std::string(second)}), enable); + } +}; + +// specialization for std::string, container +template +struct CollisionHelper { + static void impl(ModifyPlanningScene* stage, const std::string& first, const T2& second, bool enable) { + stage->enableCollisions(ModifyPlanningScene::Names({first}), second, enable); + } +}; +// specialization for string literal, container +template +struct CollisionHelper { + static void impl(ModifyPlanningScene* stage, const char first[N1], const T2& second, bool enable) { + stage->enableCollisions(ModifyPlanningScene::Names({std::string(first)}), second, enable); + } +}; + +} // namespace detail + +// implementation of method forwards to CollisionHelper::impl() +template +inline void ModifyPlanningScene::enableCollisions(const T1& first, const T2& second, bool enable_collision) { + detail::CollisionHelper::impl(this, first, second, enable_collision); +} + +// single-object variant forwards to empty Names in second arg +inline void ModifyPlanningScene::enableCollisions(const std::string &object, bool enable_collision) { + enableCollisions(Names({object}), Names(), enable_collision); +} + +} } } diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index b0affc3f..3ed07fba 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -76,6 +76,7 @@ public: ~Task(); std::string id() const; + const moveit::core::RobotModelPtr getRobotModel() const; void add(Stage::pointer &&stage); void clear() override; diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index cfb89072..b2cd5d8c 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -4,6 +4,7 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/current_state.h ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/gripper.h + ${PROJECT_INCLUDE}/stages/modify_planning_scene.h ${PROJECT_INCLUDE}/stages/move.h cartesian_position_motion.cpp @@ -11,6 +12,7 @@ add_library(${PROJECT_NAME}_stages compute_ik.cpp generate_grasp_pose.cpp gripper.cpp + modify_planning_scene.cpp move.cpp ) target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp new file mode 100644 index 00000000..eee087c3 --- /dev/null +++ b/core/src/stages/modify_planning_scene.cpp @@ -0,0 +1,77 @@ +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +ModifyPlanningScene::ModifyPlanningScene(std::string name) + : PropagatingEitherWay(name) +{} + +void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& attach_link, bool attach) +{ + auto it_inserted = attach_objects_.insert(std::make_pair(attach_link, std::make_pair(Names(), attach))); + Names &o = it_inserted.first->second.first; + o.insert(o.end(), objects.begin(), objects.end()); +} + +template <> +void ModifyPlanningScene::enableCollisions +(const ModifyPlanningScene::Names& first, const ModifyPlanningScene::Names& second, bool enable_collision) { + collision_matrix_edits_.push_back(CollisionMatrixPairs({first, second, enable_collision})); +} + +bool ModifyPlanningScene::computeForward(const InterfaceState &from){ + planning_scene::PlanningScenePtr to = apply(from.scene(), false); + sendForward(from, InterfaceState(to), robot_trajectory::RobotTrajectoryPtr()); + return true; +} + +bool ModifyPlanningScene::computeBackward(const InterfaceState &to) +{ + planning_scene::PlanningScenePtr from = apply(to.scene(), true); + sendBackward(InterfaceState(from), to, robot_trajectory::RobotTrajectoryPtr()); + return true; +} + +void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene &scene, const std::pair >& pair, bool invert) +{ + moveit_msgs::AttachedCollisionObject obj; + obj.link_name = pair.first; + bool attach = pair.second.second; + if (invert) attach = !attach; + obj.object.operation = attach ? (int8_t) moveit_msgs::CollisionObject::ADD + : (int8_t) moveit_msgs::CollisionObject::REMOVE; + for (const std::string& name : pair.second.first) { + obj.object.id = name; + scene.processAttachedCollisionObjectMsg(obj); + } +} + +void ModifyPlanningScene::setCollisions(planning_scene::PlanningScene &scene, const CollisionMatrixPairs& pairs, bool invert) +{ + collision_detection::AllowedCollisionMatrix& acm = scene.getAllowedCollisionMatrixNonConst(); + bool enable = invert ? !pairs.enable : pairs.enable; + if (pairs.second.empty()) { + for (const auto &name : pairs.first) + acm.setEntry(name, enable); + } else + acm.setEntry(pairs.first, pairs.second, enable); +} + +// invert indicates, whether to detach instead of attach (and vice versa) +// as well as to disable instead of enable collision (and vice versa) +planning_scene::PlanningScenePtr ModifyPlanningScene::apply(const planning_scene::PlanningSceneConstPtr &scene, bool invert) +{ + planning_scene::PlanningScenePtr result = scene->diff(); + // attach/detach objects + for (const auto &pair : attach_objects_) + attachObjects(*result, pair, invert); + + // enable/disable collisions + for (const auto &pairs : collision_matrix_edits_) + setCollisions(*result, pairs, invert); + return result; +} + +} } } diff --git a/core/src/task.cpp b/core/src/task.cpp index 2d19755e..3041a2eb 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -68,6 +68,10 @@ void Task::initModel () { if( !rml_->getModel() ) throw Exception("Task failed to construct RobotModel"); } +const moveit::core::RobotModelPtr Task::getRobotModel() const { + return rml_ ? rml_->getModel() : moveit::core::RobotModelPtr(); +} + void Task::initScene() { assert(rml_); From f006f0f3d61420444a13a1b867f868fc4d6d9649 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 5 Feb 2018 22:34:51 +0100 Subject: [PATCH 13/16] reduce code bloat using SFINAE template selection --- .../stages/modify_planning_scene.h | 227 ++++++++---------- .../moveit/task_constructor/type_traits.h | 66 +++++ core/src/stages/modify_planning_scene.cpp | 81 +++++-- 3 files changed, 230 insertions(+), 144 deletions(-) create mode 100644 core/include/moveit/task_constructor/type_traits.h diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index 1681479a..a30e1c7c 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -1,36 +1,112 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Robert Haschke + Desc: Modify planning scene +*/ + #pragma once #include -#include +#include +#include #include -namespace moveit { namespace planning_interface { -MOVEIT_CLASS_FORWARD(MoveGroupInterface) +namespace moveit { namespace core { +MOVEIT_CLASS_FORWARD(JointModelGroup) } } namespace moveit { namespace task_constructor { namespace stages { +/** Allow modification of planning scene + * + * This stage takes the incoming planning scene and applies previously scheduled + * changes to it, for example: + * - Modify allowed collision matrix, enabling or disabling collision pairs. + * - Attach or detach objects to robot links. + * - Spawn or remove objects. + */ class ModifyPlanningScene : public PropagatingEitherWay { public: typedef std::vector Names; - ModifyPlanningScene(std::string name); + typedef std::function ApplyCallback; + ModifyPlanningScene(const std::string& name = "modify planning scene"); bool computeForward(const InterfaceState& from) override; bool computeBackward(const InterfaceState& to) override; - /// methods to attach objects to a robot link - template - void attachObjects(const C& objects, const std::string& attach_link); - template - void detachObjects(const C& objects, const std::string& attach_link); - /// attachObjects() and detachObjects() forward to this method, setting attach = true resp. false + /// call an arbitrary function + void setCallback(const ApplyCallback& cb) { callback_ = cb; } + + /// attach or detach a list of objects to the given link void attachObjects(const Names& objects, const std::string& attach_link, bool attach); - /// method to enable / disable collision between object pairs - template - void enableCollisions(const T1& first, const T2& second, bool enable_collision); + /// conviency methods accepting a single object name + inline void attachObject(const std::string& object, const std::string& link); + inline void detachObject(const std::string& object, const std::string& link); + + /// conviency methods accepting any container of object names + template ::value && std::is_base_of::value>> + inline void attachObjects(const T& objects, const std::string& link) { + attachObjects(Names(objects.cbegin(), objects.cend()), link, true); + } + template ::value && std::is_base_of::value>> + inline void detachObjects(const T& objects, const std::string& link) { + attachObjects(Names(objects.cbegin(), objects.cend()), link, false); + } + + /// enable / disable collisions for each combination of pairs in first and second lists + void enableCollisions(const Names& first, const Names& second, bool enable_collision); /// enable / disable all collisions for given object - void enableCollisions(const std::string& object, bool enable_collision); + void enableCollisions(const std::string& object, bool enable_collision) { + enableCollisions(Names({object}), Names(), enable_collision); + } + + /// conveniency method accepting arbitrary container types + template ::value && std::is_base_of::value>, + typename E2 = typename std::enable_if_t::value && std::is_base_of::value>> + inline void enableCollisions(const T1& first, const T2& second, bool enable_collision) { + enableCollisions(Names(first.cbegin(), first.cend()), Names(second.cbegin(), second.cend()), enable_collision); + } + /// conveniency method accepting std::string and an arbitrary container of names + template ::value && std::is_base_of::value>> + inline void enableCollisions(const std::string& first, const T& second, bool enable_collision) { + enableCollisions(Names({first}), Names(second.cbegin(), second.cend()), enable_collision); + } + /// conveniency method accepting std::string and JointModelGroup + void enableCollisions(const std::string& first, const moveit::core::JointModelGroup& jmg, bool enable_collision); protected: // list of objects to attach (true) / detach (false) to a given link @@ -43,131 +119,22 @@ protected: bool enable; }; std::list collision_matrix_edits_; + ApplyCallback callback_; protected: // apply stored modifications to scene + InterfaceState apply(const InterfaceState &from, bool invert); void attachObjects(planning_scene::PlanningScene &scene, const std::pair >& pair, bool invert); - void setCollisions(planning_scene::PlanningScene &scene, const CollisionMatrixPairs& pairs, bool invert); - planning_scene::PlanningScenePtr apply(const planning_scene::PlanningSceneConstPtr &scene, bool invert); + void enableCollisions(planning_scene::PlanningScene &scene, const CollisionMatrixPairs& pairs, bool invert); }; -// As C++ doesn't allow for partial specialization of functions / methods, we need to use a helper class -namespace detail { - -// generic implementation -template struct AttachHelper { - static void impl(ModifyPlanningScene* stage, const T& objects, const std::string& link, bool attach) { - static_assert(std::is_base_of::value, "T must be a container of std::strings"); - stage->attachObjects(ModifyPlanningScene::Names(objects.cbegin(), objects.cend()), link, attach); - } -}; - -// specialization for Names -template<> -struct AttachHelper { - static void impl(ModifyPlanningScene* stage, const ModifyPlanningScene::Names& objects, const std::string& link, bool attach) { - stage->attachObjects(objects, link, attach); - } -}; - -// specialization for std::string -template<> -struct AttachHelper { - static void impl(ModifyPlanningScene* stage, const std::string& object, const std::string& link, bool attach) { - stage->attachObjects(ModifyPlanningScene::Names({object}), link, attach); - } -}; -// specialization for string literal -template -struct AttachHelper { - static void impl(ModifyPlanningScene* stage, const char object[N], const std::string& link, bool attach) { - stage->attachObjects(ModifyPlanningScene::Names({object}), link, attach); - } -}; - -} // namespace detail - -// implementation of methods forwards to AttachHelper::impl() -template -void ModifyPlanningScene::attachObjects(const T& first, const std::string& attach_link) { - detail::AttachHelper::impl(this, first, attach_link, true); -} -template -void ModifyPlanningScene::detachObjects(const T& first, const std::string& attach_link) { - detail::AttachHelper::impl(this, first, attach_link, false); +inline void ModifyPlanningScene::attachObject(const std::string &object, const std::string &link) { + attachObjects(Names({object}), link, true); } - - -// specialization for Names, implemented in .cpp -template <> -void ModifyPlanningScene::enableCollisions -(const ModifyPlanningScene::Names& first, const ModifyPlanningScene::Names& second, bool enable_collision); - -// As C++ doesn't allow for partial specialization of functions / methods, we need to use a helper class -namespace detail { - -// generic implementation -template struct CollisionHelper { - static void impl(ModifyPlanningScene* stage, const T1& first, const T2& second, bool enable) { - static_assert(std::is_base_of::value, "T1 must be a container of std::strings"); - static_assert(std::is_base_of::value, "T2 must be a container of std::strings"); - stage->enableCollisions(ModifyPlanningScene::Names(first.cbegin(), first.cend()), - ModifyPlanningScene::Names(second.cbegin(), second.cend()), enable); - } -}; - -// specialization for Names -template<> -struct CollisionHelper { - static void impl(ModifyPlanningScene* stage, const ModifyPlanningScene::Names& first, const ModifyPlanningScene::Names& second, bool enable) { - stage->enableCollisions(first, second, enable); - } -}; - -// specialization for pair of std::string -template<> -struct CollisionHelper { - static void impl(ModifyPlanningScene* stage, const std::string& first, const std::string& second, bool enable) { - stage->enableCollisions(ModifyPlanningScene::Names({first}), ModifyPlanningScene::Names({second}), enable); - } -}; -// specialization for pair of string literals -template -struct CollisionHelper { - static void impl(ModifyPlanningScene* stage, const char first[N1], const char second[N2], bool enable) { - stage->enableCollisions(ModifyPlanningScene::Names({std::string(first)}), - ModifyPlanningScene::Names({std::string(second)}), enable); - } -}; - -// specialization for std::string, container -template -struct CollisionHelper { - static void impl(ModifyPlanningScene* stage, const std::string& first, const T2& second, bool enable) { - stage->enableCollisions(ModifyPlanningScene::Names({first}), second, enable); - } -}; -// specialization for string literal, container -template -struct CollisionHelper { - static void impl(ModifyPlanningScene* stage, const char first[N1], const T2& second, bool enable) { - stage->enableCollisions(ModifyPlanningScene::Names({std::string(first)}), second, enable); - } -}; - -} // namespace detail - -// implementation of method forwards to CollisionHelper::impl() -template -inline void ModifyPlanningScene::enableCollisions(const T1& first, const T2& second, bool enable_collision) { - detail::CollisionHelper::impl(this, first, second, enable_collision); -} - -// single-object variant forwards to empty Names in second arg -inline void ModifyPlanningScene::enableCollisions(const std::string &object, bool enable_collision) { - enableCollisions(Names({object}), Names(), enable_collision); +inline void ModifyPlanningScene::detachObject(const std::string &object, const std::string &link) { + attachObjects(Names({object}), link, false); } } } } diff --git a/core/include/moveit/task_constructor/type_traits.h b/core/include/moveit/task_constructor/type_traits.h new file mode 100644 index 00000000..2404f7a5 --- /dev/null +++ b/core/include/moveit/task_constructor/type_traits.h @@ -0,0 +1,66 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Robert Haschke + Desc: type traits for SFINAE-based templating +*/ + +#pragma once + +#include + +namespace moveit { namespace task_constructor { + +// detect STL-like containers by presence of cbegin(), cend() methods +template +struct is_container : std::false_type {}; + +template +struct is_container_helper {}; + +template +struct is_container< + T, + std::conditional_t< + false, + is_container_helper< + typename T::const_iterator, + decltype(std::declval().cbegin()), + decltype(std::declval().cend()) + >, + void + > + > : public std::true_type {}; + +} } diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index eee087c3..32fe4099 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -1,10 +1,48 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Robert Haschke + Desc: Modify planning scene +*/ + #include #include #include namespace moveit { namespace task_constructor { namespace stages { -ModifyPlanningScene::ModifyPlanningScene(std::string name) +ModifyPlanningScene::ModifyPlanningScene(const std::string &name) : PropagatingEitherWay(name) {} @@ -15,26 +53,31 @@ void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& o.insert(o.end(), objects.begin(), objects.end()); } -template <> -void ModifyPlanningScene::enableCollisions -(const ModifyPlanningScene::Names& first, const ModifyPlanningScene::Names& second, bool enable_collision) { +void ModifyPlanningScene::enableCollisions(const Names& first, const Names& second, bool enable_collision) { collision_matrix_edits_.push_back(CollisionMatrixPairs({first, second, enable_collision})); } +void ModifyPlanningScene::enableCollisions(const std::string &first, const moveit::core::JointModelGroup &jmg, bool enable_collision) +{ + const auto& links = jmg.getLinkModelNamesWithCollisionGeometry(); + if (!links.empty()) + enableCollisions(Names({first}), links, enable_collision); +} + bool ModifyPlanningScene::computeForward(const InterfaceState &from){ - planning_scene::PlanningScenePtr to = apply(from.scene(), false); - sendForward(from, InterfaceState(to), robot_trajectory::RobotTrajectoryPtr()); + sendForward(from, apply(from, false), robot_trajectory::RobotTrajectoryPtr()); return true; } bool ModifyPlanningScene::computeBackward(const InterfaceState &to) { - planning_scene::PlanningScenePtr from = apply(to.scene(), true); - sendBackward(InterfaceState(from), to, robot_trajectory::RobotTrajectoryPtr()); + sendBackward(apply(to, true), to, robot_trajectory::RobotTrajectoryPtr()); return true; } -void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene &scene, const std::pair >& pair, bool invert) +void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene &scene, + const std::pair >& pair, + bool invert) { moveit_msgs::AttachedCollisionObject obj; obj.link_name = pair.first; @@ -48,7 +91,9 @@ void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene &scene, co } } -void ModifyPlanningScene::setCollisions(planning_scene::PlanningScene &scene, const CollisionMatrixPairs& pairs, bool invert) +void ModifyPlanningScene::enableCollisions(planning_scene::PlanningScene &scene, + const CollisionMatrixPairs& pairs, + bool invert) { collision_detection::AllowedCollisionMatrix& acm = scene.getAllowedCollisionMatrixNonConst(); bool enable = invert ? !pairs.enable : pairs.enable; @@ -61,16 +106,24 @@ void ModifyPlanningScene::setCollisions(planning_scene::PlanningScene &scene, co // invert indicates, whether to detach instead of attach (and vice versa) // as well as to disable instead of enable collision (and vice versa) -planning_scene::PlanningScenePtr ModifyPlanningScene::apply(const planning_scene::PlanningSceneConstPtr &scene, bool invert) +InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool invert) { - planning_scene::PlanningScenePtr result = scene->diff(); + planning_scene::PlanningScenePtr scene = from.scene()->diff(); + InterfaceState result(scene); + // initialize properties from input state + result.properties() = from.properties(); + // attach/detach objects for (const auto &pair : attach_objects_) - attachObjects(*result, pair, invert); + attachObjects(*scene, pair, invert); // enable/disable collisions for (const auto &pairs : collision_matrix_edits_) - setCollisions(*result, pairs, invert); + enableCollisions(*scene, pairs, invert); + + if (callback_) + callback_(scene, result.properties()); + return result; } From f35a6bc05d8a0bc006761f175a65c4a6504e263d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 4 Feb 2018 22:14:03 +0100 Subject: [PATCH 14/16] FixCollisionObjects stage --- .../stages/fix_collision_objects.h | 32 +++++++ core/src/stages/CMakeLists.txt | 2 + core/src/stages/fix_collision_objects.cpp | 89 +++++++++++++++++++ 3 files changed, 123 insertions(+) create mode 100644 core/include/moveit/task_constructor/stages/fix_collision_objects.h create mode 100644 core/src/stages/fix_collision_objects.cpp diff --git a/core/include/moveit/task_constructor/stages/fix_collision_objects.h b/core/include/moveit/task_constructor/stages/fix_collision_objects.h new file mode 100644 index 00000000..42bae0ea --- /dev/null +++ b/core/include/moveit/task_constructor/stages/fix_collision_objects.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#include + +namespace moveit { namespace planning_interface { +MOVEIT_CLASS_FORWARD(MoveGroupInterface) +} } + +namespace moveit { namespace task_constructor { namespace stages { + +class FixCollisionObjects : public PropagatingEitherWay { +public: + FixCollisionObjects(std::string name = "fix collisions of objects"); + + bool computeForward(const InterfaceState& from) override; + bool computeBackward(const InterfaceState& to) override; + + void setDistanceThreshold(double penetration); + +protected: + // apply stored modifications to scene + void moveCollisionObject(planning_scene::PlanningScene &scene, geometry_msgs::Pose pose, const std::string& object, const std::string& base_frame); + void checkCollisions(planning_scene::PlanningScene &scene, const std::string& object); + planning_scene::PlanningScenePtr apply(const planning_scene::PlanningSceneConstPtr &scene, bool invert) const; + +private: + double penetration_; + +}; + +} } } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index b2cd5d8c..292bb362 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -5,6 +5,7 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/gripper.h ${PROJECT_INCLUDE}/stages/modify_planning_scene.h + ${PROJECT_INCLUDE}/stages/fix_collision_objects.h ${PROJECT_INCLUDE}/stages/move.h cartesian_position_motion.cpp @@ -13,6 +14,7 @@ add_library(${PROJECT_NAME}_stages generate_grasp_pose.cpp gripper.cpp modify_planning_scene.cpp + fix_collision_objects.cpp move.cpp ) target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp new file mode 100644 index 00000000..e06816d4 --- /dev/null +++ b/core/src/stages/fix_collision_objects.cpp @@ -0,0 +1,89 @@ +#include + +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +FixCollisionObjects::FixCollisionObjects(std::string name) + : PropagatingEitherWay(name) +{} + +bool FixCollisionObjects::computeForward(const InterfaceState &from){ + planning_scene::PlanningScenePtr to = apply(from.scene(), false); + sendForward(from, InterfaceState(to), robot_trajectory::RobotTrajectoryPtr()); + return true; +} + +bool FixCollisionObjects::computeBackward(const InterfaceState &to) +{ + planning_scene::PlanningScenePtr from = apply(to.scene(), true); + sendBackward(InterfaceState(from), to, robot_trajectory::RobotTrajectoryPtr()); + return true; +} + +void FixCollisionObjects::setDistanceThreshold(double penetration) +{ + penetration_ = penetration; +} + +void FixCollisionObjects::checkCollisions(planning_scene::PlanningScene &scene, const std::string& object) +{ + collision_detection::AllowedCollisionMatrix acm = scene.getAllowedCollisionMatrix(); + + collision_detection::CollisionRequest collision_request; + collision_detection::CollisionResult collision_result; + collision_request.group_name = object; + collision_request.contacts = true; + collision_request.max_contacts = 100; + collision_request.max_contacts_per_pair = 5; + collision_request.verbose = false; + collision_request.distance = true; + + scene.checkCollision(collision_request, collision_result, scene.getCurrentState()); + + geometry_msgs::Pose pose; + + if(collision_result.collision) + { + std::cout<<"COLLIDING"<second.begin()->depth < penetration_) + { + pose.position.x = 0.3 + (collision_result.contacts.begin()->second.begin()->depth * collision_result.contacts.begin()->second.begin()->normal[0]); + pose.position.y = 0.23 + (collision_result.contacts.begin()->second.begin()->depth * collision_result.contacts.begin()->second.begin()->normal[2]); + pose.position.z = -0.02 + (collision_result.contacts.begin()->second.begin()->depth * collision_result.contacts.begin()->second.begin()->normal[1]); + + //correct collision + moveCollisionObject(scene, pose, object, "world"); + } + } + else + std::cout<<"No Collision!!!!!!!!!!!!!"<diff(); + + //Check collision of objects on the table in planning scene + (const_cast(this))->checkCollisions(*result, "object"); + + return result; +} + +} } } From 0d338a7f5f96310b0474533c68a7df9412f74c65 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 6 Feb 2018 09:02:35 +0100 Subject: [PATCH 15/16] visualize collisions --- .../stages/fix_collision_objects.h | 58 +++++-- core/src/stages/fix_collision_objects.cpp | 151 ++++++++++++------ 2 files changed, 145 insertions(+), 64 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/fix_collision_objects.h b/core/include/moveit/task_constructor/stages/fix_collision_objects.h index 42bae0ea..66b63edb 100644 --- a/core/include/moveit/task_constructor/stages/fix_collision_objects.h +++ b/core/include/moveit/task_constructor/stages/fix_collision_objects.h @@ -1,32 +1,62 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Elham Iravani, Robert Haschke + Desc: Fix collisions in input scene +*/ + #pragma once #include +#include #include - -namespace moveit { namespace planning_interface { -MOVEIT_CLASS_FORWARD(MoveGroupInterface) -} } +#include namespace moveit { namespace task_constructor { namespace stages { class FixCollisionObjects : public PropagatingEitherWay { public: - FixCollisionObjects(std::string name = "fix collisions of objects"); + FixCollisionObjects(const std::string& name = "fix collisions of objects"); bool computeForward(const InterfaceState& from) override; bool computeBackward(const InterfaceState& to) override; - void setDistanceThreshold(double penetration); - -protected: - // apply stored modifications to scene - void moveCollisionObject(planning_scene::PlanningScene &scene, geometry_msgs::Pose pose, const std::string& object, const std::string& base_frame); - void checkCollisions(planning_scene::PlanningScene &scene, const std::string& object); - planning_scene::PlanningScenePtr apply(const planning_scene::PlanningSceneConstPtr &scene, bool invert) const; + void setMaxPenetration(double penetration); private: - double penetration_; - + bool fixCollisions(planning_scene::PlanningScene &scene, std::deque& markers) const; + void fixCollision(planning_scene::PlanningScene &scene, geometry_msgs::Pose pose, const std::string& object) const; }; } } } diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index e06816d4..9b9fdf61 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -1,71 +1,132 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Elham Iravani, Robert Haschke + Desc: Fix collisions in input scene +*/ + #include #include #include -#include +#include +#include +#include +#include + +namespace vm = visualization_msgs; namespace moveit { namespace task_constructor { namespace stages { -FixCollisionObjects::FixCollisionObjects(std::string name) +FixCollisionObjects::FixCollisionObjects(const std::string &name) : PropagatingEitherWay(name) -{} +{ + auto& p = properties(); + p.declare("max_penetration", "maximally corrected penetration depth"); +} -bool FixCollisionObjects::computeForward(const InterfaceState &from){ - planning_scene::PlanningScenePtr to = apply(from.scene(), false); - sendForward(from, InterfaceState(to), robot_trajectory::RobotTrajectoryPtr()); - return true; +void FixCollisionObjects::setMaxPenetration(double penetration) +{ + setProperty("max_penetration", penetration); +} + +bool FixCollisionObjects::computeForward(const InterfaceState &from) +{ + planning_scene::PlanningScenePtr to = from.scene()->diff(); + SubTrajectory solution; + bool success = fixCollisions(*to, solution.markers()); + if (!success) solution.setCost(std::numeric_limits::infinity()); + sendForward(from, InterfaceState(to), std::move(solution)); + return success; } bool FixCollisionObjects::computeBackward(const InterfaceState &to) { - planning_scene::PlanningScenePtr from = apply(to.scene(), true); - sendBackward(InterfaceState(from), to, robot_trajectory::RobotTrajectoryPtr()); - return true; + planning_scene::PlanningScenePtr from = to.scene()->diff(); + SubTrajectory solution; + bool success = fixCollisions(*from, solution.markers()); + if (!success) solution.setCost(std::numeric_limits::infinity()); + sendBackward(InterfaceState(from), to, std::move(solution)); + return success; } -void FixCollisionObjects::setDistanceThreshold(double penetration) +bool FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &scene, std::deque &markers) const { - penetration_ = penetration; -} + const auto& props = properties(); + double penetration = props.get("max_penetration"); -void FixCollisionObjects::checkCollisions(planning_scene::PlanningScene &scene, const std::string& object) -{ - collision_detection::AllowedCollisionMatrix acm = scene.getAllowedCollisionMatrix(); + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + req.group_name = ""; // check collisions for complete robot + req.contacts = true; + req.max_contacts = 100; + req.max_contacts_per_pair = 5; + req.verbose = false; + req.distance = false; - collision_detection::CollisionRequest collision_request; - collision_detection::CollisionResult collision_result; - collision_request.group_name = object; - collision_request.contacts = true; - collision_request.max_contacts = 100; - collision_request.max_contacts_per_pair = 5; - collision_request.verbose = false; - collision_request.distance = true; - - scene.checkCollision(collision_request, collision_result, scene.getCurrentState()); + scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(), scene.getCurrentState(), + scene.getAllowedCollisionMatrix()); geometry_msgs::Pose pose; - if(collision_result.collision) - { - std::cout<<"COLLIDING"<second.begin()->depth < penetration_) - { - pose.position.x = 0.3 + (collision_result.contacts.begin()->second.begin()->depth * collision_result.contacts.begin()->second.begin()->normal[0]); - pose.position.y = 0.23 + (collision_result.contacts.begin()->second.begin()->depth * collision_result.contacts.begin()->second.begin()->normal[2]); - pose.position.z = -0.02 + (collision_result.contacts.begin()->second.begin()->depth * collision_result.contacts.begin()->second.begin()->normal[1]); + if (!res.collision) + return true; - //correct collision - moveCollisionObject(scene, pose, object, "world"); + vm::Marker m; + m.header.frame_id = scene.getPlanningFrame(); + m.ns = "collisions"; + rviz_marker_tools::setColor(m.color, rviz_marker_tools::RED); + + ROS_INFO_THROTTLE(1, "collision(s) detected"); + for (const auto& info : res.contacts) { + for (const auto& contact : info.second) { + tf::poseEigenToMsg(Eigen::Translation3d(contact.pos) * Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), contact.normal), m.pose); + rviz_marker_tools::makeArrow(m, contact.depth); + markers.push_back(m); } } - else - std::cout<<"No Collision!!!!!!!!!!!!!"<checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(), scene.getCurrentState(), + scene.getAllowedCollisionMatrix()); + return !res.collision; } -void FixCollisionObjects::moveCollisionObject(planning_scene::PlanningScene &scene, geometry_msgs::Pose pose, const std::string& object, const std::string& base_frame) +void FixCollisionObjects::fixCollision(planning_scene::PlanningScene &scene, geometry_msgs::Pose pose, const std::string& object) const { moveit_msgs::CollisionObject collision_obj; - collision_obj.header.frame_id = base_frame; + collision_obj.header.frame_id = scene.getPlanningFrame(); collision_obj.id = object; collision_obj.operation = moveit_msgs::CollisionObject::MOVE; @@ -76,14 +137,4 @@ void FixCollisionObjects::moveCollisionObject(planning_scene::PlanningScene &sce std::cout<<"Moving FAILED"<diff(); - - //Check collision of objects on the table in planning scene - (const_cast(this))->checkCollisions(*result, "object"); - - return result; -} - } } } From 724f0c3014b22f56b2fec4f8bf3a15281d17f1dd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 1 Feb 2018 23:19:14 +0100 Subject: [PATCH 16/16] modular planning Separate planning approaches (using MoveIt pipeline or computeCartesianPath) from stages. This allows to reuse planning in various stages without code duplication. Reworked stages: Move -> Connect Gripper + CartesianPositionMotion -> MoveTo CartesianPositionMotion -> MoveRelative --- .../task_constructor/solvers/cartesian_path.h | 69 +++++ .../solvers/pipeline_planner.h | 78 +++++ .../solvers/planner_interface.h | 94 ++++++ .../moveit/task_constructor/stages/connect.h | 57 ++++ .../task_constructor/stages/move_relative.h | 78 +++++ .../moveit/task_constructor/stages/move_to.h | 74 +++++ .../include/moveit/task_constructor/storage.h | 3 +- core/src/CMakeLists.txt | 8 + core/src/solvers/cartesian_path.cpp | 114 ++++++++ core/src/solvers/pipeline_planner.cpp | 137 +++++++++ core/src/solvers/planner_interface.cpp | 51 ++++ core/src/stage.cpp | 4 +- core/src/stages/CMakeLists.txt | 12 +- core/src/stages/connect.cpp | 73 +++++ core/src/stages/move_relative.cpp | 274 ++++++++++++++++++ core/src/stages/move_to.cpp | 193 ++++++++++++ 16 files changed, 1314 insertions(+), 5 deletions(-) create mode 100644 core/include/moveit/task_constructor/solvers/cartesian_path.h create mode 100644 core/include/moveit/task_constructor/solvers/pipeline_planner.h create mode 100644 core/include/moveit/task_constructor/solvers/planner_interface.h create mode 100644 core/include/moveit/task_constructor/stages/connect.h create mode 100644 core/include/moveit/task_constructor/stages/move_relative.h create mode 100644 core/include/moveit/task_constructor/stages/move_to.h create mode 100644 core/src/solvers/cartesian_path.cpp create mode 100644 core/src/solvers/pipeline_planner.cpp create mode 100644 core/src/solvers/planner_interface.cpp create mode 100644 core/src/stages/connect.cpp create mode 100644 core/src/stages/move_relative.cpp create mode 100644 core/src/stages/move_to.cpp diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h new file mode 100644 index 00000000..18ffced5 --- /dev/null +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -0,0 +1,69 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner, Robert Haschke + Desc: generate and validate a straight-line Cartesian path +*/ + +#pragma once + +#include + +namespace moveit { namespace task_constructor { namespace solvers { + +/** Use RobotState's computeCartesianPath() to generate a straigh-line path between to scenes */ +class CartesianPath : public PlannerInterface { +public: + CartesianPath(); + + void setGroup(const std::string &group) { setProperty("group", group); } + void setTimeout(double timeout) { setProperty("timeout", timeout); } + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + + bool plan(const planning_scene::PlanningSceneConstPtr from, + const planning_scene::PlanningSceneConstPtr to, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) override; + + bool plan(const planning_scene::PlanningSceneConstPtr from, + const moveit::core::LinkModel &link, + const Eigen::Affine3d& target, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) override; +}; + +} } } diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h new file mode 100644 index 00000000..804c800d --- /dev/null +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -0,0 +1,78 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Robert Haschke + Desc: plan using MoveIt's PlanningPipeline +*/ + +#pragma once + +#include +#include + +namespace planning_pipeline { +MOVEIT_CLASS_FORWARD(PlanningPipeline) +} + +namespace moveit { namespace task_constructor { namespace solvers { + +/** Use MoveIt's PlanningPipeline to plan a trajectory between to scenes */ +class PipelinePlanner : public PlannerInterface { +public: + PipelinePlanner(); + + void setGroup(const std::string &group) { setProperty("group", group); } + void setPlannerId(const std::string &planner) { setProperty("planner", planner); } + void setTimeout(double timeout) { setProperty("timeout", timeout); } + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + + bool plan(const planning_scene::PlanningSceneConstPtr from, + const planning_scene::PlanningSceneConstPtr to, + const core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) override; + + bool plan(const planning_scene::PlanningSceneConstPtr from, + const moveit::core::LinkModel &link, + const Eigen::Affine3d& target, + const core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) override; + +protected: + planning_pipeline::PlanningPipelinePtr planner_; +}; + +} } } diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h new file mode 100644 index 00000000..197f5ae0 --- /dev/null +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -0,0 +1,94 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Robert Haschke + Desc: planner interface +*/ + +#pragma once + +#include +#include +#include + +namespace planning_scene { +MOVEIT_CLASS_FORWARD(PlanningScene) +} +namespace robot_trajectory { +MOVEIT_CLASS_FORWARD(RobotTrajectory) +} +namespace moveit { namespace core { +MOVEIT_CLASS_FORWARD(LinkModel) +MOVEIT_CLASS_FORWARD(RobotModel) +MOVEIT_CLASS_FORWARD(JointModelGroup) +} } + +namespace moveit { namespace task_constructor { namespace solvers { + +MOVEIT_CLASS_FORWARD(PlannerInterface) +class PlannerInterface { + // these properties take precedence over stage properties + PropertyMap properties_; + +public: + PlannerInterface() {} + + PropertyMap& properties() { return properties_; } + const PropertyMap& properties() const { return properties_; } + + void setProperty(const std::string& name, const boost::any& value) { + properties_.set(name, value); + } + + virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0; + + /// plan trajectory between to robot states + virtual bool plan(const planning_scene::PlanningSceneConstPtr from, + const planning_scene::PlanningSceneConstPtr to, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) = 0; + + /// plan trajectory from current robot state to Cartesian target + virtual bool plan(const planning_scene::PlanningSceneConstPtr from, + const moveit::core::LinkModel &link, + const Eigen::Affine3d& target, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) = 0; +}; + +std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg); + +} } } diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h new file mode 100644 index 00000000..1d7ee2b4 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner, Robert Haschke + Desc: Connect arbitrary states by motion planning +*/ + +#pragma once + +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +class Connect : public Connecting { +public: + Connect(std::string name, const solvers::PlannerInterfacePtr &planner); + + void init(const planning_scene::PlanningSceneConstPtr &scene); + bool compute(const InterfaceState &from, const InterfaceState &to); + +protected: + solvers::PlannerInterfacePtr planner_; +}; + +} } } diff --git a/core/include/moveit/task_constructor/stages/move_relative.h b/core/include/moveit/task_constructor/stages/move_relative.h new file mode 100644 index 00000000..308cba43 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/move_relative.h @@ -0,0 +1,78 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner, Robert Haschke + Desc: Move to joint-state or Cartesian goal pose +*/ + +#pragma once + +#include +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +/** Perform a Cartesian motion relative to some link */ +class MoveRelative : public PropagatingEitherWay { +public: + MoveRelative(std::string name, const solvers::PlannerInterfacePtr& planner); + + void init(const planning_scene::PlanningSceneConstPtr &scene) override; + bool computeForward(const InterfaceState& from) override; + bool computeBackward(const InterfaceState& to) override; + + void setGroup(const std::string& group); + void setLink(const std::string& link); + + /// set minimum / maximum distance to move + void setMinDistance(double distance); + void setMaxDistance(double distance); + void setMinMaxDistance(double min_distance, double max_distance); + + /// perform twist motion on specified link + void along(const geometry_msgs::TwistStamped& twist); + /// translate link along given direction + void along(const geometry_msgs::Vector3Stamped& direction); + +protected: + bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene, + SubTrajectory &trajectory, Direction dir); + +protected: + solvers::PlannerInterfacePtr planner_; +}; + +} } } diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h new file mode 100644 index 00000000..08743580 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner, Robert Haschke + Desc: Move to joint-state or Cartesian goal pose +*/ + +#pragma once + +#include +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +class MoveTo : public PropagatingEitherWay { +public: + MoveTo(std::string name, const solvers::PlannerInterfacePtr& planner); + + void init(const planning_scene::PlanningSceneConstPtr &scene) override; + bool computeForward(const InterfaceState& from) override; + bool computeBackward(const InterfaceState& to) override; + + void setGroup(const std::string& group); + void setLink(const std::string& link); + + /// move link to given pose + void setGoal(const geometry_msgs::PoseStamped& pose); + /// move link to given point, keeping current orientation + void setGoal(const geometry_msgs::PointStamped& point); + /// move joint model group to given named pose + void setGoal(const std::string& joint_pose); + +protected: + bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene, + SubTrajectory &trajectory, Direction dir); + +protected: + solvers::PlannerInterfacePtr planner_; +}; + +} } } diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 9a3a47a3..d39a5ca8 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -234,13 +234,14 @@ public: {} robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; } + void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; } void fillMessage(moveit_task_constructor_msgs::Solution &msg, Introspection* introspection = nullptr) const override; private: // actual trajectory, might be empty - const robot_trajectory::RobotTrajectoryPtr trajectory_; + robot_trajectory::RobotTrajectoryPtr trajectory_; }; diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 0a87c55f..b5d02dc8 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -11,6 +11,10 @@ add_library(${PROJECT_NAME} ${PROJECT_INCLUDE}/task.h ${PROJECT_INCLUDE}/utils.h + ${PROJECT_INCLUDE}/solvers/planner_interface.h + ${PROJECT_INCLUDE}/solvers/cartesian_path.h + ${PROJECT_INCLUDE}/solvers/pipeline_planner.h + container.cpp introspection.cpp marker_tools.cpp @@ -18,6 +22,10 @@ add_library(${PROJECT_NAME} stage.cpp storage.cpp task.cpp + + solvers/planner_interface.cpp + solvers/cartesian_path.cpp + solvers/pipeline_planner.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) target_include_directories(${PROJECT_NAME} diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp new file mode 100644 index 00000000..163d0af2 --- /dev/null +++ b/core/src/solvers/cartesian_path.cpp @@ -0,0 +1,114 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner, Robert Haschke + Desc: generate and validate a straight-line Cartesian path +*/ + +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace solvers { + +CartesianPath::CartesianPath() +{ + auto& p = properties(); + p.declare("step_size", 0.01, "step size between consecutive waypoints"); + p.declare("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step"); + p.declare("min_fraction", 1.0, "fraction of motion required for success"); + p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); + p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); +} + +void CartesianPath::init(const core::RobotModelConstPtr &robot_model) +{ +} + +bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from, + const planning_scene::PlanningSceneConstPtr to, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) +{ + const moveit::core::LinkModel* link; + const std::string& link_name = solvers::getEndEffectorLink(jmg); + if (!(link = from->getRobotModel()->getLinkModel(link_name))) { + ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName()); + return false; + } + + // reach pose of forward kinematics + return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result); +} + +bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from, + const moveit::core::LinkModel &link, + const Eigen::Affine3d &target, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr &result) +{ + const auto& props = properties(); + planning_scene::PlanningScenePtr sandbox_scene = from->diff(); + + auto isValid = [&sandbox_scene](moveit::core::RobotState* state, + const moveit::core::JointModelGroup* jmg, + const double* joint_positions) { + state->setJointGroupPositions(jmg, joint_positions); + state->update(); + return !sandbox_scene->isStateColliding(const_cast(*state), jmg->getName()); + }; + + std::vector trajectory; + double achieved_fraction = sandbox_scene->getCurrentStateNonConst().computeCartesianPath( + jmg, trajectory, &link, target, true, + props.get("step_size"), + props.get("jump_threshold"), + isValid); + if (achieved_fraction >= props.get("min_fraction")) { + result.reset(new robot_trajectory::RobotTrajectory(sandbox_scene->getRobotModel(), jmg)); + for (const auto& waypoint : trajectory) + result->addSuffixWayPoint(waypoint, 0.0); + + trajectory_processing::IterativeParabolicTimeParameterization timing; + timing.computeTimeStamps(*result, + props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); + return true; + } + return false; +} + +} } } diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp new file mode 100644 index 00000000..c5c7fa54 --- /dev/null +++ b/core/src/solvers/pipeline_planner.cpp @@ -0,0 +1,137 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Robert Haschke + Desc: plan using MoveIt's PlanningPipeline +*/ + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace solvers { + +PipelinePlanner::PipelinePlanner() +{ + auto& p = properties(); + p.declare("planner", "", "planner id"); + + p.declare("num_planning_attempts", 1u, "number of planning attempts"); + p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); + p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); + p.declare("workspace_parameters", moveit_msgs::WorkspaceParameters(), "allowed workspace of mobile base?"); + + p.declare("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals"); + p.declare("goal_position_tolerance", 1e-4, "tolerance for reaching position goals"); + p.declare("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals"); +} + +void PipelinePlanner::init(const core::RobotModelConstPtr &robot_model) +{ + planner_ = Task::createPlanner(robot_model); +} + +void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, + const PropertyMap& p, + const moveit::core::JointModelGroup *jmg, + double timeout) +{ + req.group_name = jmg->getName(); + req.planner_id = p.get("planner"); + req.allowed_planning_time = timeout; + + req.num_planning_attempts = p.get("num_planning_attempts"); + req.max_velocity_scaling_factor = p.get("max_velocity_scaling_factor"); + req.max_acceleration_scaling_factor = p.get("max_acceleration_scaling_factor"); + req.workspace_parameters = p.get("workspace_parameters"); +} + +bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from, + const planning_scene::PlanningSceneConstPtr to, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) +{ + const auto& props = properties(); + moveit_msgs::MotionPlanRequest req; + initMotionPlanRequest(req, props, jmg, timeout); + + req.goal_constraints.resize(1); + req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( + to->getCurrentState(), jmg, + props.get("goal_joint_tolerance")); + + ::planning_interface::MotionPlanResponse res; + if(!planner_->generatePlan(from, req, res)) + return false; + + result = res.trajectory_; + return true; +} + +bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from, + const moveit::core::LinkModel &link, + const Eigen::Affine3d& target_eigen, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) + +{ + const auto& props = properties(); + moveit_msgs::MotionPlanRequest req; + initMotionPlanRequest(req, props, jmg, timeout); + + geometry_msgs::PoseStamped target; + target.header.frame_id = from->getPlanningFrame(); + tf::poseEigenToMsg(target_eigen, target.pose); + + req.goal_constraints.resize(1); + req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( + link.getName(), target, + props.get("goal_position_tolerance"), + props.get("goal_orientation_tolerance")); + + ::planning_interface::MotionPlanResponse res; + if(!planner_->generatePlan(from, req, res)) + return false; + + result = res.trajectory_; + return true; +} + +} } } diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp new file mode 100644 index 00000000..64cfa93b --- /dev/null +++ b/core/src/solvers/planner_interface.cpp @@ -0,0 +1,51 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Robert Haschke + Desc: planner interface +*/ + +#include +#include + +namespace moveit { namespace task_constructor { namespace solvers { + +std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg) +{ + // TODO: directly return LinkModel* + const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip(); + return link ? link->getName() : std::string(); +} + +} } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 8e0845f7..5a7f57ae 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -216,8 +216,8 @@ SubTrajectory& ComputeBasePrivate::addTrajectory(SubTrajectory&& trajectory) { return *solutions_.insert(std::move(trajectory)); } else if (me()->storeFailures()) { // only store failures when introspection is enabled - failures_.emplace_back(std::move(trajectory)); - return failures_.back(); + auto it = failures_.insert(failures_.end(), std::move(trajectory)); + return *it; } else return trajectory; } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index cfb89072..fcc35d01 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -1,15 +1,23 @@ add_library(${PROJECT_NAME}_stages - ${PROJECT_INCLUDE}/stages/cartesian_position_motion.h ${PROJECT_INCLUDE}/stages/compute_ik.h ${PROJECT_INCLUDE}/stages/current_state.h ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h + ${PROJECT_INCLUDE}/stages/connect.h + ${PROJECT_INCLUDE}/stages/move_to.h + ${PROJECT_INCLUDE}/stages/move_relative.h + + ${PROJECT_INCLUDE}/stages/cartesian_position_motion.h ${PROJECT_INCLUDE}/stages/gripper.h ${PROJECT_INCLUDE}/stages/move.h - cartesian_position_motion.cpp current_state.cpp compute_ik.cpp generate_grasp_pose.cpp + connect.cpp + move_to.cpp + move_relative.cpp + + cartesian_position_motion.cpp gripper.cpp move.cpp ) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp new file mode 100644 index 00000000..2a11d5f0 --- /dev/null +++ b/core/src/stages/connect.cpp @@ -0,0 +1,73 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner, Robert Haschke + Desc: Connect arbitrary states by motion planning +*/ + +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner) + : Connecting(name) + , planner_(planner) +{ + auto& p = properties(); + p.declare("timeout", 10.0, "planning timeout"); + p.declare("group", "name of planning group"); +} + +void Connect::init(const planning_scene::PlanningSceneConstPtr &scene) +{ + Connecting::init(scene); + planner_->init(scene->getRobotModel()); +} + +bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { + const auto& props = properties(); + const std::string& group = props.get("group"); + double timeout = props.get("timeout"); + const moveit::core::JointModelGroup* jmg = from.scene()->getRobotModel()->getJointModelGroup(group); + + robot_trajectory::RobotTrajectoryPtr trajectory; + if (!planner_->plan(from.scene(), to.scene(), jmg, timeout, trajectory)) + return false; + + connect(from, to, trajectory); + return true; +} + +} } } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp new file mode 100644 index 00000000..ec94f96b --- /dev/null +++ b/core/src/stages/move_relative.cpp @@ -0,0 +1,274 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner, Robert Haschke + Desc: Move link along Cartesian direction +*/ + +#include +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +MoveRelative::MoveRelative(std::string name, const solvers::PlannerInterfacePtr& planner) + : PropagatingEitherWay(name) + , planner_(planner) +{ + auto& p = properties(); + p.declare("timeout", 10.0, "planning timeout"); + p.declare("marker_ns", "", "marker namespace"); + p.declare("group", "name of planning group"); + p.declare("link", "", "link to move (default is tip of jmg)"); + p.declare("min_distance", "minimum distance to move"); + p.declare("max_distance", "maximum distance to move"); + + p.declare("twist", "Cartesian twist transform"); + p.declare("direction", "Cartesian translation direction"); +} + +void MoveRelative::setGroup(const std::string& group){ + setProperty("group", group); +} + +void MoveRelative::setLink(const std::string& link){ + setProperty("link", link); +} + +void MoveRelative::setMinDistance(double distance){ + setProperty("min_distance", distance); +} + +void MoveRelative::setMaxDistance(double distance){ + setProperty("max_distance", distance); +} + +void MoveRelative::setMinMaxDistance(double min_distance, double max_distance){ + setProperty("min_distance", min_distance); + setProperty("max_distance", max_distance); +} + +void MoveRelative::along(const geometry_msgs::TwistStamped &twist) +{ + setProperty("twist", twist); +} + +void MoveRelative::along(const geometry_msgs::Vector3Stamped &direction) +{ + setProperty("direction", direction); +} + +void MoveRelative::init(const planning_scene::PlanningSceneConstPtr &scene) +{ + PropagatingEitherWay::init(scene); + planner_->init(scene->getRobotModel()); +} + +bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, + SubTrajectory &trajectory, Direction dir) { + scene = state.scene()->diff(); + assert(scene->getRobotModel()); + + const auto& props = properties(); + double timeout = props.get("timeout"); + const std::string& group = props.get("group"); + const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group); + if (!jmg) { + ROS_WARN_STREAM("MoveTo: invalid joint model group: " << group); + return false; + } + + // only allow single target + size_t count_goals = props.countDefined({"twist", "direction"}); + if (count_goals != 1) { + if (count_goals == 0) ROS_WARN("MoveTo: no goal defined"); + else ROS_WARN("MoveTo: cannot plan to multiple goals"); + return false; + } + + // Cartesian targets require the link name + std::string link_name = props.get("link"); + const moveit::core::LinkModel* link; + if (link_name.empty()) + link_name = solvers::getEndEffectorLink(jmg); + if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) { + ROS_WARN_STREAM("MoveTo: no or invalid link name specified: " << link_name); + return false; + } + + Property prop = props.property("max_distance"); + double max_distance = prop.value().empty() ? 0.0 : std::abs(boost::any_cast(prop.value())); + prop = props.property("min_distance"); + double min_distance = prop.value().empty() ? -1.0 : boost::any_cast(prop.value()); + + bool use_rotation_distance = false; // measure achieved distance as rotation? + Eigen::Vector3d linear; // linear translation + Eigen::Vector3d angular; // angular rotation + double linear_norm = 0.0, angular_norm = 0.0; + + Eigen::Affine3d target_eigen; + const Eigen::Affine3d& link_pose = scene->getFrameTransform(link_name); + + boost::any goal = props.get("twist"); + if (!goal.empty()) { + const geometry_msgs::TwistStamped& target = boost::any_cast(goal); + const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id); + tf::vectorMsgToEigen(target.twist.linear, linear); + tf::vectorMsgToEigen(target.twist.angular, angular); + + linear_norm = linear.norm(); + angular_norm = angular.norm(); + angular /= angular_norm; // normalize angular + use_rotation_distance = linear_norm < std::numeric_limits::epsilon(); + + // use max distance? + if (max_distance > 0.0) { + double scale; + if (!use_rotation_distance) // non-zero linear motion defines distance + scale = max_distance / linear_norm; + else if (angular_norm > std::numeric_limits::epsilon()) + scale = max_distance / angular_norm; + linear *= scale; + linear_norm *= scale; + angular_norm *= scale; + } + + // invert direction? + if (dir == BACKWARD) { + linear *= -1.0; + angular *= -1.0; + } + + // compute absolute transform for link + linear = frame_pose.linear() * linear; + angular = frame_pose.linear() * angular; + target_eigen = link_pose; + target_eigen.linear() = target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular); + target_eigen.translation() += linear; + } + + goal = props.get("direction"); + if (!goal.empty()) { + const geometry_msgs::Vector3Stamped& target = boost::any_cast(goal); + const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id); + tf::vectorMsgToEigen(target.vector, linear); + + // use max distance? + if (max_distance > 0.0) { + linear.normalize(); + linear *= max_distance; + } + linear_norm = linear.norm(); + + // invert direction? + if (dir == BACKWARD) + linear *= -1.0; + + // compute absolute transform for link + linear = frame_pose.linear() * linear; + target_eigen = link_pose; + target_eigen.translation() += linear; + } + + robot_trajectory::RobotTrajectoryPtr robot_trajectory; + bool success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory); + + // min_distance reached? + if (success && min_distance > 0.0) { + const robot_state::RobotState& reached_state = robot_trajectory->getLastWayPoint(); + Eigen::Affine3d reached_pose = reached_state.getGlobalLinkTransform(link); + if (use_rotation_distance) { + Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose()); + success = rotation.angle() > min_distance; + } else + success = (reached_pose.translation() - link_pose.translation()).norm() > min_distance; + } + + // store result + if (success || (robot_trajectory && storeFailures())) { + scene->setCurrentState(robot_trajectory->getLastWayPoint()); + if (dir == BACKWARD) robot_trajectory->reverse(); + trajectory.setTrajectory(robot_trajectory); + } + + // add an arrow marker + visualization_msgs::Marker m; + m.ns = props.get("marker_ns"); + if (!m.ns.empty()) { + m.header.frame_id = scene->getPlanningFrame(); + if (linear_norm > 1e-3) { + rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN + : rviz_marker_tools::RED); + rviz_marker_tools::makeArrow(m, linear_norm); + auto quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), linear); + Eigen::Vector3d pos(link_pose.translation()); + if (dir == BACKWARD) { + // flip arrow direction + quat = quat * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()); + // arrow tip at goal_pose + pos += quat * Eigen::Vector3d(-linear_norm, 0, 0); + } + tf::pointEigenToMsg(pos, m.pose.position); + tf::quaternionEigenToMsg(quat, m.pose.orientation); + trajectory.markers().push_back(m); + } + } + + return success; +} + +bool MoveRelative::computeForward(const InterfaceState &from) +{ + planning_scene::PlanningScenePtr to; + SubTrajectory trajectory; + + bool success = compute(from, to, trajectory, FORWARD); + if (!success) trajectory.setCost(std::numeric_limits::infinity()); + sendForward(from, InterfaceState(to), std::move(trajectory)); + return success; +} + +bool MoveRelative::computeBackward(const InterfaceState &to) +{ + planning_scene::PlanningScenePtr from; + SubTrajectory trajectory; + + bool success = compute(to, from, trajectory, BACKWARD); + if (!success) trajectory.setCost(std::numeric_limits::infinity()); + sendBackward(InterfaceState(from), to, std::move(trajectory)); + return success; +} + +} } } diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp new file mode 100644 index 00000000..5903b089 --- /dev/null +++ b/core/src/stages/move_to.cpp @@ -0,0 +1,193 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner, Robert Haschke + Desc: Move to joint-state or Cartesian goal pose +*/ + +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +MoveTo::MoveTo(std::string name, const solvers::PlannerInterfacePtr& planner) + : PropagatingEitherWay(name) + , planner_(planner) +{ + auto& p = properties(); + p.declare("timeout", 10.0, "planning timeout"); + p.declare("group", "name of planning group"); + p.declare("link", "", "link to move (default is tip of jmg)"); + + p.declare("pose", "Cartesian target pose"); + p.declare("point", "Cartesian target point"); + p.declare("joint_pose", "named joint pose"); +} + +void MoveTo::setGroup(const std::string& group){ + setProperty("group", group); +} + +void MoveTo::setLink(const std::string& link){ + setProperty("link", link); +} + +void MoveTo::setGoal(const geometry_msgs::PoseStamped &pose) +{ + setProperty("pose", pose); +} + +void MoveTo::setGoal(const geometry_msgs::PointStamped &point) +{ + setProperty("point", point); +} + +void MoveTo::setGoal(const std::string &joint_pose) +{ + setProperty("joint_pose", joint_pose); +} + +void MoveTo::init(const planning_scene::PlanningSceneConstPtr &scene) +{ + PropagatingEitherWay::init(scene); + planner_->init(scene->getRobotModel()); +} + +bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, + SubTrajectory &trajectory, Direction dir) { + scene = state.scene()->diff(); + assert(scene->getRobotModel()); + + const auto& props = properties(); + double timeout = props.get("timeout"); + const std::string& group = props.get("group"); + const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group); + if (!jmg) { + ROS_WARN_STREAM("MoveTo: invalid joint model group: " << group); + return false; + } + + // only allow single target + size_t count_goals = props.countDefined({"joint_pose", "pose", "point"}); + if (count_goals != 1) { + if (count_goals == 0) ROS_WARN("MoveTo: no goal defined"); + else ROS_WARN("MoveTo: cannot plan to multiple goals"); + return false; + } + + robot_trajectory::RobotTrajectoryPtr robot_trajectory; + bool success = false; + + boost::any goal = props.get("joint_pose"); + if (!goal.empty()) { + const std::string& named_joint_pose = boost::any_cast(goal); + if (!scene->getCurrentStateNonConst().setToDefaultValues(jmg, named_joint_pose)) { + ROS_WARN("MoveTo: unknown joint pose '%s' for jmg '%s'", named_joint_pose.c_str(), group.c_str()); + return false; + } + success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory); + } else { + // Cartesian targets require the link name + std::string link_name = props.get("link"); + const moveit::core::LinkModel* link; + if (link_name.empty()) + link_name = solvers::getEndEffectorLink(jmg); + if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) { + ROS_WARN_STREAM("MoveTo: no or invalid link name specified: " << link_name); + return false; + } + + Eigen::Affine3d target_eigen; + + goal = props.get("pose"); + if (!goal.empty()) { + const geometry_msgs::PoseStamped& target = boost::any_cast(goal); + tf::poseMsgToEigen(target.pose, target_eigen); + + // transform target into global frame + const Eigen::Affine3d& frame = scene->getFrameTransform(target.header.frame_id); + target_eigen = frame * target_eigen; + } + + goal = props.get("point"); + if (!goal.empty()) { + const geometry_msgs::PointStamped& target = boost::any_cast(goal); + Eigen::Vector3d target_point; + tf::pointMsgToEigen(target.point, target_point); + + // transform target into global frame + const Eigen::Affine3d& frame = scene->getFrameTransform(target.header.frame_id); + target_point = frame * target_point; + + // retain link orientation + Eigen::Affine3d target_eigen = scene->getCurrentState().getGlobalLinkTransform(link_name); + target_eigen.translation() = target_point; + } + + success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory); + } + + // store result + if (success || (robot_trajectory && storeFailures())) { + scene->setCurrentState(robot_trajectory->getLastWayPoint()); + if (dir == BACKWARD) robot_trajectory->reverse(); + trajectory.setTrajectory(robot_trajectory); + } + + return success; +} + +bool MoveTo::computeForward(const InterfaceState &from){ + planning_scene::PlanningScenePtr to; + SubTrajectory trajectory; + + bool success = compute(from, to, trajectory, FORWARD); + if (!success) trajectory.setCost(std::numeric_limits::infinity()); + sendForward(from, InterfaceState(to), std::move(trajectory)); + return success; +} + +bool MoveTo::computeBackward(const InterfaceState &to) +{ + planning_scene::PlanningScenePtr from; + SubTrajectory trajectory; + + bool success = compute(to, from, trajectory, BACKWARD); + if (!success) trajectory.setCost(std::numeric_limits::infinity()); + sendBackward(InterfaceState(from), to, std::move(trajectory)); + return success; +} + +} } }