From ff297d29290868b98271890bddb986a5b11a06ca Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 2 Nov 2019 22:05:58 +0800 Subject: [PATCH] more tutorial demos --- demo/CMakeLists.txt | 6 ++ demo/config/mtc.rviz | 46 ++++++------ demo/launch/demo.launch | 8 +-- demo/launch/pickplace.launch | 8 +++ demo/src/cartesian.cpp | 132 +++++++++++++++++++++++++++++++++++ demo/src/modular.cpp | 124 ++++++++++++++++++++++++++++++++ demo/src/pick_place_task.cpp | 4 +- 7 files changed, 297 insertions(+), 31 deletions(-) create mode 100644 demo/launch/pickplace.launch create mode 100644 demo/src/cartesian.cpp create mode 100644 demo/src/modular.cpp diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index a32d2d8a..0960aec8 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -19,6 +19,12 @@ include_directories( ${catkin_INCLUDE_DIRS} ) +add_executable(cartesian src/cartesian.cpp) +target_link_libraries(cartesian ${catkin_LIBRARIES}) + +add_executable(modular src/modular.cpp) +target_link_libraries(modular ${catkin_LIBRARIES}) + add_library(${PROJECT_NAME}_lib src/pick_place_task.cpp) set_target_properties(${PROJECT_NAME}_lib PROPERTIES OUTPUT_NAME moveit_task_constructor_demo_pick_place) add_dependencies(${PROJECT_NAME}_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/demo/config/mtc.rviz b/demo/config/mtc.rviz index 83505a9b..cc8187fb 100644 --- a/demo/config/mtc.rviz +++ b/demo/config/mtc.rviz @@ -5,7 +5,6 @@ Panels: Property Tree Widget: Expanded: - /Motion Planning Tasks1 - - /Motion Planning Tasks1/Markers1 Splitter Ratio: 0.5393258333206177 Tree Height: 533 - Class: rviz/Help @@ -22,20 +21,20 @@ Panels: Name: Motion Planning Tasks Tasks View: property_splitter: - - 541 + - 540 - 0 solution_sorting: - column: 0 - order: 1 + column: 1 + order: 0 solutions_splitter: - - 328 - - 76 + - 306 + - 98 solutions_view_columns: - 38 - - 0 + - 52 - 0 tasks_view_columns: - - 250 + - 226 - 38 - 38 Preferences: @@ -70,7 +69,8 @@ Visualization Manager: Markers: All at once?: false Value: true - approach_object: true + x +0.2: true + y -0.3: true Name: Motion Planning Tasks Robot: Fixed Robot Color: 150; 50; 150 @@ -132,14 +132,14 @@ Visualization Manager: Value: true panda_link8: Alpha: 1 - Show Axes: false + Show Axes: true Show Trail: false panda_rightfinger: Alpha: 1 Show Axes: false Show Trail: false Value: true - Robot Alpha: 0.5 + Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true Use Fixed Robot Color: false @@ -154,9 +154,9 @@ Visualization Manager: Voxel Rendering: Occupied Voxels Show Trail: false State Display Time: 0.05 s - Task Solution Topic: /moveit_task_constructor_demo/pick_place_task/solution + Task Solution Topic: /mtc_tutorial/solution Tasks: - pick_place_task: 10 + Cartesian Path: 1 Trail Step Size: 1 Value: true Enabled: true @@ -175,25 +175,25 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 1.9269260168075562 + Distance: 1.3878222703933716 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: - X: 0.8292451500892639 - Y: -0.2452867180109024 - Z: 2.2351800055275817e-7 + X: 0.30880630016326904 + Y: -0.1259305477142334 + Z: 1.3560062370743253e-6 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.3197975158691406 + Pitch: 0.264797568321228 Target Frame: panda_link0 - Value: XYOrbit (rviz) - Yaw: 6.239960670471191 + Yaw: 4.939944744110107 Saved: ~ Window Geometry: Displays: @@ -207,9 +207,9 @@ Window Geometry: collapsed: false Motion Planning Tasks - Slider: collapsed: false - QMainWindow State: 000000ff00000000fd000000020000000000000166000002a6fc020000000efb000000100044006900730070006c006100790073010000003d000002a6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069000000027d0000004b0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000120020002d00200053006c00690064006500720000000000ffffffff00000000000000000000000100000198000002a6fc0200000002fb0000002a004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b0073010000003d00000254000000e500fffffffb0000003c004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b00730020002d00200053006c006900640065007201000002970000004c0000004100ffffff0000028f000002a600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 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 Views: collapsed: false - Width: 1433 - X: 472 + Width: 1621 + X: 249 Y: 25 diff --git a/demo/launch/demo.launch b/demo/launch/demo.launch index d6de926c..69b68398 100644 --- a/demo/launch/demo.launch +++ b/demo/launch/demo.launch @@ -5,7 +5,7 @@ - + @@ -30,10 +30,4 @@ - - - - - - diff --git a/demo/launch/pickplace.launch b/demo/launch/pickplace.launch new file mode 100644 index 00000000..a1318a8d --- /dev/null +++ b/demo/launch/pickplace.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/demo/src/cartesian.cpp b/demo/src/cartesian.cpp new file mode 100644 index 00000000..edf8a76b --- /dev/null +++ b/demo/src/cartesian.cpp @@ -0,0 +1,132 @@ +/********************************************************************* + * Copyright (c) 2019 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 the copyright holder 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 HOLDER 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 + Desc: Planning a simple sequence of Cartesian motions +*/ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace moveit::task_constructor; + +Task createTask() { + Task t; + t.stages()->setName("Cartesian Path"); + + const std::string group = "panda_arm"; + + // create Cartesian interpolation "planner" to be used in stages + auto cartesian = std::make_shared(); + + // start from a fixed robot state + t.loadRobotModel(); + auto scene = std::make_shared(t.getRobotModel()); + { + auto& state = scene->getCurrentStateNonConst(); + state.setToDefaultValues(state.getJointModelGroup(group), "ready"); + + auto fixed = std::make_unique("initial state"); + fixed->setState(scene); + t.add(std::move(fixed)); + } + + { + auto stage = std::make_unique("x +0.2", cartesian); + stage->setGroup(group); + geometry_msgs::Vector3Stamped direction; + direction.header.frame_id = "world"; + direction.vector.x = 0.2; + stage->setDirection(direction); + t.add(std::move(stage)); + } + + { + auto stage = std::make_unique("y -0.3", cartesian); + stage->setGroup(group); + geometry_msgs::Vector3Stamped direction; + direction.header.frame_id = "world"; + direction.vector.y = -0.3; + stage->setDirection(direction); + t.add(std::move(stage)); + } + + { // rotate about TCP + auto stage = std::make_unique("rz +45°", cartesian); + stage->setGroup(group); + geometry_msgs::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.z = M_PI / 4.; + stage->setDirection(twist); + t.add(std::move(stage)); + } + + { // move from reached state back to the original state, using joint interpolation + auto joint_interpolation = std::make_shared(); + stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation } }; + auto connect = std::make_unique("connect", planners); + t.add(std::move(connect)); + } + + { // final state is original state again + auto fixed = std::make_unique("final state"); + fixed->setState(scene); + t.add(std::move(fixed)); + } + + return t; +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "mtc_tutorial"); + // run an asynchronous spinner to communicate with the move_group node and rviz + ros::AsyncSpinner spinner(1); + spinner.start(); + + auto task = createTask(); + try { + if (task.plan()) + task.introspection().publishSolution(*task.solutions().front()); + } catch (const InitStageException& ex) { + std::cerr << "planning failed with exception" << std::endl << ex << task; + } + + ros::waitForShutdown(); // keep alive for interactive inspection in rviz + return 0; +} diff --git a/demo/src/modular.cpp b/demo/src/modular.cpp new file mode 100644 index 00000000..a112c0a4 --- /dev/null +++ b/demo/src/modular.cpp @@ -0,0 +1,124 @@ +/********************************************************************* + * Copyright (c) 2019 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 the copyright holder 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 HOLDER 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 + Desc: Planning a simple sequence of Cartesian motions +*/ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace moveit::task_constructor; + +std::unique_ptr createModule(const std::string& group) { + auto c = std::make_unique("Cartesian Path"); + c->setProperty("group", group); + + // create Cartesian interpolation "planner" to be used in stages + auto cartesian = std::make_shared(); + + { + auto stage = std::make_unique("x +0.2", cartesian); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + geometry_msgs::Vector3Stamped direction; + direction.header.frame_id = "world"; + direction.vector.x = 0.2; + stage->setDirection(direction); + c->insert(std::move(stage)); + } + + { + auto stage = std::make_unique("y -0.3", cartesian); + stage->properties().configureInitFrom(Stage::PARENT); + geometry_msgs::Vector3Stamped direction; + direction.header.frame_id = "world"; + direction.vector.y = -0.3; + stage->setDirection(direction); + c->insert(std::move(stage)); + } + + { // rotate about TCP + auto stage = std::make_unique("rz +45°", cartesian); + stage->properties().configureInitFrom(Stage::PARENT); + geometry_msgs::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.z = M_PI / 4.; + stage->setDirection(twist); + c->insert(std::move(stage)); + } + + { // move back to ready pose + auto stage = std::make_unique("moveTo ready", cartesian); + stage->properties().configureInitFrom(Stage::PARENT); + stage->setGoal("ready"); + c->insert(std::move(stage)); + } + return c; +} + +Task createTask() { + Task t; + t.stages()->setName("Reusable Containers"); + t.add(std::make_unique("current")); + + const std::string group = "panda_arm"; + t.add(createModule(group)); + t.add(createModule(group)); + + return t; +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "mtc_tutorial"); + // run an asynchronous spinner to communicate with the move_group node and rviz + ros::AsyncSpinner spinner(1); + spinner.start(); + + auto task = createTask(); + try { + if (task.plan()) + task.introspection().publishSolution(*task.solutions().front()); + } catch (const InitStageException& ex) { + std::cerr << "planning failed with exception" << std::endl << ex << task; + } + + ros::waitForShutdown(); // keep alive for interactive inspection in rviz + return 0; +} diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 53d7365b..cfcde431 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -89,8 +89,10 @@ void PickPlaceTask::init() { // Reset ROS introspection before constructing the new object // TODO(henningkayser): verify this is a bug, fix if possible task_.reset(); - task_.reset(new moveit::task_constructor::Task(task_name_)); + task_.reset(new moveit::task_constructor::Task()); + Task& t = *task_; + t.stages()->setName(task_name_); t.loadRobotModel(); // Sampling planner