From 3fe4f951deb82625a9a04574fa86260c9617054d Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Mon, 22 Jul 2019 11:53:25 -0600 Subject: [PATCH 01/18] Add demo package for tutorial --- demo/.clang-format | 66 +++ demo/CMakeLists.txt | 120 +++++ demo/README.md | 11 + demo/config/panda_config.yaml | 42 ++ .../pick_place_task.h | 125 +++++ demo/launch/demo.launch | 11 + demo/package.xml | 26 + demo/src/moveit_task_constructor_demo.cpp | 124 +++++ demo/src/pick_place_task.cpp | 469 ++++++++++++++++++ 9 files changed, 994 insertions(+) create mode 100644 demo/.clang-format create mode 100644 demo/CMakeLists.txt create mode 100644 demo/README.md create mode 100644 demo/config/panda_config.yaml create mode 100644 demo/include/moveit_task_constructor_demo/pick_place_task.h create mode 100644 demo/launch/demo.launch create mode 100644 demo/package.xml create mode 100644 demo/src/moveit_task_constructor_demo.cpp create mode 100644 demo/src/pick_place_task.cpp diff --git a/demo/.clang-format b/demo/.clang-format new file mode 100644 index 000000000..eca6051ae --- /dev/null +++ b/demo/.clang-format @@ -0,0 +1,66 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortLoopsOnASingleLine: false +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} +... diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt new file mode 100644 index 000000000..16a57a55f --- /dev/null +++ b/demo/CMakeLists.txt @@ -0,0 +1,120 @@ +cmake_minimum_required(VERSION 2.8.3) +project(moveit_task_constructor_demo) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + roscpp + moveit_core + moveit_task_constructor_core + moveit_visual_tools + moveit_ros_planning_interface +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES moveit_task_constructor_demo +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/pick_place_task.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_moveit_task_constructor_demo src/moveit_task_constructor_demo.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +set_target_properties(${PROJECT_NAME}_moveit_task_constructor_demo PROPERTIES OUTPUT_NAME moveit_task_constructor_demo PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(${PROJECT_NAME}_moveit_task_constructor_demo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_moveit_task_constructor_demo + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_moveit_task_constructor_demo + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(FILES + launch/demo.launch + # myfile2 + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_moveit_task_constructor_demo.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/demo/README.md b/demo/README.md new file mode 100644 index 000000000..0205f2a50 --- /dev/null +++ b/demo/README.md @@ -0,0 +1,11 @@ +# moveit_task_constructor_demo + +Description: A simple pick & place demo using MoveIt Task Constructor. This uses the Panda from Franka Emika + +Developed by Henning Kayser & Simon Goldstien at [PickNik Consulting](http://picknik.ai/) + +## Run + +Run demo + + roslaunch moveit_task_constructor_demo demo.launch diff --git a/demo/config/panda_config.yaml b/demo/config/panda_config.yaml new file mode 100644 index 000000000..642f12074 --- /dev/null +++ b/demo/config/panda_config.yaml @@ -0,0 +1,42 @@ +# planning group names +group_name: "panda_arm" +eef_name: "hand" +hand_name: "hand" +hand_frame: "panda_link8" + +# surface +table_surface_frame: "world" +object_surface_frame: "table" +surface_link: "table" + +# collision object +# CYLINDER object specifications +object_name: "object" +object_height: 0.25 +object_radius: 0.02 +object_pos_x: 0.0 +object_pos_y: -0.1 + +table_height: 0.1 +table_width: 0.4 +table_length: 0.4 +table_pos_x: 0.5 +table_pos_y: -0.25 + +# gripper grasp frame transform +grasp_offset_x: 0.15 # horizontal +grasp_offset_z: 0.0 # vertical +grasp_rotation_z: 0.5 # rotation around Z access + +# place pose metrics (x/y position on table_top) +place_pos_x: 0.1 +place_pos_y: 0.1 +place_surface_offset: 0.10001 # place offset from table + +# valid distance range when approaching an object for picking +approach_object_min_dist: 0.10 +approach_object_max_dist: 0.15 + +# valid height range when lifting an object after pick +lift_object_min_dist: 0.01 +lift_object_max_dist: 0.1 diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h new file mode 100644 index 000000000..99d5adcb0 --- /dev/null +++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h @@ -0,0 +1,125 @@ +/********************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019 PickNik LLC. + * 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: Henning Kayser, Simon Goldstein + Desc: A demo to show MoveIt Task Constructor in action +*/ + +// ROS +#include + +// MoveIt +#include +#include +#include + +// MTC +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#ifndef MOVEIT_TASK_CONSTRUCTOR_DEMO_PICK_PLACE_TASK_H +#define MOVEIT_TASK_CONSTRUCTOR_DEMO_PICK_PLACE_TASK_H + +namespace moveit_task_constructor_demo +{ +using namespace moveit::task_constructor; + +class PickPlaceTask +{ +public: + PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); + ~PickPlaceTask() = default; + + void init(); + + bool plan(); + + bool execute(); + +private: + ros::NodeHandle nh_; + + std::string task_name_; + moveit::task_constructor::TaskPtr task_; + + // planning group properties + std::string group_name_; + std::string eef_name_; + std::string hand_name_; + std::string hand_frame_; + + // object + surface + std::vector support_surfaces_; + std::string table_surface_frame_; + std::string object_surface_frame_; + std::string surface_link_; + std::string object_name_; + // std::string table_name_; + double object_height_; + double object_radius_; + double table_height_; + double table_length_; + double table_width_; + + // execution + actionlib::SimpleActionClient execute_; + + // ros params + double approach_object_min_dist_; + double approach_object_max_dist_; + double lift_object_min_dist_; + double lift_object_max_dist_; + double place_pos_x_; + double place_pos_y_; + double place_surface_offset_; + + Eigen::Isometry3d grasp_frame_transform_; +}; +} // moveit_task_constructor_demo +#endif // MOVEIT_TASK_CONSTRUCTOR_DEMO_PICK_PLACE_TASK_H diff --git a/demo/launch/demo.launch b/demo/launch/demo.launch new file mode 100644 index 000000000..9a0700db5 --- /dev/null +++ b/demo/launch/demo.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/demo/package.xml b/demo/package.xml new file mode 100644 index 000000000..e2d66e8d3 --- /dev/null +++ b/demo/package.xml @@ -0,0 +1,26 @@ + + + moveit_task_constructor_demo + 0.0.1 + The moveit_task_constructor_demo package + + simon Goldstein + Simon Goldstein + + + + + BSD + + catkin + moveit_task_constructor_core + moveit_visual_tools + moveit_ros_planning_interface + moveit_core + + + + + + + diff --git a/demo/src/moveit_task_constructor_demo.cpp b/demo/src/moveit_task_constructor_demo.cpp new file mode 100644 index 000000000..31e447cce --- /dev/null +++ b/demo/src/moveit_task_constructor_demo.cpp @@ -0,0 +1,124 @@ +/********************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019 PickNik LLC. + * 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: Henning Kayser, Simon Goldstein + Desc: A demo to show MoveIt Task Constructor in action +*/ + +// ROS +#include + +// MTC pick/place demo implementation +#include + +#include +#include + +void spawnTable() +{ + ros::Duration(1.0).sleep(); + ros::NodeHandle pnh("~"); + std::string table_name = pnh.param("table_name", "table"); + std::string surface_frame = pnh.param("table_surface_frame", "world"); + double height = pnh.param("table_height", 0.3); + double width = pnh.param("table_width", 0.5); + double length = pnh.param("table_length", 0.5); + double position_x = pnh.param("table_pos_x", 0.5); + double position_y = pnh.param("table_pos_y", 0.0); + + moveit::planning_interface::PlanningSceneInterface psi; + moveit_msgs::CollisionObject object; + object.id = table_name = table_name; + object.header.frame_id = surface_frame; + object.primitives.resize(1); + object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; + object.primitives[0].dimensions = { length, width, height }; + object.primitive_poses.resize(1); + object.primitive_poses[0].position.x = position_x; + object.primitive_poses[0].position.y = position_y; + object.primitive_poses[0].position.z = 0.5 * height; + object.primitive_poses[0].orientation.w = 1.0; + psi.applyCollisionObject(object); + ros::Duration(1.0).sleep(); +} + +void spawnObject() +{ + ros::Duration(1.0).sleep(); + ros::NodeHandle pnh("~"); + std::string object_name = pnh.param("object_name", "object"); + std::string surface_frame = pnh.param("object_surface_frame", "world"); + double height = pnh.param("object_height", 0.2); + double radius = pnh.param("object_radius", 0.03); + double position_x = pnh.param("object_pos_x", 0.0); + double position_y = pnh.param("object_pos_y", 0.0); + double table_height = pnh.param("table_height", 0.3); + double place_surface_offset = pnh.param("place_surface_offset", 0.0001); + + moveit::planning_interface::PlanningSceneInterface psi; + moveit_msgs::CollisionObject object; + object.id = object_name = object_name; + object.header.frame_id = surface_frame; + object.primitives.resize(1); + object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; + object.primitives[0].dimensions = { height, radius }; + object.primitive_poses.resize(1); + object.primitive_poses[0].position.x = position_x; + object.primitive_poses[0].position.y = position_y; + object.primitive_poses[0].position.z = 0.5 * (height + table_height) + place_surface_offset; + object.primitive_poses[0].orientation.w = 1.0; + psi.applyCollisionObject(object); + ros::Duration(1.0).sleep(); +} + +int main(int argc, char** argv) +{ + ROS_INFO("Init moveit_task_constructor_demo"); + ros::init(argc, argv, "moveit_task_constructor_demo"); + ros::NodeHandle nh; + ros::AsyncSpinner spinner(1); + spinner.start(); + + // Add table and object to planning scene + spawnTable(); + spawnObject(); + + // Construct and run pick/place task + moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh); + pick_place_task.init(); + if (pick_place_task.plan()) + pick_place_task.execute(); + + // Keep introspection alive + ros::waitForShutdown(); + return 0; +} diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp new file mode 100644 index 000000000..eba18204e --- /dev/null +++ b/demo/src/pick_place_task.cpp @@ -0,0 +1,469 @@ +/********************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019 PickNik LLC. + * 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: Henning Kayser, Simon Goldstein + Desc: A demo to show MoveIt Task Constructor in action +*/ + +#include + +namespace moveit_task_constructor_demo +{ +PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh) + : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) +{ + ROS_INFO("waiting for task execution"); + // execute_.waitForServer(); + + /**************************************************** + * * + * Load Parameters * + * * + ***************************************************/ + ros::NodeHandle pnh("~"); + + // Planning group properties + group_name_ = pnh.param("group_name", "manipulator"); + hand_name_ = pnh.param("hand_name", "hand"); + eef_name_ = pnh.param("eef_name", "hand"); + hand_frame_ = pnh.param("hand_frame", "panda_hand"); + + // Object + surface + table_surface_frame_ = pnh.param("table_surface_frame", "table_top"); + object_surface_frame_ = pnh.param("object_surface_frame", "table"); + surface_link_ = pnh.param("surface_link", "table"); + support_surfaces_ = { surface_link_ }; + // table_name_ = pnh.param("table_name", "FAKE"); + table_height_ = pnh.param("table_height", 0.3); + table_length_ = pnh.param("table_length", 0.5); + table_width_ = pnh.param("table_width", 0.5); + + object_name_ = pnh.param("object_name", "object"); + object_height_ = pnh.param("object_height", 0.2); + object_radius_ = pnh.param("object_radius", 0.03); + // Pick + approach_object_min_dist_ = pnh.param("approach_object_min_dist", 0.1); + approach_object_max_dist_ = pnh.param("approach_object_max_dist", 0.15); + + // Lift + lift_object_min_dist_ = pnh.param("lift_object_min_dist", 0.01); + lift_object_max_dist_ = pnh.param("lift_object_max_dist", 0.1); + + // Place + place_surface_offset_ = pnh.param("place_surface_offset", 0.01); + place_pos_x_ = pnh.param("place_pos_x", 0.1); + place_pos_y_ = pnh.param("place_pos_y", 0.1); + + // compute hand grasp frame + double rotation = pnh.param("grasp_rotation_z", 1.0); + double grasp_offset_x = pnh.param("grasp_offset_x", 0.1); + double grasp_offset_z = pnh.param("prasp_offset_z", 0.0); + grasp_frame_transform_ = Eigen::AngleAxisd(M_PI * rotation, Eigen::Vector3d::UnitZ()) * + Eigen::Translation3d(grasp_offset_x, 0, grasp_offset_z); +} + +void PickPlaceTask::init() +{ + const std::string object = "object"; + + // 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& t = *task_; + t.loadRobotModel(); + + // Sampling planner + auto sampling_planner = std::make_shared(); + sampling_planner->setProperty("goal_joint_tolerance", 1e-3); + + // Cartesian planner + auto cartesian_planner = std::make_shared(); + cartesian_planner->setMaxVelocityScaling(1.0); + cartesian_planner->setMaxAccelerationScaling(1.0); + cartesian_planner->setStepSize(.01); + + // Set task properties + t.setProperty("group", group_name_); + t.setProperty("eef", eef_name_); + t.setProperty("hand", hand_name_); + t.setProperty("hand_grasping_frame", hand_frame_); + t.setProperty("ik_frame", hand_frame_); + + /**************************************************** + * * + * Current State * + * * + ***************************************************/ + Stage* current_state = nullptr; // Forward current_state on to grasp pose generator + { + auto _current_state = std::make_unique("current state"); + + // Verify that object is not attachd + auto applicability_filter = + std::make_unique("applicability test", std::move(_current_state)); + applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) { + if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) + { + comment = "object with id '" + object + "' is already attached and cannot be picked"; + return false; + } + return true; + }); + + current_state = applicability_filter.get(); + t.add(std::move(applicability_filter)); + } + + /**************************************************** + * * + * Open Hand * + * * + ***************************************************/ + { // Open Hand + auto stage = std::make_unique("open hand", sampling_planner); + stage->setGroup("hand"); + stage->setGoal("open"); + t.add(std::move(stage)); + } + + /**************************************************** + * * + * Move to Pick * + * * + ***************************************************/ + { // Move-to pre-grasp + auto stage = std::make_unique( + "move to pick", stages::Connect::GroupPlannerVector{ { group_name_, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } + + /**************************************************** + * * + * Pick Object * + * * + ***************************************************/ + Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator + { + auto grasp = std::make_unique("pick object"); + t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); + grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); + + /**************************************************** +---- * Approach Object * + ***************************************************/ + { + auto stage = std::make_unique("approach object", cartesian_planner); + stage->properties().set("marker_ns", "approach_object"); + stage->properties().set("link", hand_frame_); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); + + // Set hand forward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = hand_frame_; + vec.vector.x = 1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } + + /**************************************************** +---- * Generate Grasp Pose * + ***************************************************/ + { + // Sample grasp pose + auto stage = std::make_unique("generate grasp pose"); + stage->properties().configureInitFrom(Stage::PARENT); + stage->properties().set("marker_ns", "grasp_pose"); + stage->setPreGraspPose("open"); + stage->setObject(object); + stage->setAngleDelta(M_PI / 12); + stage->setMonitoredStage(current_state); // Hook into current state + + // Compute IK + auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(8); + wrapper->setMinSolutionDistance(1.0); + // wrapper->setIgnoreCollisions(true); + wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef" }); + wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + grasp->insert(std::move(wrapper)); + } + + /**************************************************** +---- * Allow Collision (hand object) * + ***************************************************/ + { + auto stage = std::make_unique("allow collision (hand,object)"); + stage->allowCollisions( + object, t.getRobotModel()->getJointModelGroup("hand")->getLinkModelNamesWithCollisionGeometry(), true); + grasp->insert(std::move(stage)); + } + + /**************************************************** +---- * Close Hand * + ***************************************************/ + { + auto stage = std::make_unique("close hand", sampling_planner); + stage->properties().property("group").configureInitFrom(Stage::PARENT, "hand"); + stage->setGoal("close"); + grasp->insert(std::move(stage)); + } + + /**************************************************** +.... * Attach Object * + ***************************************************/ + { + auto stage = std::make_unique("attach object"); + stage->attachObject(object, hand_frame_); + attach_object_stage = stage.get(); + grasp->insert(std::move(stage)); + } + + /**************************************************** +.... * Allow collision (object support) * + ***************************************************/ + { + auto stage = std::make_unique("allow collision (object,support)"); + stage->allowCollisions({ object }, support_surfaces_, true); + grasp->insert(std::move(stage)); + } + + /**************************************************** +.... * Lift object * + ***************************************************/ + { + auto stage = std::make_unique("lift object", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(lift_object_min_dist_, lift_object_max_dist_); + stage->setIKFrame(hand_frame_); + stage->properties().set("marker_ns", "lift_object"); + + // Set upward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = "world"; + vec.vector.z = 1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } + + /**************************************************** +.... * Forbid collision (object support) * + ***************************************************/ + { + auto stage = std::make_unique("forbid collision (object,surface)"); + stage->allowCollisions({ object }, support_surfaces_, false); + grasp->insert(std::move(stage)); + } + + // Add grasp container to task + t.add(std::move(grasp)); + } + + /****************************************************** + * * + * Move to Place * + * * + *****************************************************/ + { + auto stage = std::make_unique( + "move to place", stages::Connect::GroupPlannerVector{ { group_name_, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } + + /****************************************************** + * * + * Place Object * + * * + *****************************************************/ + { + auto place = std::make_unique("place object"); + t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); + place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); + + /****************************************************** +---- * Lower Object * + *****************************************************/ + { + auto stage = std::make_unique("lower object", cartesian_planner); + stage->properties().set("marker_ns", "lower_object"); + stage->properties().set("link", hand_frame_); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(.03, .13); + + // Set downward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = "world"; + vec.vector.z = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } + + /****************************************************** +---- * Generate Place Pose * + *****************************************************/ + { + // Generate Place Pose + auto stage = std::make_unique("generate place pose"); + stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); + stage->properties().set("marker_ns", "place_pose"); + stage->setObject(object); + // stage->setAugmentRotations(false); + + // Set target pose + geometry_msgs::PoseStamped p; + p.header.frame_id = object_surface_frame_; + p.pose.orientation.w = 1; + p.pose.position.x = place_pos_x_; + p.pose.position.y = place_pos_y_; + p.pose.position.z = 0.5 * object_height_ + place_surface_offset_; + stage->setPose(p); + stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage + + // Compute IK + auto wrapper = std::make_unique("place pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(2); + wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef" }); + wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + place->insert(std::move(wrapper)); + } + + /****************************************************** +---- * Open Hand * + *****************************************************/ + { + auto stage = std::make_unique("open hand", sampling_planner); + stage->properties().property("group").configureInitFrom(Stage::PARENT, "hand"); + stage->setGoal("open"); + place->insert(std::move(stage)); + } + + /****************************************************** +---- * Forbid collision (hand, object) * + *****************************************************/ + { + auto stage = std::make_unique("forbid collision (hand,object)"); + stage->allowCollisions( + object_name_, t.getRobotModel()->getJointModelGroup("hand")->getLinkModelNamesWithCollisionGeometry(), false); + place->insert(std::move(stage)); + } + + /****************************************************** +---- * Detach Object * + *****************************************************/ + { + auto stage = std::make_unique("detach object"); + stage->detachObject(object_name_, hand_frame_); + place->insert(std::move(stage)); + } + + /****************************************************** +---- * Retreat Motion * + *****************************************************/ + { + auto stage = std::make_unique("retreat after place", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(.12, .25); + stage->setIKFrame(hand_frame_); + stage->properties().set("marker_ns", "retreat"); + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = hand_frame_; + vec.vector.x = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } + + // Add place container to task + t.add(std::move(place)); + } + + /****************************************************** + * * + * Move to Home * + * * + *****************************************************/ + { + auto stage = std::make_unique("move home", sampling_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setGoal("ready"); + stage->restrictDirection(stages::MoveTo::FORWARD); + t.add(std::move(stage)); + } +} + +bool PickPlaceTask::plan() +{ + try + { + task_->plan(10); + } + catch (InitStageException& e) + { + ROS_ERROR_STREAM("Initialization failed: " << e); + return false; + } + if (task_->numSolutions() == 0) + { + ROS_ERROR("Planning failed"); + return false; + } + return true; +} + +bool PickPlaceTask::execute() +{ + moveit_task_constructor_msgs::Solution solution; + task_->solutions().front()->fillMessage(solution); + + ROS_INFO_STREAM("last trajectory in solution:\n" << solution.sub_trajectory.back().trajectory); + + moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; + execute_goal.solution = solution; + execute_.sendGoal(execute_goal); + execute_.waitForResult(); + moveit_msgs::MoveItErrorCodes execute_result = execute_.getResult()->error_code; + + if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) + { + ROS_ERROR_STREAM("task execution failed and returned: " << execute_.getState().toString()); + return false; + } + + return true; +} +} From ada11ad375321329aa432be93e6b4cd24bcad81e Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Mon, 29 Jul 2019 17:32:14 -0600 Subject: [PATCH 02/18] Pass Travis --- demo/src/pick_place_task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index eba18204e..d58311e65 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -88,7 +88,7 @@ PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle double grasp_offset_x = pnh.param("grasp_offset_x", 0.1); double grasp_offset_z = pnh.param("prasp_offset_z", 0.0); grasp_frame_transform_ = Eigen::AngleAxisd(M_PI * rotation, Eigen::Vector3d::UnitZ()) * - Eigen::Translation3d(grasp_offset_x, 0, grasp_offset_z); + Eigen::Translation3d(grasp_offset_x, 0, grasp_offset_z); } void PickPlaceTask::init() From f7bb6e3a46777a103bf20fa9ac1cee2e7a9ca747 Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Wed, 31 Jul 2019 17:34:05 -0600 Subject: [PATCH 03/18] debugging --- demo/src/moveit_task_constructor_demo.cpp | 4 ++++ demo/src/pick_place_task.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/demo/src/moveit_task_constructor_demo.cpp b/demo/src/moveit_task_constructor_demo.cpp index 31e447cce..9ebec2341 100644 --- a/demo/src/moveit_task_constructor_demo.cpp +++ b/demo/src/moveit_task_constructor_demo.cpp @@ -116,7 +116,11 @@ int main(int argc, char** argv) moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh); pick_place_task.init(); if (pick_place_task.plan()) + { + ROS_INFO("Planning succeded"); pick_place_task.execute(); + } + ROS_INFO("Planning failed"); // Keep introspection alive ros::waitForShutdown(); diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index d58311e65..88912fc19 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -104,7 +104,7 @@ void PickPlaceTask::init() // Sampling planner auto sampling_planner = std::make_shared(); - sampling_planner->setProperty("goal_joint_tolerance", 1e-3); + sampling_planner->setProperty("goal_joint_tolerance", 1e-5); // Cartesian planner auto cartesian_planner = std::make_shared(); From d924bb549d3f70dce2c401c8e8c7ab7dedbddf34 Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Thu, 1 Aug 2019 16:11:12 -0600 Subject: [PATCH 04/18] Functioning tutorial --- demo/config/panda_config.yaml | 12 +++++----- demo/src/moveit_task_constructor_demo.cpp | 16 +++++++------- demo/src/pick_place_task.cpp | 27 +++++++++++++---------- 3 files changed, 30 insertions(+), 25 deletions(-) diff --git a/demo/config/panda_config.yaml b/demo/config/panda_config.yaml index 642f12074..b0cc3ca35 100644 --- a/demo/config/panda_config.yaml +++ b/demo/config/panda_config.yaml @@ -24,17 +24,19 @@ table_pos_x: 0.5 table_pos_y: -0.25 # gripper grasp frame transform -grasp_offset_x: 0.15 # horizontal -grasp_offset_z: 0.0 # vertical -grasp_rotation_z: 0.5 # rotation around Z access +grasp_offset_x: 0.1 # X +grasp_offset_y: 0.0 # Y +grasp_offset_z: 0.0 # Z +grasp_rotationY: 1.5 # rotation +grasp_rotationX: 0.75 # rotation # place pose metrics (x/y position on table_top) place_pos_x: 0.1 place_pos_y: 0.1 -place_surface_offset: 0.10001 # place offset from table +place_surface_offset: 0.0001 # place offset from table # valid distance range when approaching an object for picking -approach_object_min_dist: 0.10 +approach_object_min_dist: 0.1 approach_object_max_dist: 0.15 # valid height range when lifting an object after pick diff --git a/demo/src/moveit_task_constructor_demo.cpp b/demo/src/moveit_task_constructor_demo.cpp index 9ebec2341..a92aab47c 100644 --- a/demo/src/moveit_task_constructor_demo.cpp +++ b/demo/src/moveit_task_constructor_demo.cpp @@ -49,11 +49,11 @@ void spawnTable() ros::NodeHandle pnh("~"); std::string table_name = pnh.param("table_name", "table"); std::string surface_frame = pnh.param("table_surface_frame", "world"); - double height = pnh.param("table_height", 0.3); - double width = pnh.param("table_width", 0.5); - double length = pnh.param("table_length", 0.5); + double height = pnh.param("table_height", 0.1); + double width = pnh.param("table_width", 0.4); + double length = pnh.param("table_length", 0.4); double position_x = pnh.param("table_pos_x", 0.5); - double position_y = pnh.param("table_pos_y", 0.0); + double position_y = pnh.param("table_pos_y", -0.25); moveit::planning_interface::PlanningSceneInterface psi; moveit_msgs::CollisionObject object; @@ -77,11 +77,11 @@ void spawnObject() ros::NodeHandle pnh("~"); std::string object_name = pnh.param("object_name", "object"); std::string surface_frame = pnh.param("object_surface_frame", "world"); - double height = pnh.param("object_height", 0.2); - double radius = pnh.param("object_radius", 0.03); + double height = pnh.param("object_height", 0.25); + double radius = pnh.param("object_radius", 0.02); double position_x = pnh.param("object_pos_x", 0.0); - double position_y = pnh.param("object_pos_y", 0.0); - double table_height = pnh.param("table_height", 0.3); + double position_y = pnh.param("object_pos_y", -0.1); + double table_height = pnh.param("table_height", 0.1); double place_surface_offset = pnh.param("place_surface_offset", 0.0001); moveit::planning_interface::PlanningSceneInterface psi; diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 88912fc19..e88b2cbff 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -63,13 +63,13 @@ PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle surface_link_ = pnh.param("surface_link", "table"); support_surfaces_ = { surface_link_ }; // table_name_ = pnh.param("table_name", "FAKE"); - table_height_ = pnh.param("table_height", 0.3); - table_length_ = pnh.param("table_length", 0.5); - table_width_ = pnh.param("table_width", 0.5); + table_height_ = pnh.param("table_height", 0.1); + table_length_ = pnh.param("table_length", 0.4); + table_width_ = pnh.param("table_width", 0.4); object_name_ = pnh.param("object_name", "object"); - object_height_ = pnh.param("object_height", 0.2); - object_radius_ = pnh.param("object_radius", 0.03); + object_height_ = pnh.param("object_height", 0.25); + object_radius_ = pnh.param("object_radius", 0.02); // Pick approach_object_min_dist_ = pnh.param("approach_object_min_dist", 0.1); approach_object_max_dist_ = pnh.param("approach_object_max_dist", 0.15); @@ -79,16 +79,19 @@ PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle lift_object_max_dist_ = pnh.param("lift_object_max_dist", 0.1); // Place - place_surface_offset_ = pnh.param("place_surface_offset", 0.01); + place_surface_offset_ = pnh.param("place_surface_offset", 0.0001); place_pos_x_ = pnh.param("place_pos_x", 0.1); place_pos_y_ = pnh.param("place_pos_y", 0.1); // compute hand grasp frame - double rotation = pnh.param("grasp_rotation_z", 1.0); + double rotationy = pnh.param("grasp_rotationy", 1.5); + double rotationx = pnh.param("grasp_rotationx", 0.75); double grasp_offset_x = pnh.param("grasp_offset_x", 0.1); + double grasp_offset_y = pnh.param("grasp_offset_y", 0.0); double grasp_offset_z = pnh.param("prasp_offset_z", 0.0); - grasp_frame_transform_ = Eigen::AngleAxisd(M_PI * rotation, Eigen::Vector3d::UnitZ()) * - Eigen::Translation3d(grasp_offset_x, 0, grasp_offset_z); + grasp_frame_transform_ = Eigen::AngleAxisd(M_PI * rotationy, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(M_PI * rotationx, Eigen::Vector3d::UnitX()) * + Eigen::Translation3d(grasp_offset_x, grasp_offset_y, grasp_offset_z); } void PickPlaceTask::init() @@ -193,7 +196,7 @@ void PickPlaceTask::init() // Set hand forward direction geometry_msgs::Vector3Stamped vec; vec.header.frame_id = hand_frame_; - vec.vector.x = 1.0; + vec.vector.z = 1.0; stage->setDirection(vec); grasp->insert(std::move(stage)); } @@ -350,7 +353,7 @@ void PickPlaceTask::init() p.pose.orientation.w = 1; p.pose.position.x = place_pos_x_; p.pose.position.y = place_pos_y_; - p.pose.position.z = 0.5 * object_height_ + place_surface_offset_; + p.pose.position.z = 0.5 * object_height_ + place_surface_offset_ + 0.5 * table_height_; stage->setPose(p); stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage @@ -403,7 +406,7 @@ void PickPlaceTask::init() stage->properties().set("marker_ns", "retreat"); geometry_msgs::Vector3Stamped vec; vec.header.frame_id = hand_frame_; - vec.vector.x = -1.0; + vec.vector.z = -1.0; stage->setDirection(vec); place->insert(std::move(stage)); } From c85d3a5229786378d6f7186c3361b03ad91f109d Mon Sep 17 00:00:00 2001 From: simonGoldstein <42970868+simonGoldstein@users.noreply.github.com> Date: Mon, 5 Aug 2019 10:43:05 -0600 Subject: [PATCH 05/18] Update demo/README.md Co-Authored-By: Henning Kayser --- demo/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/demo/README.md b/demo/README.md index 0205f2a50..f0dd6421d 100644 --- a/demo/README.md +++ b/demo/README.md @@ -2,7 +2,7 @@ Description: A simple pick & place demo using MoveIt Task Constructor. This uses the Panda from Franka Emika -Developed by Henning Kayser & Simon Goldstien at [PickNik Consulting](http://picknik.ai/) +Developed by Henning Kayser & Simon Goldstein at [PickNik Consulting](http://picknik.ai/) ## Run From ef883c6ed24d38cf49a32a4ccdd8740843e5f17d Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Mon, 5 Aug 2019 16:46:54 -0600 Subject: [PATCH 06/18] Addressed feedback --- demo/.clang-format | 66 ------------ demo/CMakeLists.txt | 66 ------------ demo/config/panda_config.yaml | 23 ++-- .../pick_place_task.h | 14 ++- demo/src/moveit_task_constructor_demo.cpp | 51 +++++---- demo/src/pick_place_task.cpp | 100 ++++++++++-------- 6 files changed, 110 insertions(+), 210 deletions(-) delete mode 100644 demo/.clang-format diff --git a/demo/.clang-format b/demo/.clang-format deleted file mode 100644 index eca6051ae..000000000 --- a/demo/.clang-format +++ /dev/null @@ -1,66 +0,0 @@ ---- -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AllowShortLoopsOnASingleLine: false -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} -... diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 16a57a55f..12b7e0e97 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -1,10 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(moveit_task_constructor_demo) -## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++14) -## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS roscpp moveit_core @@ -13,108 +11,44 @@ find_package(catkin REQUIRED COMPONENTS moveit_ros_planning_interface ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES moveit_task_constructor_demo -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( include ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library add_library(${PROJECT_NAME} src/pick_place_task.cpp ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide add_executable(${PROJECT_NAME}_moveit_task_constructor_demo src/moveit_task_constructor_demo.cpp) -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" set_target_properties(${PROJECT_NAME}_moveit_task_constructor_demo PROPERTIES OUTPUT_NAME moveit_task_constructor_demo PREFIX "") -## Add cmake target dependencies of the executable -## same as for the library above add_dependencies(${PROJECT_NAME}_moveit_task_constructor_demo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME}_moveit_task_constructor_demo ${PROJECT_NAME} ${catkin_LIBRARIES} ) -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_moveit_task_constructor_demo ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -## Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE ) -## Mark other files for installation (e.g. launch and bag files, etc.) install(FILES launch/demo.launch - # myfile2 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_moveit_task_constructor_demo.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/demo/config/panda_config.yaml b/demo/config/panda_config.yaml index b0cc3ca35..6fe8b0172 100644 --- a/demo/config/panda_config.yaml +++ b/demo/config/panda_config.yaml @@ -1,13 +1,20 @@ # planning group names -group_name: "panda_arm" +arm_group_name: "panda_arm" eef_name: "hand" hand_name: "hand" hand_frame: "panda_link8" +number_of_plans: 10 + +# Poses +open_gripper_pose: "open" +close_gripper_pose: "close" +home_pose: "ready" # surface -table_surface_frame: "world" -object_surface_frame: "table" +table_refrence_frame: "world" +object_refrence_frame: "table/table_top" surface_link: "table" +world_frame: "world" # collision object # CYLINDER object specifications @@ -15,10 +22,12 @@ object_name: "object" object_height: 0.25 object_radius: 0.02 object_pos_x: 0.0 -object_pos_y: -0.1 +object_pos_y: 0.0 +table_name: "table" +table_subframe_name: "table_top" table_height: 0.1 -table_width: 0.4 +table_width: 0.5 table_length: 0.4 table_pos_x: 0.5 table_pos_y: -0.25 @@ -27,8 +36,8 @@ table_pos_y: -0.25 grasp_offset_x: 0.1 # X grasp_offset_y: 0.0 # Y grasp_offset_z: 0.0 # Z -grasp_rotationY: 1.5 # rotation -grasp_rotationX: 0.75 # rotation +grasp_rotation_x: 0.75 # rotation X axis +grasp_rotation_y: 1.5 # rotation Y axis # place pose metrics (x/y position on table_top) place_pos_x: 0.1 diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h index 99d5adcb0..97a1fb7db 100644 --- a/demo/include/moveit_task_constructor_demo/pick_place_task.h +++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h @@ -89,24 +89,30 @@ class PickPlaceTask moveit::task_constructor::TaskPtr task_; // planning group properties - std::string group_name_; + std::string arm_group_name_; std::string eef_name_; std::string hand_name_; std::string hand_frame_; // object + surface std::vector support_surfaces_; - std::string table_surface_frame_; - std::string object_surface_frame_; + std::string table_refrence_frame_; + std::string object_refrence_frame_; std::string surface_link_; std::string object_name_; - // std::string table_name_; + std::string table_name_; + std::string world_frame_; double object_height_; double object_radius_; double table_height_; double table_length_; double table_width_; + // pose_names + std::string open_gripper_pose_; + std::string close_gripper_pose_; + std::string home_pose_; + // execution actionlib::SimpleActionClient execute_; diff --git a/demo/src/moveit_task_constructor_demo.cpp b/demo/src/moveit_task_constructor_demo.cpp index a92aab47c..5c47fd5cb 100644 --- a/demo/src/moveit_task_constructor_demo.cpp +++ b/demo/src/moveit_task_constructor_demo.cpp @@ -45,20 +45,20 @@ void spawnTable() { - ros::Duration(1.0).sleep(); ros::NodeHandle pnh("~"); - std::string table_name = pnh.param("table_name", "table"); - std::string surface_frame = pnh.param("table_surface_frame", "world"); - double height = pnh.param("table_height", 0.1); - double width = pnh.param("table_width", 0.4); - double length = pnh.param("table_length", 0.4); - double position_x = pnh.param("table_pos_x", 0.5); - double position_y = pnh.param("table_pos_y", -0.25); + std::string table_name = pnh.param("table_name", "table_name"); + std::string table_subframe_name = pnh.param("table_subframe_name", "table_subframe_name"); + std::string table_refrence_frame = pnh.param("table_refrence_frame", "frame_table_is_in"); + double height = pnh.param("table_height", 0.0); + double width = pnh.param("table_width", 0.0); + double length = pnh.param("table_length", 0.0); + double position_x = pnh.param("table_pos_x", 0.0); + double position_y = pnh.param("table_pos_y", 0.0); moveit::planning_interface::PlanningSceneInterface psi; moveit_msgs::CollisionObject object; object.id = table_name = table_name; - object.header.frame_id = surface_frame; + object.header.frame_id = table_refrence_frame; object.primitives.resize(1); object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; object.primitives[0].dimensions = { length, width, height }; @@ -67,37 +67,43 @@ void spawnTable() object.primitive_poses[0].position.y = position_y; object.primitive_poses[0].position.z = 0.5 * height; object.primitive_poses[0].orientation.w = 1.0; + + // Make the table top subframe at the center of the top surface + object.subframe_names.resize(1); + object.subframe_poses.resize(1); + object.subframe_names[0] = table_subframe_name; + object.subframe_poses[0].position.x = position_x; + object.subframe_poses[0].position.y = position_y; + object.subframe_poses[0].position.z = height; + psi.applyCollisionObject(object); - ros::Duration(1.0).sleep(); } void spawnObject() { - ros::Duration(1.0).sleep(); ros::NodeHandle pnh("~"); - std::string object_name = pnh.param("object_name", "object"); - std::string surface_frame = pnh.param("object_surface_frame", "world"); - double height = pnh.param("object_height", 0.25); - double radius = pnh.param("object_radius", 0.02); + std::string object_name = pnh.param("object_name", "name_of_moved_object"); + std::string object_refrence_frame = pnh.param("object_refrence_frame", "frame_object_lays_upon"); + double height = pnh.param("object_height", 0.0); + double radius = pnh.param("object_radius", 0.0); double position_x = pnh.param("object_pos_x", 0.0); - double position_y = pnh.param("object_pos_y", -0.1); - double table_height = pnh.param("table_height", 0.1); - double place_surface_offset = pnh.param("place_surface_offset", 0.0001); + double position_y = pnh.param("object_pos_y", 0.0); + double table_height = pnh.param("table_height", 0.0); + double place_surface_offset = pnh.param("place_surface_offset", 0.0); moveit::planning_interface::PlanningSceneInterface psi; moveit_msgs::CollisionObject object; object.id = object_name = object_name; - object.header.frame_id = surface_frame; + object.header.frame_id = object_refrence_frame; object.primitives.resize(1); object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; object.primitives[0].dimensions = { height, radius }; object.primitive_poses.resize(1); object.primitive_poses[0].position.x = position_x; object.primitive_poses[0].position.y = position_y; - object.primitive_poses[0].position.z = 0.5 * (height + table_height) + place_surface_offset; + object.primitive_poses[0].position.z = 0.5 * height + place_surface_offset; object.primitive_poses[0].orientation.w = 1.0; psi.applyCollisionObject(object); - ros::Duration(1.0).sleep(); } int main(int argc, char** argv) @@ -109,8 +115,11 @@ int main(int argc, char** argv) spinner.start(); // Add table and object to planning scene + ros::Duration(0.3).sleep(); spawnTable(); + ros::Duration(0.3).sleep(); spawnObject(); + ros::Duration(0.3).sleep(); // Construct and run pick/place task moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh); diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index e88b2cbff..2c903adcc 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -42,7 +42,6 @@ PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) { ROS_INFO("waiting for task execution"); - // execute_.waitForServer(); /**************************************************** * * @@ -52,41 +51,49 @@ PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle ros::NodeHandle pnh("~"); // Planning group properties - group_name_ = pnh.param("group_name", "manipulator"); - hand_name_ = pnh.param("hand_name", "hand"); - eef_name_ = pnh.param("eef_name", "hand"); - hand_frame_ = pnh.param("hand_frame", "panda_hand"); + arm_group_name_ = pnh.param("arm_group_name", "arm_group_name"); + hand_name_ = pnh.param("hand_name", "hand_group_name"); + eef_name_ = pnh.param("eef_name", "eef_name"); + hand_frame_ = pnh.param("hand_frame", "hand_frame_name"); + world_frame_ = pnh.param("world_frame", "world_frame_name"); + + // poses + open_gripper_pose_ = pnh.param("open_gripper_pose", "open_pose"); + close_gripper_pose_ = pnh.param("close_gripper_pose", "close_pose"); + home_pose_ = pnh.param("home_pose", "home_pose"); // Object + surface - table_surface_frame_ = pnh.param("table_surface_frame", "table_top"); - object_surface_frame_ = pnh.param("object_surface_frame", "table"); - surface_link_ = pnh.param("surface_link", "table"); + table_name_ = pnh.param("table_name", "table_name"); + table_height_ = pnh.param("table_height", 0.0); + table_length_ = pnh.param("table_length", 0.0); + table_width_ = pnh.param("table_width", 0.0); + + object_name_ = pnh.param("object_name", "name_of_moved_object"); + object_height_ = pnh.param("object_height", 0.0); + object_radius_ = pnh.param("object_radius", 0.0); + + table_refrence_frame_ = pnh.param("table_refrence_frame", "frame_table_is_in"); + object_refrence_frame_ = pnh.param("object_refrence_frame", "frame_object_lays_upon"); + surface_link_ = pnh.param("surface_link", "name_of_table"); support_surfaces_ = { surface_link_ }; - // table_name_ = pnh.param("table_name", "FAKE"); - table_height_ = pnh.param("table_height", 0.1); - table_length_ = pnh.param("table_length", 0.4); - table_width_ = pnh.param("table_width", 0.4); - - object_name_ = pnh.param("object_name", "object"); - object_height_ = pnh.param("object_height", 0.25); - object_radius_ = pnh.param("object_radius", 0.02); + // Pick - approach_object_min_dist_ = pnh.param("approach_object_min_dist", 0.1); - approach_object_max_dist_ = pnh.param("approach_object_max_dist", 0.15); + approach_object_min_dist_ = pnh.param("approach_object_min_dist", 0.0); + approach_object_max_dist_ = pnh.param("approach_object_max_dist", 0.0); // Lift - lift_object_min_dist_ = pnh.param("lift_object_min_dist", 0.01); - lift_object_max_dist_ = pnh.param("lift_object_max_dist", 0.1); + lift_object_min_dist_ = pnh.param("lift_object_min_dist", 0.0); + lift_object_max_dist_ = pnh.param("lift_object_max_dist", 0.0); // Place - place_surface_offset_ = pnh.param("place_surface_offset", 0.0001); - place_pos_x_ = pnh.param("place_pos_x", 0.1); - place_pos_y_ = pnh.param("place_pos_y", 0.1); + place_surface_offset_ = pnh.param("place_surface_offset", 0.0); + place_pos_x_ = pnh.param("place_pos_x", 0.0); + place_pos_y_ = pnh.param("place_pos_y", 0.0); // compute hand grasp frame - double rotationy = pnh.param("grasp_rotationy", 1.5); - double rotationx = pnh.param("grasp_rotationx", 0.75); - double grasp_offset_x = pnh.param("grasp_offset_x", 0.1); + double rotationy = pnh.param("grasp_rotation_y", 0.0); + double rotationx = pnh.param("grasp_rotation_x", 0.0); + double grasp_offset_x = pnh.param("grasp_offset_x", 0.0); double grasp_offset_y = pnh.param("grasp_offset_y", 0.0); double grasp_offset_z = pnh.param("prasp_offset_z", 0.0); grasp_frame_transform_ = Eigen::AngleAxisd(M_PI * rotationy, Eigen::Vector3d::UnitY()) * @@ -116,7 +123,7 @@ void PickPlaceTask::init() cartesian_planner->setStepSize(.01); // Set task properties - t.setProperty("group", group_name_); + t.setProperty("group", arm_group_name_); t.setProperty("eef", eef_name_); t.setProperty("hand", hand_name_); t.setProperty("hand_grasping_frame", hand_frame_); @@ -154,8 +161,8 @@ void PickPlaceTask::init() ***************************************************/ { // Open Hand auto stage = std::make_unique("open hand", sampling_planner); - stage->setGroup("hand"); - stage->setGoal("open"); + stage->setGroup(hand_name_); + stage->setGoal(open_gripper_pose_); t.add(std::move(stage)); } @@ -166,7 +173,7 @@ void PickPlaceTask::init() ***************************************************/ { // Move-to pre-grasp auto stage = std::make_unique( - "move to pick", stages::Connect::GroupPlannerVector{ { group_name_, sampling_planner } }); + "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); stage->setTimeout(5.0); stage->properties().configureInitFrom(Stage::PARENT); t.add(std::move(stage)); @@ -209,7 +216,7 @@ void PickPlaceTask::init() auto stage = std::make_unique("generate grasp pose"); stage->properties().configureInitFrom(Stage::PARENT); stage->properties().set("marker_ns", "grasp_pose"); - stage->setPreGraspPose("open"); + stage->setPreGraspPose(open_gripper_pose_); stage->setObject(object); stage->setAngleDelta(M_PI / 12); stage->setMonitoredStage(current_state); // Hook into current state @@ -218,7 +225,6 @@ void PickPlaceTask::init() auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); wrapper->setMaxIKSolutions(8); wrapper->setMinSolutionDistance(1.0); - // wrapper->setIgnoreCollisions(true); wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); wrapper->properties().configureInitFrom(Stage::PARENT, { "eef" }); wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); @@ -231,7 +237,7 @@ void PickPlaceTask::init() { auto stage = std::make_unique("allow collision (hand,object)"); stage->allowCollisions( - object, t.getRobotModel()->getJointModelGroup("hand")->getLinkModelNamesWithCollisionGeometry(), true); + object, t.getRobotModel()->getJointModelGroup(hand_name_)->getLinkModelNamesWithCollisionGeometry(), true); grasp->insert(std::move(stage)); } @@ -240,8 +246,8 @@ void PickPlaceTask::init() ***************************************************/ { auto stage = std::make_unique("close hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, "hand"); - stage->setGoal("close"); + stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_name_); + stage->setGoal(close_gripper_pose_); grasp->insert(std::move(stage)); } @@ -276,7 +282,7 @@ void PickPlaceTask::init() // Set upward direction geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = "world"; + vec.header.frame_id = world_frame_; vec.vector.z = 1.0; stage->setDirection(vec); grasp->insert(std::move(stage)); @@ -302,7 +308,7 @@ void PickPlaceTask::init() *****************************************************/ { auto stage = std::make_unique( - "move to place", stages::Connect::GroupPlannerVector{ { group_name_, sampling_planner } }); + "move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); stage->setTimeout(5.0); stage->properties().configureInitFrom(Stage::PARENT); t.add(std::move(stage)); @@ -330,7 +336,7 @@ void PickPlaceTask::init() // Set downward direction geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = "world"; + vec.header.frame_id = world_frame_; vec.vector.z = -1.0; stage->setDirection(vec); place->insert(std::move(stage)); @@ -345,15 +351,14 @@ void PickPlaceTask::init() stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); stage->properties().set("marker_ns", "place_pose"); stage->setObject(object); - // stage->setAugmentRotations(false); // Set target pose geometry_msgs::PoseStamped p; - p.header.frame_id = object_surface_frame_; + p.header.frame_id = object_refrence_frame_; p.pose.orientation.w = 1; p.pose.position.x = place_pos_x_; p.pose.position.y = place_pos_y_; - p.pose.position.z = 0.5 * object_height_ + place_surface_offset_ + 0.5 * table_height_; + p.pose.position.z = 0.5 * object_height_ + place_surface_offset_; stage->setPose(p); stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage @@ -371,8 +376,8 @@ void PickPlaceTask::init() *****************************************************/ { auto stage = std::make_unique("open hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, "hand"); - stage->setGoal("open"); + stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_name_); + stage->setGoal(open_gripper_pose_); place->insert(std::move(stage)); } @@ -382,7 +387,7 @@ void PickPlaceTask::init() { auto stage = std::make_unique("forbid collision (hand,object)"); stage->allowCollisions( - object_name_, t.getRobotModel()->getJointModelGroup("hand")->getLinkModelNamesWithCollisionGeometry(), false); + object_name_, t.getRobotModel()->getJointModelGroup(hand_name_)->getLinkModelNamesWithCollisionGeometry(), false); place->insert(std::move(stage)); } @@ -423,7 +428,7 @@ void PickPlaceTask::init() { auto stage = std::make_unique("move home", sampling_planner); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setGoal("ready"); + stage->setGoal(home_pose_); stage->restrictDirection(stages::MoveTo::FORWARD); t.add(std::move(stage)); } @@ -431,9 +436,12 @@ void PickPlaceTask::init() bool PickPlaceTask::plan() { + ros::NodeHandle pnh("~"); + int number_of_plans = pnh.param("number_of_plans", 10); + try { - task_->plan(10); + task_->plan(number_of_plans); } catch (InitStageException& e) { From 616a7d4da59e37ae6e5232ed06063e87561ba61f Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Tue, 6 Aug 2019 14:50:25 -0600 Subject: [PATCH 07/18] propper clang formatting --- .../pick_place_task.h | 99 +- demo/src/moveit_task_constructor_demo.cpp | 156 ++-- demo/src/pick_place_task.cpp | 848 +++++++++--------- 3 files changed, 544 insertions(+), 559 deletions(-) diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h index 97a1fb7db..429a49507 100644 --- a/demo/include/moveit_task_constructor_demo/pick_place_task.h +++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h @@ -66,66 +66,65 @@ #ifndef MOVEIT_TASK_CONSTRUCTOR_DEMO_PICK_PLACE_TASK_H #define MOVEIT_TASK_CONSTRUCTOR_DEMO_PICK_PLACE_TASK_H -namespace moveit_task_constructor_demo -{ +namespace moveit_task_constructor_demo { using namespace moveit::task_constructor; class PickPlaceTask { public: - PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); - ~PickPlaceTask() = default; + PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); + ~PickPlaceTask() = default; - void init(); + void init(); - bool plan(); + bool plan(); - bool execute(); + bool execute(); private: - ros::NodeHandle nh_; - - std::string task_name_; - moveit::task_constructor::TaskPtr task_; - - // planning group properties - std::string arm_group_name_; - std::string eef_name_; - std::string hand_name_; - std::string hand_frame_; - - // object + surface - std::vector support_surfaces_; - std::string table_refrence_frame_; - std::string object_refrence_frame_; - std::string surface_link_; - std::string object_name_; - std::string table_name_; - std::string world_frame_; - double object_height_; - double object_radius_; - double table_height_; - double table_length_; - double table_width_; - - // pose_names - std::string open_gripper_pose_; - std::string close_gripper_pose_; - std::string home_pose_; - - // execution - actionlib::SimpleActionClient execute_; - - // ros params - double approach_object_min_dist_; - double approach_object_max_dist_; - double lift_object_min_dist_; - double lift_object_max_dist_; - double place_pos_x_; - double place_pos_y_; - double place_surface_offset_; - - Eigen::Isometry3d grasp_frame_transform_; + ros::NodeHandle nh_; + + std::string task_name_; + moveit::task_constructor::TaskPtr task_; + + // planning group properties + std::string arm_group_name_; + std::string eef_name_; + std::string hand_name_; + std::string hand_frame_; + + // object + surface + std::vector support_surfaces_; + std::string table_refrence_frame_; + std::string object_refrence_frame_; + std::string surface_link_; + std::string object_name_; + std::string table_name_; + std::string world_frame_; + double object_height_; + double object_radius_; + double table_height_; + double table_length_; + double table_width_; + + // pose_names + std::string open_gripper_pose_; + std::string close_gripper_pose_; + std::string home_pose_; + + // execution + actionlib::SimpleActionClient execute_; + + // ros params + double approach_object_min_dist_; + double approach_object_max_dist_; + double lift_object_min_dist_; + double lift_object_max_dist_; + double place_pos_x_; + double place_pos_y_; + double place_surface_offset_; + + Eigen::Isometry3d grasp_frame_transform_; }; } // moveit_task_constructor_demo #endif // MOVEIT_TASK_CONSTRUCTOR_DEMO_PICK_PLACE_TASK_H diff --git a/demo/src/moveit_task_constructor_demo.cpp b/demo/src/moveit_task_constructor_demo.cpp index 5c47fd5cb..e860a2e0d 100644 --- a/demo/src/moveit_task_constructor_demo.cpp +++ b/demo/src/moveit_task_constructor_demo.cpp @@ -43,95 +43,91 @@ #include #include -void spawnTable() -{ - ros::NodeHandle pnh("~"); - std::string table_name = pnh.param("table_name", "table_name"); - std::string table_subframe_name = pnh.param("table_subframe_name", "table_subframe_name"); - std::string table_refrence_frame = pnh.param("table_refrence_frame", "frame_table_is_in"); - double height = pnh.param("table_height", 0.0); - double width = pnh.param("table_width", 0.0); - double length = pnh.param("table_length", 0.0); - double position_x = pnh.param("table_pos_x", 0.0); - double position_y = pnh.param("table_pos_y", 0.0); +void spawnTable() { + ros::NodeHandle pnh("~"); + std::string table_name = pnh.param("table_name", "table_name"); + std::string table_subframe_name = pnh.param("table_subframe_name", "table_subframe_name"); + std::string table_refrence_frame = pnh.param("table_refrence_frame", "frame_table_is_in"); + double height = pnh.param("table_height", 0.0); + double width = pnh.param("table_width", 0.0); + double length = pnh.param("table_length", 0.0); + double position_x = pnh.param("table_pos_x", 0.0); + double position_y = pnh.param("table_pos_y", 0.0); - moveit::planning_interface::PlanningSceneInterface psi; - moveit_msgs::CollisionObject object; - object.id = table_name = table_name; - object.header.frame_id = table_refrence_frame; - object.primitives.resize(1); - object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; - object.primitives[0].dimensions = { length, width, height }; - object.primitive_poses.resize(1); - object.primitive_poses[0].position.x = position_x; - object.primitive_poses[0].position.y = position_y; - object.primitive_poses[0].position.z = 0.5 * height; - object.primitive_poses[0].orientation.w = 1.0; + moveit::planning_interface::PlanningSceneInterface psi; + moveit_msgs::CollisionObject object; + object.id = table_name = table_name; + object.header.frame_id = table_refrence_frame; + object.primitives.resize(1); + object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; + object.primitives[0].dimensions = { length, width, height }; + object.primitive_poses.resize(1); + object.primitive_poses[0].position.x = position_x; + object.primitive_poses[0].position.y = position_y; + object.primitive_poses[0].position.z = 0.5 * height; + object.primitive_poses[0].orientation.w = 1.0; - // Make the table top subframe at the center of the top surface - object.subframe_names.resize(1); - object.subframe_poses.resize(1); - object.subframe_names[0] = table_subframe_name; - object.subframe_poses[0].position.x = position_x; - object.subframe_poses[0].position.y = position_y; - object.subframe_poses[0].position.z = height; + // Make the table top subframe at the center of the top surface + object.subframe_names.resize(1); + object.subframe_poses.resize(1); + object.subframe_names[0] = table_subframe_name; + object.subframe_poses[0].position.x = position_x; + object.subframe_poses[0].position.y = position_y; + object.subframe_poses[0].position.z = height; - psi.applyCollisionObject(object); + psi.applyCollisionObject(object); } -void spawnObject() -{ - ros::NodeHandle pnh("~"); - std::string object_name = pnh.param("object_name", "name_of_moved_object"); - std::string object_refrence_frame = pnh.param("object_refrence_frame", "frame_object_lays_upon"); - double height = pnh.param("object_height", 0.0); - double radius = pnh.param("object_radius", 0.0); - double position_x = pnh.param("object_pos_x", 0.0); - double position_y = pnh.param("object_pos_y", 0.0); - double table_height = pnh.param("table_height", 0.0); - double place_surface_offset = pnh.param("place_surface_offset", 0.0); +void spawnObject() { + ros::NodeHandle pnh("~"); + std::string object_name = pnh.param("object_name", "name_of_moved_object"); + std::string object_refrence_frame = pnh.param("object_refrence_frame", "frame_object_lays_upon"); + double height = pnh.param("object_height", 0.0); + double radius = pnh.param("object_radius", 0.0); + double position_x = pnh.param("object_pos_x", 0.0); + double position_y = pnh.param("object_pos_y", 0.0); + double table_height = pnh.param("table_height", 0.0); + double place_surface_offset = pnh.param("place_surface_offset", 0.0); - moveit::planning_interface::PlanningSceneInterface psi; - moveit_msgs::CollisionObject object; - object.id = object_name = object_name; - object.header.frame_id = object_refrence_frame; - object.primitives.resize(1); - object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; - object.primitives[0].dimensions = { height, radius }; - object.primitive_poses.resize(1); - object.primitive_poses[0].position.x = position_x; - object.primitive_poses[0].position.y = position_y; - object.primitive_poses[0].position.z = 0.5 * height + place_surface_offset; - object.primitive_poses[0].orientation.w = 1.0; - psi.applyCollisionObject(object); + moveit::planning_interface::PlanningSceneInterface psi; + moveit_msgs::CollisionObject object; + object.id = object_name = object_name; + object.header.frame_id = object_refrence_frame; + object.primitives.resize(1); + object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; + object.primitives[0].dimensions = { height, radius }; + object.primitive_poses.resize(1); + object.primitive_poses[0].position.x = position_x; + object.primitive_poses[0].position.y = position_y; + object.primitive_poses[0].position.z = 0.5 * height + place_surface_offset; + object.primitive_poses[0].orientation.w = 1.0; + psi.applyCollisionObject(object); } -int main(int argc, char** argv) -{ - ROS_INFO("Init moveit_task_constructor_demo"); - ros::init(argc, argv, "moveit_task_constructor_demo"); - ros::NodeHandle nh; - ros::AsyncSpinner spinner(1); - spinner.start(); +int main(int argc, char** argv) { + ROS_INFO("Init moveit_task_constructor_demo"); + ros::init(argc, argv, "moveit_task_constructor_demo"); + ros::NodeHandle nh; + ros::AsyncSpinner spinner(1); + spinner.start(); - // Add table and object to planning scene - ros::Duration(0.3).sleep(); - spawnTable(); - ros::Duration(0.3).sleep(); - spawnObject(); - ros::Duration(0.3).sleep(); + // Add table and object to planning scene + ros::Duration(0.3).sleep(); + spawnTable(); + ros::Duration(0.3).sleep(); + spawnObject(); + ros::Duration(0.3).sleep(); - // Construct and run pick/place task - moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh); - pick_place_task.init(); - if (pick_place_task.plan()) - { - ROS_INFO("Planning succeded"); - pick_place_task.execute(); - } - ROS_INFO("Planning failed"); + // Construct and run pick/place task + moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh); + pick_place_task.init(); + if (pick_place_task.plan()) { + ROS_INFO("Planning succeded"); + pick_place_task.execute(); + } + ROS_INFO("Planning failed"); - // Keep introspection alive - ros::waitForShutdown(); - return 0; + // Keep introspection alive + ros::waitForShutdown(); + return 0; } diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 2c903adcc..3d8c9011d 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -36,445 +36,435 @@ #include -namespace moveit_task_constructor_demo -{ +namespace moveit_task_constructor_demo { PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh) - : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) -{ - ROS_INFO("waiting for task execution"); - - /**************************************************** - * * - * Load Parameters * - * * - ***************************************************/ - ros::NodeHandle pnh("~"); - - // Planning group properties - arm_group_name_ = pnh.param("arm_group_name", "arm_group_name"); - hand_name_ = pnh.param("hand_name", "hand_group_name"); - eef_name_ = pnh.param("eef_name", "eef_name"); - hand_frame_ = pnh.param("hand_frame", "hand_frame_name"); - world_frame_ = pnh.param("world_frame", "world_frame_name"); - - // poses - open_gripper_pose_ = pnh.param("open_gripper_pose", "open_pose"); - close_gripper_pose_ = pnh.param("close_gripper_pose", "close_pose"); - home_pose_ = pnh.param("home_pose", "home_pose"); - - // Object + surface - table_name_ = pnh.param("table_name", "table_name"); - table_height_ = pnh.param("table_height", 0.0); - table_length_ = pnh.param("table_length", 0.0); - table_width_ = pnh.param("table_width", 0.0); - - object_name_ = pnh.param("object_name", "name_of_moved_object"); - object_height_ = pnh.param("object_height", 0.0); - object_radius_ = pnh.param("object_radius", 0.0); - - table_refrence_frame_ = pnh.param("table_refrence_frame", "frame_table_is_in"); - object_refrence_frame_ = pnh.param("object_refrence_frame", "frame_object_lays_upon"); - surface_link_ = pnh.param("surface_link", "name_of_table"); - support_surfaces_ = { surface_link_ }; - - // Pick - approach_object_min_dist_ = pnh.param("approach_object_min_dist", 0.0); - approach_object_max_dist_ = pnh.param("approach_object_max_dist", 0.0); - - // Lift - lift_object_min_dist_ = pnh.param("lift_object_min_dist", 0.0); - lift_object_max_dist_ = pnh.param("lift_object_max_dist", 0.0); - - // Place - place_surface_offset_ = pnh.param("place_surface_offset", 0.0); - place_pos_x_ = pnh.param("place_pos_x", 0.0); - place_pos_y_ = pnh.param("place_pos_y", 0.0); - - // compute hand grasp frame - double rotationy = pnh.param("grasp_rotation_y", 0.0); - double rotationx = pnh.param("grasp_rotation_x", 0.0); - double grasp_offset_x = pnh.param("grasp_offset_x", 0.0); - double grasp_offset_y = pnh.param("grasp_offset_y", 0.0); - double grasp_offset_z = pnh.param("prasp_offset_z", 0.0); - grasp_frame_transform_ = Eigen::AngleAxisd(M_PI * rotationy, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(M_PI * rotationx, Eigen::Vector3d::UnitX()) * - Eigen::Translation3d(grasp_offset_x, grasp_offset_y, grasp_offset_z); + : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) { + ROS_INFO("waiting for task execution"); + + /**************************************************** + * * + * Load Parameters * + * * + ***************************************************/ + ros::NodeHandle pnh("~"); + + // Planning group properties + arm_group_name_ = pnh.param("arm_group_name", "arm_group_name"); + hand_name_ = pnh.param("hand_name", "hand_group_name"); + eef_name_ = pnh.param("eef_name", "eef_name"); + hand_frame_ = pnh.param("hand_frame", "hand_frame_name"); + world_frame_ = pnh.param("world_frame", "world_frame_name"); + + // poses + open_gripper_pose_ = pnh.param("open_gripper_pose", "open_pose"); + close_gripper_pose_ = pnh.param("close_gripper_pose", "close_pose"); + home_pose_ = pnh.param("home_pose", "home_pose"); + + // Object + surface + table_name_ = pnh.param("table_name", "table_name"); + table_height_ = pnh.param("table_height", 0.0); + table_length_ = pnh.param("table_length", 0.0); + table_width_ = pnh.param("table_width", 0.0); + + object_name_ = pnh.param("object_name", "name_of_moved_object"); + object_height_ = pnh.param("object_height", 0.0); + object_radius_ = pnh.param("object_radius", 0.0); + + table_refrence_frame_ = pnh.param("table_refrence_frame", "frame_table_is_in"); + object_refrence_frame_ = pnh.param("object_refrence_frame", "frame_object_lays_upon"); + surface_link_ = pnh.param("surface_link", "name_of_table"); + support_surfaces_ = { surface_link_ }; + + // Pick + approach_object_min_dist_ = pnh.param("approach_object_min_dist", 0.0); + approach_object_max_dist_ = pnh.param("approach_object_max_dist", 0.0); + + // Lift + lift_object_min_dist_ = pnh.param("lift_object_min_dist", 0.0); + lift_object_max_dist_ = pnh.param("lift_object_max_dist", 0.0); + + // Place + place_surface_offset_ = pnh.param("place_surface_offset", 0.0); + place_pos_x_ = pnh.param("place_pos_x", 0.0); + place_pos_y_ = pnh.param("place_pos_y", 0.0); + + // compute hand grasp frame + double rotationy = pnh.param("grasp_rotation_y", 0.0); + double rotationx = pnh.param("grasp_rotation_x", 0.0); + double grasp_offset_x = pnh.param("grasp_offset_x", 0.0); + double grasp_offset_y = pnh.param("grasp_offset_y", 0.0); + double grasp_offset_z = pnh.param("prasp_offset_z", 0.0); + grasp_frame_transform_ = Eigen::AngleAxisd(M_PI * rotationy, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(M_PI * rotationx, Eigen::Vector3d::UnitX()) * + Eigen::Translation3d(grasp_offset_x, grasp_offset_y, grasp_offset_z); } -void PickPlaceTask::init() -{ - const std::string object = "object"; - - // 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& t = *task_; - t.loadRobotModel(); - - // Sampling planner - auto sampling_planner = std::make_shared(); - sampling_planner->setProperty("goal_joint_tolerance", 1e-5); - - // Cartesian planner - auto cartesian_planner = std::make_shared(); - cartesian_planner->setMaxVelocityScaling(1.0); - cartesian_planner->setMaxAccelerationScaling(1.0); - cartesian_planner->setStepSize(.01); - - // Set task properties - t.setProperty("group", arm_group_name_); - t.setProperty("eef", eef_name_); - t.setProperty("hand", hand_name_); - t.setProperty("hand_grasping_frame", hand_frame_); - t.setProperty("ik_frame", hand_frame_); - - /**************************************************** - * * - * Current State * - * * - ***************************************************/ - Stage* current_state = nullptr; // Forward current_state on to grasp pose generator - { - auto _current_state = std::make_unique("current state"); - - // Verify that object is not attachd - auto applicability_filter = - std::make_unique("applicability test", std::move(_current_state)); - applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) { - if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) - { - comment = "object with id '" + object + "' is already attached and cannot be picked"; - return false; - } - return true; - }); - - current_state = applicability_filter.get(); - t.add(std::move(applicability_filter)); - } - - /**************************************************** - * * - * Open Hand * - * * - ***************************************************/ - { // Open Hand - auto stage = std::make_unique("open hand", sampling_planner); - stage->setGroup(hand_name_); - stage->setGoal(open_gripper_pose_); - t.add(std::move(stage)); - } - - /**************************************************** - * * - * Move to Pick * - * * - ***************************************************/ - { // Move-to pre-grasp - auto stage = std::make_unique( - "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); - stage->setTimeout(5.0); - stage->properties().configureInitFrom(Stage::PARENT); - t.add(std::move(stage)); - } - - /**************************************************** - * * - * Pick Object * - * * - ***************************************************/ - Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator - { - auto grasp = std::make_unique("pick object"); - t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); - grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); - - /**************************************************** ----- * Approach Object * - ***************************************************/ - { - auto stage = std::make_unique("approach object", cartesian_planner); - stage->properties().set("marker_ns", "approach_object"); - stage->properties().set("link", hand_frame_); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); - - // Set hand forward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = hand_frame_; - vec.vector.z = 1.0; - stage->setDirection(vec); - grasp->insert(std::move(stage)); - } - - /**************************************************** ----- * Generate Grasp Pose * - ***************************************************/ - { - // Sample grasp pose - auto stage = std::make_unique("generate grasp pose"); - stage->properties().configureInitFrom(Stage::PARENT); - stage->properties().set("marker_ns", "grasp_pose"); - stage->setPreGraspPose(open_gripper_pose_); - stage->setObject(object); - stage->setAngleDelta(M_PI / 12); - stage->setMonitoredStage(current_state); // Hook into current state - - // Compute IK - auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); - wrapper->setMaxIKSolutions(8); - wrapper->setMinSolutionDistance(1.0); - wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); - wrapper->properties().configureInitFrom(Stage::PARENT, { "eef" }); - wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); - grasp->insert(std::move(wrapper)); - } - - /**************************************************** ----- * Allow Collision (hand object) * - ***************************************************/ - { - auto stage = std::make_unique("allow collision (hand,object)"); - stage->allowCollisions( - object, t.getRobotModel()->getJointModelGroup(hand_name_)->getLinkModelNamesWithCollisionGeometry(), true); - grasp->insert(std::move(stage)); - } - - /**************************************************** ----- * Close Hand * - ***************************************************/ - { - auto stage = std::make_unique("close hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_name_); - stage->setGoal(close_gripper_pose_); - grasp->insert(std::move(stage)); - } - - /**************************************************** -.... * Attach Object * - ***************************************************/ - { - auto stage = std::make_unique("attach object"); - stage->attachObject(object, hand_frame_); - attach_object_stage = stage.get(); - grasp->insert(std::move(stage)); - } - - /**************************************************** -.... * Allow collision (object support) * - ***************************************************/ - { - auto stage = std::make_unique("allow collision (object,support)"); - stage->allowCollisions({ object }, support_surfaces_, true); - grasp->insert(std::move(stage)); - } - - /**************************************************** -.... * Lift object * - ***************************************************/ - { - auto stage = std::make_unique("lift object", cartesian_planner); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(lift_object_min_dist_, lift_object_max_dist_); - stage->setIKFrame(hand_frame_); - stage->properties().set("marker_ns", "lift_object"); - - // Set upward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = world_frame_; - vec.vector.z = 1.0; - stage->setDirection(vec); - grasp->insert(std::move(stage)); - } - - /**************************************************** -.... * Forbid collision (object support) * - ***************************************************/ - { - auto stage = std::make_unique("forbid collision (object,surface)"); - stage->allowCollisions({ object }, support_surfaces_, false); - grasp->insert(std::move(stage)); - } - - // Add grasp container to task - t.add(std::move(grasp)); - } - - /****************************************************** - * * - * Move to Place * - * * - *****************************************************/ - { - auto stage = std::make_unique( - "move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); - stage->setTimeout(5.0); - stage->properties().configureInitFrom(Stage::PARENT); - t.add(std::move(stage)); - } - - /****************************************************** - * * - * Place Object * - * * - *****************************************************/ - { - auto place = std::make_unique("place object"); - t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); - place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); - - /****************************************************** ----- * Lower Object * - *****************************************************/ - { - auto stage = std::make_unique("lower object", cartesian_planner); - stage->properties().set("marker_ns", "lower_object"); - stage->properties().set("link", hand_frame_); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(.03, .13); - - // Set downward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = world_frame_; - vec.vector.z = -1.0; - stage->setDirection(vec); - place->insert(std::move(stage)); - } - - /****************************************************** ----- * Generate Place Pose * - *****************************************************/ - { - // Generate Place Pose - auto stage = std::make_unique("generate place pose"); - stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); - stage->properties().set("marker_ns", "place_pose"); - stage->setObject(object); - - // Set target pose - geometry_msgs::PoseStamped p; - p.header.frame_id = object_refrence_frame_; - p.pose.orientation.w = 1; - p.pose.position.x = place_pos_x_; - p.pose.position.y = place_pos_y_; - p.pose.position.z = 0.5 * object_height_ + place_surface_offset_; - stage->setPose(p); - stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage - - // Compute IK - auto wrapper = std::make_unique("place pose IK", std::move(stage)); - wrapper->setMaxIKSolutions(2); - wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); - wrapper->properties().configureInitFrom(Stage::PARENT, { "eef" }); - wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); - place->insert(std::move(wrapper)); - } - - /****************************************************** ----- * Open Hand * - *****************************************************/ - { - auto stage = std::make_unique("open hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_name_); - stage->setGoal(open_gripper_pose_); - place->insert(std::move(stage)); - } - - /****************************************************** ----- * Forbid collision (hand, object) * - *****************************************************/ - { - auto stage = std::make_unique("forbid collision (hand,object)"); - stage->allowCollisions( - object_name_, t.getRobotModel()->getJointModelGroup(hand_name_)->getLinkModelNamesWithCollisionGeometry(), false); - place->insert(std::move(stage)); - } - - /****************************************************** ----- * Detach Object * - *****************************************************/ - { - auto stage = std::make_unique("detach object"); - stage->detachObject(object_name_, hand_frame_); - place->insert(std::move(stage)); - } - - /****************************************************** ----- * Retreat Motion * - *****************************************************/ - { - auto stage = std::make_unique("retreat after place", cartesian_planner); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(.12, .25); - stage->setIKFrame(hand_frame_); - stage->properties().set("marker_ns", "retreat"); - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = hand_frame_; - vec.vector.z = -1.0; - stage->setDirection(vec); - place->insert(std::move(stage)); - } - - // Add place container to task - t.add(std::move(place)); - } - - /****************************************************** - * * - * Move to Home * - * * - *****************************************************/ - { - auto stage = std::make_unique("move home", sampling_planner); - stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setGoal(home_pose_); - stage->restrictDirection(stages::MoveTo::FORWARD); - t.add(std::move(stage)); - } +void PickPlaceTask::init() { + const std::string object = "object"; + + // 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& t = *task_; + t.loadRobotModel(); + + // Sampling planner + auto sampling_planner = std::make_shared(); + sampling_planner->setProperty("goal_joint_tolerance", 1e-5); + + // Cartesian planner + auto cartesian_planner = std::make_shared(); + cartesian_planner->setMaxVelocityScaling(1.0); + cartesian_planner->setMaxAccelerationScaling(1.0); + cartesian_planner->setStepSize(.01); + + // Set task properties + t.setProperty("group", arm_group_name_); + t.setProperty("eef", eef_name_); + t.setProperty("hand", hand_name_); + t.setProperty("hand_grasping_frame", hand_frame_); + t.setProperty("ik_frame", hand_frame_); + + /**************************************************** + * * + * Current State * + * * + ***************************************************/ + Stage* current_state = nullptr; // Forward current_state on to grasp pose generator + { + auto _current_state = std::make_unique("current state"); + + // Verify that object is not attachd + auto applicability_filter = + std::make_unique("applicability test", std::move(_current_state)); + applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) { + if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) { + comment = "object with id '" + object + "' is already attached and cannot be picked"; + return false; + } + return true; + }); + + current_state = applicability_filter.get(); + t.add(std::move(applicability_filter)); + } + + /**************************************************** + * * + * Open Hand * + * * + ***************************************************/ + { // Open Hand + auto stage = std::make_unique("open hand", sampling_planner); + stage->setGroup(hand_name_); + stage->setGoal(open_gripper_pose_); + t.add(std::move(stage)); + } + + /**************************************************** + * * + * Move to Pick * + * * + ***************************************************/ + { // Move-to pre-grasp + auto stage = std::make_unique( + "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } + + /**************************************************** + * * + * Pick Object * + * * + ***************************************************/ + Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator + { + auto grasp = std::make_unique("pick object"); + t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); + grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); + + /**************************************************** + ---- * Approach Object * + ***************************************************/ + { + auto stage = std::make_unique("approach object", cartesian_planner); + stage->properties().set("marker_ns", "approach_object"); + stage->properties().set("link", hand_frame_); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); + + // Set hand forward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = hand_frame_; + vec.vector.z = 1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } + + /**************************************************** + ---- * Generate Grasp Pose * + ***************************************************/ + { + // Sample grasp pose + auto stage = std::make_unique("generate grasp pose"); + stage->properties().configureInitFrom(Stage::PARENT); + stage->properties().set("marker_ns", "grasp_pose"); + stage->setPreGraspPose(open_gripper_pose_); + stage->setObject(object); + stage->setAngleDelta(M_PI / 12); + stage->setMonitoredStage(current_state); // Hook into current state + + // Compute IK + auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(8); + wrapper->setMinSolutionDistance(1.0); + wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef" }); + wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + grasp->insert(std::move(wrapper)); + } + + /**************************************************** + ---- * Allow Collision (hand object) * + ***************************************************/ + { + auto stage = std::make_unique("allow collision (hand,object)"); + stage->allowCollisions( + object, t.getRobotModel()->getJointModelGroup(hand_name_)->getLinkModelNamesWithCollisionGeometry(), true); + grasp->insert(std::move(stage)); + } + + /**************************************************** + ---- * Close Hand * + ***************************************************/ + { + auto stage = std::make_unique("close hand", sampling_planner); + stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_name_); + stage->setGoal(close_gripper_pose_); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Attach Object * + ***************************************************/ + { + auto stage = std::make_unique("attach object"); + stage->attachObject(object, hand_frame_); + attach_object_stage = stage.get(); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Allow collision (object support) * + ***************************************************/ + { + auto stage = std::make_unique("allow collision (object,support)"); + stage->allowCollisions({ object }, support_surfaces_, true); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Lift object * + ***************************************************/ + { + auto stage = std::make_unique("lift object", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(lift_object_min_dist_, lift_object_max_dist_); + stage->setIKFrame(hand_frame_); + stage->properties().set("marker_ns", "lift_object"); + + // Set upward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = world_frame_; + vec.vector.z = 1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Forbid collision (object support) * + ***************************************************/ + { + auto stage = std::make_unique("forbid collision (object,surface)"); + stage->allowCollisions({ object }, support_surfaces_, false); + grasp->insert(std::move(stage)); + } + + // Add grasp container to task + t.add(std::move(grasp)); + } + + /****************************************************** + * * + * Move to Place * + * * + *****************************************************/ + { + auto stage = std::make_unique( + "move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } + + /****************************************************** + * * + * Place Object * + * * + *****************************************************/ + { + auto place = std::make_unique("place object"); + t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); + place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); + + /****************************************************** + ---- * Lower Object * + *****************************************************/ + { + auto stage = std::make_unique("lower object", cartesian_planner); + stage->properties().set("marker_ns", "lower_object"); + stage->properties().set("link", hand_frame_); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(.03, .13); + + // Set downward direction + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = world_frame_; + vec.vector.z = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Generate Place Pose * + *****************************************************/ + { + // Generate Place Pose + auto stage = std::make_unique("generate place pose"); + stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); + stage->properties().set("marker_ns", "place_pose"); + stage->setObject(object); + + // Set target pose + geometry_msgs::PoseStamped p; + p.header.frame_id = object_refrence_frame_; + p.pose.orientation.w = 1; + p.pose.position.x = place_pos_x_; + p.pose.position.y = place_pos_y_; + p.pose.position.z = 0.5 * object_height_ + place_surface_offset_; + stage->setPose(p); + stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage + + // Compute IK + auto wrapper = std::make_unique("place pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(2); + wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef" }); + wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + place->insert(std::move(wrapper)); + } + + /****************************************************** + ---- * Open Hand * + *****************************************************/ + { + auto stage = std::make_unique("open hand", sampling_planner); + stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_name_); + stage->setGoal(open_gripper_pose_); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Forbid collision (hand, object) * + *****************************************************/ + { + auto stage = std::make_unique("forbid collision (hand,object)"); + stage->allowCollisions( + object_name_, t.getRobotModel()->getJointModelGroup(hand_name_)->getLinkModelNamesWithCollisionGeometry(), + false); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Detach Object * + *****************************************************/ + { + auto stage = std::make_unique("detach object"); + stage->detachObject(object_name_, hand_frame_); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Retreat Motion * + *****************************************************/ + { + auto stage = std::make_unique("retreat after place", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(.12, .25); + stage->setIKFrame(hand_frame_); + stage->properties().set("marker_ns", "retreat"); + geometry_msgs::Vector3Stamped vec; + vec.header.frame_id = hand_frame_; + vec.vector.z = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } + + // Add place container to task + t.add(std::move(place)); + } + + /****************************************************** + * * + * Move to Home * + * * + *****************************************************/ + { + auto stage = std::make_unique("move home", sampling_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setGoal(home_pose_); + stage->restrictDirection(stages::MoveTo::FORWARD); + t.add(std::move(stage)); + } } -bool PickPlaceTask::plan() -{ - ros::NodeHandle pnh("~"); - int number_of_plans = pnh.param("number_of_plans", 10); - - try - { - task_->plan(number_of_plans); - } - catch (InitStageException& e) - { - ROS_ERROR_STREAM("Initialization failed: " << e); - return false; - } - if (task_->numSolutions() == 0) - { - ROS_ERROR("Planning failed"); - return false; - } - return true; +bool PickPlaceTask::plan() { + ros::NodeHandle pnh("~"); + int number_of_plans = pnh.param("number_of_plans", 10); + + try { + task_->plan(number_of_plans); + } catch (InitStageException& e) { + ROS_ERROR_STREAM("Initialization failed: " << e); + return false; + } + if (task_->numSolutions() == 0) { + ROS_ERROR("Planning failed"); + return false; + } + return true; } -bool PickPlaceTask::execute() -{ - moveit_task_constructor_msgs::Solution solution; - task_->solutions().front()->fillMessage(solution); +bool PickPlaceTask::execute() { + moveit_task_constructor_msgs::Solution solution; + task_->solutions().front()->fillMessage(solution); - ROS_INFO_STREAM("last trajectory in solution:\n" << solution.sub_trajectory.back().trajectory); + ROS_INFO_STREAM("last trajectory in solution:\n" << solution.sub_trajectory.back().trajectory); - moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; - execute_goal.solution = solution; - execute_.sendGoal(execute_goal); - execute_.waitForResult(); - moveit_msgs::MoveItErrorCodes execute_result = execute_.getResult()->error_code; + moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; + execute_goal.solution = solution; + execute_.sendGoal(execute_goal); + execute_.waitForResult(); + moveit_msgs::MoveItErrorCodes execute_result = execute_.getResult()->error_code; - if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) - { - ROS_ERROR_STREAM("task execution failed and returned: " << execute_.getState().toString()); - return false; - } + if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { + ROS_ERROR_STREAM("task execution failed and returned: " << execute_.getState().toString()); + return false; + } - return true; + return true; } } From ec3afd64bbec2c13b973a54f813f10df3cb99aa7 Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Thu, 8 Aug 2019 15:45:03 -0600 Subject: [PATCH 08/18] removed subframes to pass travis ci --- demo/config/panda_config.yaml | 3 +-- demo/src/moveit_task_constructor_demo.cpp | 12 +----------- demo/src/pick_place_task.cpp | 2 +- 3 files changed, 3 insertions(+), 14 deletions(-) diff --git a/demo/config/panda_config.yaml b/demo/config/panda_config.yaml index 6fe8b0172..950538882 100644 --- a/demo/config/panda_config.yaml +++ b/demo/config/panda_config.yaml @@ -12,7 +12,7 @@ home_pose: "ready" # surface table_refrence_frame: "world" -object_refrence_frame: "table/table_top" +object_refrence_frame: "table" surface_link: "table" world_frame: "world" @@ -25,7 +25,6 @@ object_pos_x: 0.0 object_pos_y: 0.0 table_name: "table" -table_subframe_name: "table_top" table_height: 0.1 table_width: 0.5 table_length: 0.4 diff --git a/demo/src/moveit_task_constructor_demo.cpp b/demo/src/moveit_task_constructor_demo.cpp index e860a2e0d..0039d8c01 100644 --- a/demo/src/moveit_task_constructor_demo.cpp +++ b/demo/src/moveit_task_constructor_demo.cpp @@ -46,7 +46,6 @@ void spawnTable() { ros::NodeHandle pnh("~"); std::string table_name = pnh.param("table_name", "table_name"); - std::string table_subframe_name = pnh.param("table_subframe_name", "table_subframe_name"); std::string table_refrence_frame = pnh.param("table_refrence_frame", "frame_table_is_in"); double height = pnh.param("table_height", 0.0); double width = pnh.param("table_width", 0.0); @@ -66,15 +65,6 @@ void spawnTable() { object.primitive_poses[0].position.y = position_y; object.primitive_poses[0].position.z = 0.5 * height; object.primitive_poses[0].orientation.w = 1.0; - - // Make the table top subframe at the center of the top surface - object.subframe_names.resize(1); - object.subframe_poses.resize(1); - object.subframe_names[0] = table_subframe_name; - object.subframe_poses[0].position.x = position_x; - object.subframe_poses[0].position.y = position_y; - object.subframe_poses[0].position.z = height; - psi.applyCollisionObject(object); } @@ -99,7 +89,7 @@ void spawnObject() { object.primitive_poses.resize(1); object.primitive_poses[0].position.x = position_x; object.primitive_poses[0].position.y = position_y; - object.primitive_poses[0].position.z = 0.5 * height + place_surface_offset; + object.primitive_poses[0].position.z = 0.5 * (height + table_height) + place_surface_offset; object.primitive_poses[0].orientation.w = 1.0; psi.applyCollisionObject(object); } diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 3d8c9011d..91d6a5bd2 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -354,7 +354,7 @@ void PickPlaceTask::init() { p.pose.orientation.w = 1; p.pose.position.x = place_pos_x_; p.pose.position.y = place_pos_y_; - p.pose.position.z = 0.5 * object_height_ + place_surface_offset_; + p.pose.position.z = 0.5 * (object_height_ + table_height_) + place_surface_offset_; stage->setPose(p); stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage From e23633563279815d6afb711aee98257ee617af36 Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Fri, 9 Aug 2019 18:09:30 -0600 Subject: [PATCH 09/18] parameter method --- .../pick_place_task.h | 2 + demo/src/moveit_task_constructor_demo.cpp | 1 + demo/src/pick_place_task.cpp | 114 +++++++++--------- 3 files changed, 61 insertions(+), 56 deletions(-) diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h index 429a49507..18e48b9e8 100644 --- a/demo/include/moveit_task_constructor_demo/pick_place_task.h +++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h @@ -75,6 +75,8 @@ class PickPlaceTask PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); ~PickPlaceTask() = default; + void loadParameters(); + void init(); bool plan(); diff --git a/demo/src/moveit_task_constructor_demo.cpp b/demo/src/moveit_task_constructor_demo.cpp index 0039d8c01..272451dd3 100644 --- a/demo/src/moveit_task_constructor_demo.cpp +++ b/demo/src/moveit_task_constructor_demo.cpp @@ -110,6 +110,7 @@ int main(int argc, char** argv) { // Construct and run pick/place task moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh); + pick_place_task.loadParameters(); pick_place_task.init(); if (pick_place_task.plan()) { ROS_INFO("Planning succeded"); diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 91d6a5bd2..ef5c10294 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -40,63 +40,65 @@ namespace moveit_task_constructor_demo { PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh) : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) { ROS_INFO("waiting for task execution"); +} - /**************************************************** - * * - * Load Parameters * - * * - ***************************************************/ - ros::NodeHandle pnh("~"); - - // Planning group properties - arm_group_name_ = pnh.param("arm_group_name", "arm_group_name"); - hand_name_ = pnh.param("hand_name", "hand_group_name"); - eef_name_ = pnh.param("eef_name", "eef_name"); - hand_frame_ = pnh.param("hand_frame", "hand_frame_name"); - world_frame_ = pnh.param("world_frame", "world_frame_name"); - - // poses - open_gripper_pose_ = pnh.param("open_gripper_pose", "open_pose"); - close_gripper_pose_ = pnh.param("close_gripper_pose", "close_pose"); - home_pose_ = pnh.param("home_pose", "home_pose"); - - // Object + surface - table_name_ = pnh.param("table_name", "table_name"); - table_height_ = pnh.param("table_height", 0.0); - table_length_ = pnh.param("table_length", 0.0); - table_width_ = pnh.param("table_width", 0.0); - - object_name_ = pnh.param("object_name", "name_of_moved_object"); - object_height_ = pnh.param("object_height", 0.0); - object_radius_ = pnh.param("object_radius", 0.0); - - table_refrence_frame_ = pnh.param("table_refrence_frame", "frame_table_is_in"); - object_refrence_frame_ = pnh.param("object_refrence_frame", "frame_object_lays_upon"); - surface_link_ = pnh.param("surface_link", "name_of_table"); - support_surfaces_ = { surface_link_ }; - - // Pick - approach_object_min_dist_ = pnh.param("approach_object_min_dist", 0.0); - approach_object_max_dist_ = pnh.param("approach_object_max_dist", 0.0); - - // Lift - lift_object_min_dist_ = pnh.param("lift_object_min_dist", 0.0); - lift_object_max_dist_ = pnh.param("lift_object_max_dist", 0.0); - - // Place - place_surface_offset_ = pnh.param("place_surface_offset", 0.0); - place_pos_x_ = pnh.param("place_pos_x", 0.0); - place_pos_y_ = pnh.param("place_pos_y", 0.0); - - // compute hand grasp frame - double rotationy = pnh.param("grasp_rotation_y", 0.0); - double rotationx = pnh.param("grasp_rotation_x", 0.0); - double grasp_offset_x = pnh.param("grasp_offset_x", 0.0); - double grasp_offset_y = pnh.param("grasp_offset_y", 0.0); - double grasp_offset_z = pnh.param("prasp_offset_z", 0.0); - grasp_frame_transform_ = Eigen::AngleAxisd(M_PI * rotationy, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(M_PI * rotationx, Eigen::Vector3d::UnitX()) * - Eigen::Translation3d(grasp_offset_x, grasp_offset_y, grasp_offset_z); +void PickPlaceTask::loadParameters() { + /**************************************************** + * * + * Load Parameters * + * * + ***************************************************/ + ros::NodeHandle pnh("~"); + + // Planning group properties + arm_group_name_ = pnh.param("arm_group_name", "arm_group_name"); + hand_name_ = pnh.param("hand_name", "hand_group_name"); + eef_name_ = pnh.param("eef_name", "eef_name"); + hand_frame_ = pnh.param("hand_frame", "hand_frame_name"); + world_frame_ = pnh.param("world_frame", "world_frame_name"); + + // poses + open_gripper_pose_ = pnh.param("open_gripper_pose", "open_pose"); + close_gripper_pose_ = pnh.param("close_gripper_pose", "close_pose"); + home_pose_ = pnh.param("home_pose", "home_pose"); + + // Object + surface + table_name_ = pnh.param("table_name", "table_name"); + table_height_ = pnh.param("table_height", 0.0); + table_length_ = pnh.param("table_length", 0.0); + table_width_ = pnh.param("table_width", 0.0); + + object_name_ = pnh.param("object_name", "name_of_moved_object"); + object_height_ = pnh.param("object_height", 0.0); + object_radius_ = pnh.param("object_radius", 0.0); + + table_refrence_frame_ = pnh.param("table_refrence_frame", "frame_table_is_in"); + object_refrence_frame_ = pnh.param("object_refrence_frame", "frame_object_lays_upon"); + surface_link_ = pnh.param("surface_link", "name_of_table"); + support_surfaces_ = { surface_link_ }; + + // Pick + approach_object_min_dist_ = pnh.param("approach_object_min_dist", 0.0); + approach_object_max_dist_ = pnh.param("approach_object_max_dist", 0.0); + + // Lift + lift_object_min_dist_ = pnh.param("lift_object_min_dist", 0.0); + lift_object_max_dist_ = pnh.param("lift_object_max_dist", 0.0); + + // Place + place_surface_offset_ = pnh.param("place_surface_offset", 0.0); + place_pos_x_ = pnh.param("place_pos_x", 0.0); + place_pos_y_ = pnh.param("place_pos_y", 0.0); + + // compute hand grasp frame + double rotationy = pnh.param("grasp_rotation_y", 0.0); + double rotationx = pnh.param("grasp_rotation_x", 0.0); + double grasp_offset_x = pnh.param("grasp_offset_x", 0.0); + double grasp_offset_y = pnh.param("grasp_offset_y", 0.0); + double grasp_offset_z = pnh.param("prasp_offset_z", 0.0); + grasp_frame_transform_ = Eigen::AngleAxisd(M_PI * rotationy, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(M_PI * rotationx, Eigen::Vector3d::UnitX()) * + Eigen::Translation3d(grasp_offset_x, grasp_offset_y, grasp_offset_z); } void PickPlaceTask::init() { From 053b5ce8f345e563a33f79d926cce4d2fb4b9339 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 10 Aug 2019 13:33:40 +0200 Subject: [PATCH 10/18] remove explicit sleeps --- demo/src/moveit_task_constructor_demo.cpp | 26 ++++++++++++----------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/demo/src/moveit_task_constructor_demo.cpp b/demo/src/moveit_task_constructor_demo.cpp index 272451dd3..2f0ed5cc8 100644 --- a/demo/src/moveit_task_constructor_demo.cpp +++ b/demo/src/moveit_task_constructor_demo.cpp @@ -43,7 +43,13 @@ #include #include -void spawnTable() { +void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) { + bool success = false; + while (!success) + success = psi.applyCollisionObject(object); +} + +moveit_msgs::CollisionObject createTable() { ros::NodeHandle pnh("~"); std::string table_name = pnh.param("table_name", "table_name"); std::string table_refrence_frame = pnh.param("table_refrence_frame", "frame_table_is_in"); @@ -53,7 +59,6 @@ void spawnTable() { double position_x = pnh.param("table_pos_x", 0.0); double position_y = pnh.param("table_pos_y", 0.0); - moveit::planning_interface::PlanningSceneInterface psi; moveit_msgs::CollisionObject object; object.id = table_name = table_name; object.header.frame_id = table_refrence_frame; @@ -65,10 +70,10 @@ void spawnTable() { object.primitive_poses[0].position.y = position_y; object.primitive_poses[0].position.z = 0.5 * height; object.primitive_poses[0].orientation.w = 1.0; - psi.applyCollisionObject(object); + return object; } -void spawnObject() { +moveit_msgs::CollisionObject createObject() { ros::NodeHandle pnh("~"); std::string object_name = pnh.param("object_name", "name_of_moved_object"); std::string object_refrence_frame = pnh.param("object_refrence_frame", "frame_object_lays_upon"); @@ -79,7 +84,6 @@ void spawnObject() { double table_height = pnh.param("table_height", 0.0); double place_surface_offset = pnh.param("place_surface_offset", 0.0); - moveit::planning_interface::PlanningSceneInterface psi; moveit_msgs::CollisionObject object; object.id = object_name = object_name; object.header.frame_id = object_refrence_frame; @@ -91,7 +95,7 @@ void spawnObject() { object.primitive_poses[0].position.y = position_y; object.primitive_poses[0].position.z = 0.5 * (height + table_height) + place_surface_offset; object.primitive_poses[0].orientation.w = 1.0; - psi.applyCollisionObject(object); + return object; } int main(int argc, char** argv) { @@ -102,15 +106,13 @@ int main(int argc, char** argv) { spinner.start(); // Add table and object to planning scene - ros::Duration(0.3).sleep(); - spawnTable(); - ros::Duration(0.3).sleep(); - spawnObject(); - ros::Duration(0.3).sleep(); + moveit::planning_interface::PlanningSceneInterface psi; + spawnObject(psi, createTable()); + spawnObject(psi, createObject()); // Construct and run pick/place task moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh); - pick_place_task.loadParameters(); + pick_place_task.loadParameters(); pick_place_task.init(); if (pick_place_task.plan()) { ROS_INFO("Planning succeded"); From 6d69c6937a384e939ed494398487f2416337e460 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 10 Aug 2019 13:34:01 +0200 Subject: [PATCH 11/18] demo.launch: use custom .rviz config --- demo/launch/demo.launch | 34 ++++++++++++++++++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/demo/launch/demo.launch b/demo/launch/demo.launch index 9a0700db5..d52cf9c70 100644 --- a/demo/launch/demo.launch +++ b/demo/launch/demo.launch @@ -1,7 +1,37 @@ - - + + + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + From c42c60ca51c0a4a34a2d5c32fbea3dc8f02b55dc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 10 Aug 2019 13:43:34 +0200 Subject: [PATCH 12/18] fixup! demo.launch: use custom .rviz config --- demo/config/mtc.rviz | 215 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 215 insertions(+) create mode 100644 demo/config/mtc.rviz diff --git a/demo/config/mtc.rviz b/demo/config/mtc.rviz new file mode 100644 index 000000000..e3a9fc5ab --- /dev/null +++ b/demo/config/mtc.rviz @@ -0,0 +1,215 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /Motion Planning Tasks1 + Splitter Ratio: 0.7425600290298462 + Tree Height: 270 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_visual_tools/RvizVisualToolsGui + Name: RvizVisualToolsGui + - Class: moveit_task_constructor/Motion Planning Tasks + Global Settings: + Task View Settings: + Task Expansion: All Expanded + Name: Motion Planning Tasks + Tasks View: + property_splitter: + - 87 + - 87 + solution_sorting: + column: 0 + order: 1 + solutions_splitter: + - 302 + - 70 + solutions_view_columns: ~ + tasks_view_columns: + - 153 + - 38 + - 38 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_task_constructor/Motion Planning Tasks + Enabled: true + Interrupt Display: false + Loop Animation: false + Markers: + All at once?: false + Value: true + Name: Motion Planning Tasks + Robot: + Fixed Robot Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Use Fixed Robot Color: false + Value: "" + Robot Description: robot_description + Scene: + Attached Body Color: 150; 50; 150 + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Value: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Show Trail: false + State Display Time: 0.05 s + Task Solution Topic: /moveit_task_constructor_demo/pick_place_task/solution + Tasks: + {} + Trail Step Size: 1 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: panda_link0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz_visual_tools/KeyTool + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 2.827594041824341 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.11356700211763382 + Y: 0.10592000186443329 + Z: 2.2351800055275817e-7 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.47020357847213745 + Target Frame: panda_link0 + Value: XYOrbit (rviz) + Yaw: 6.06496000289917 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 741 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + Motion Planning Tasks: + collapsed: false + Motion Planning Tasks - Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001780000028bfc020000000ffb000000100044006900730070006c006100790073010000003d0000019f000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069000000027d0000004b0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000120020002d00200053006c00690064006500720000000000ffffffff0000004100fffffffb0000002a004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b007301000001e2000000e6000000e600ffffff000003d80000028b00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RvizVisualToolsGui: + collapsed: false + Views: + collapsed: false + Width: 1366 + X: 544 + Y: 315 From 5ed82dc127ffb3e76bc64d39cad8d03d4a7fc9cb Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 10 Aug 2019 13:44:59 +0200 Subject: [PATCH 13/18] fixup! parameter method --- .../pick_place_task.h | 2 +- demo/src/pick_place_task.cpp | 112 +++++++++--------- 2 files changed, 57 insertions(+), 57 deletions(-) diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h index 18e48b9e8..17535a278 100644 --- a/demo/include/moveit_task_constructor_demo/pick_place_task.h +++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h @@ -75,7 +75,7 @@ class PickPlaceTask PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); ~PickPlaceTask() = default; - void loadParameters(); + void loadParameters(); void init(); diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index ef5c10294..abbdfab55 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -43,62 +43,62 @@ PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle } void PickPlaceTask::loadParameters() { - /**************************************************** - * * - * Load Parameters * - * * - ***************************************************/ - ros::NodeHandle pnh("~"); - - // Planning group properties - arm_group_name_ = pnh.param("arm_group_name", "arm_group_name"); - hand_name_ = pnh.param("hand_name", "hand_group_name"); - eef_name_ = pnh.param("eef_name", "eef_name"); - hand_frame_ = pnh.param("hand_frame", "hand_frame_name"); - world_frame_ = pnh.param("world_frame", "world_frame_name"); - - // poses - open_gripper_pose_ = pnh.param("open_gripper_pose", "open_pose"); - close_gripper_pose_ = pnh.param("close_gripper_pose", "close_pose"); - home_pose_ = pnh.param("home_pose", "home_pose"); - - // Object + surface - table_name_ = pnh.param("table_name", "table_name"); - table_height_ = pnh.param("table_height", 0.0); - table_length_ = pnh.param("table_length", 0.0); - table_width_ = pnh.param("table_width", 0.0); - - object_name_ = pnh.param("object_name", "name_of_moved_object"); - object_height_ = pnh.param("object_height", 0.0); - object_radius_ = pnh.param("object_radius", 0.0); - - table_refrence_frame_ = pnh.param("table_refrence_frame", "frame_table_is_in"); - object_refrence_frame_ = pnh.param("object_refrence_frame", "frame_object_lays_upon"); - surface_link_ = pnh.param("surface_link", "name_of_table"); - support_surfaces_ = { surface_link_ }; - - // Pick - approach_object_min_dist_ = pnh.param("approach_object_min_dist", 0.0); - approach_object_max_dist_ = pnh.param("approach_object_max_dist", 0.0); - - // Lift - lift_object_min_dist_ = pnh.param("lift_object_min_dist", 0.0); - lift_object_max_dist_ = pnh.param("lift_object_max_dist", 0.0); - - // Place - place_surface_offset_ = pnh.param("place_surface_offset", 0.0); - place_pos_x_ = pnh.param("place_pos_x", 0.0); - place_pos_y_ = pnh.param("place_pos_y", 0.0); - - // compute hand grasp frame - double rotationy = pnh.param("grasp_rotation_y", 0.0); - double rotationx = pnh.param("grasp_rotation_x", 0.0); - double grasp_offset_x = pnh.param("grasp_offset_x", 0.0); - double grasp_offset_y = pnh.param("grasp_offset_y", 0.0); - double grasp_offset_z = pnh.param("prasp_offset_z", 0.0); - grasp_frame_transform_ = Eigen::AngleAxisd(M_PI * rotationy, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(M_PI * rotationx, Eigen::Vector3d::UnitX()) * - Eigen::Translation3d(grasp_offset_x, grasp_offset_y, grasp_offset_z); + /**************************************************** + * * + * Load Parameters * + * * + ***************************************************/ + ros::NodeHandle pnh("~"); + + // Planning group properties + arm_group_name_ = pnh.param("arm_group_name", "arm_group_name"); + hand_name_ = pnh.param("hand_name", "hand_group_name"); + eef_name_ = pnh.param("eef_name", "eef_name"); + hand_frame_ = pnh.param("hand_frame", "hand_frame_name"); + world_frame_ = pnh.param("world_frame", "world_frame_name"); + + // poses + open_gripper_pose_ = pnh.param("open_gripper_pose", "open_pose"); + close_gripper_pose_ = pnh.param("close_gripper_pose", "close_pose"); + home_pose_ = pnh.param("home_pose", "home_pose"); + + // Object + surface + table_name_ = pnh.param("table_name", "table_name"); + table_height_ = pnh.param("table_height", 0.0); + table_length_ = pnh.param("table_length", 0.0); + table_width_ = pnh.param("table_width", 0.0); + + object_name_ = pnh.param("object_name", "name_of_moved_object"); + object_height_ = pnh.param("object_height", 0.0); + object_radius_ = pnh.param("object_radius", 0.0); + + table_refrence_frame_ = pnh.param("table_refrence_frame", "frame_table_is_in"); + object_refrence_frame_ = pnh.param("object_refrence_frame", "frame_object_lays_upon"); + surface_link_ = pnh.param("surface_link", "name_of_table"); + support_surfaces_ = { surface_link_ }; + + // Pick + approach_object_min_dist_ = pnh.param("approach_object_min_dist", 0.0); + approach_object_max_dist_ = pnh.param("approach_object_max_dist", 0.0); + + // Lift + lift_object_min_dist_ = pnh.param("lift_object_min_dist", 0.0); + lift_object_max_dist_ = pnh.param("lift_object_max_dist", 0.0); + + // Place + place_surface_offset_ = pnh.param("place_surface_offset", 0.0); + place_pos_x_ = pnh.param("place_pos_x", 0.0); + place_pos_y_ = pnh.param("place_pos_y", 0.0); + + // compute hand grasp frame + double rotationy = pnh.param("grasp_rotation_y", 0.0); + double rotationx = pnh.param("grasp_rotation_x", 0.0); + double grasp_offset_x = pnh.param("grasp_offset_x", 0.0); + double grasp_offset_y = pnh.param("grasp_offset_y", 0.0); + double grasp_offset_z = pnh.param("prasp_offset_z", 0.0); + grasp_frame_transform_ = Eigen::AngleAxisd(M_PI * rotationy, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(M_PI * rotationx, Eigen::Vector3d::UnitX()) * + Eigen::Translation3d(grasp_offset_x, grasp_offset_y, grasp_offset_z); } void PickPlaceTask::init() { From a1a13d040468e30308453b5e50f989da32802b0a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 12 Aug 2019 17:51:49 +0200 Subject: [PATCH 14/18] remove (potentially infinite) loop --- demo/src/moveit_task_constructor_demo.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/demo/src/moveit_task_constructor_demo.cpp b/demo/src/moveit_task_constructor_demo.cpp index 2f0ed5cc8..e4e1cefe6 100644 --- a/demo/src/moveit_task_constructor_demo.cpp +++ b/demo/src/moveit_task_constructor_demo.cpp @@ -44,9 +44,8 @@ #include void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) { - bool success = false; - while (!success) - success = psi.applyCollisionObject(object); + if (!psi.applyCollisionObject(object)) + throw std::runtime_error("Failed to spawn object: " + object.id); } moveit_msgs::CollisionObject createTable() { From 08983bc3545d06d5725f032a6354b6fb3c92cc9c Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 10 Sep 2019 18:01:16 +0200 Subject: [PATCH 15/18] Cleanup demo package * Simplify parameters using rosparam_shortcuts * Remove example with picknik_ur5_moveit_config * Remove "hacky" object poses * Remove unused parameters --- demo/CMakeLists.txt | 1 + demo/config/panda_config.yaml | 60 ++++---- .../pick_place_task.h | 96 ++++++------ demo/launch/demo.launch | 2 - demo/package.xml | 12 +- demo/src/moveit_task_constructor_demo.cpp | 85 ++++++----- demo/src/pick_place_task.cpp | 138 ++++++++---------- 7 files changed, 179 insertions(+), 215 deletions(-) diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 12b7e0e97..99c2c0871 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS moveit_task_constructor_core moveit_visual_tools moveit_ros_planning_interface + rosparam_shortcuts ) catkin_package( diff --git a/demo/config/panda_config.yaml b/demo/config/panda_config.yaml index 950538882..3546e39c8 100644 --- a/demo/config/panda_config.yaml +++ b/demo/config/panda_config.yaml @@ -1,52 +1,46 @@ -# planning group names +# Total planning attempts +planning_attempts: 10 + +# Planning group and link names arm_group_name: "panda_arm" eef_name: "hand" -hand_name: "hand" +hand_group_name: "hand" hand_frame: "panda_link8" -number_of_plans: 10 # Poses -open_gripper_pose: "open" -close_gripper_pose: "close" -home_pose: "ready" +hand_open_pose: "open" +hand_close_pose: "close" +arm_home_pose: "ready" -# surface -table_refrence_frame: "world" -object_refrence_frame: "table" -surface_link: "table" +# Scene frames world_frame: "world" +table_reference_frame: "world" +object_reference_frame: "world" +surface_link: "table" -# collision object +# Collision object for picking # CYLINDER object specifications object_name: "object" -object_height: 0.25 -object_radius: 0.02 -object_pos_x: 0.0 -object_pos_y: 0.0 +object_dimensions: [0.25, 0.02] # [height, radius] +object_pose: [0.5, -0.25, 0.0, 0, 0, 0] +# Table model +spawn_table: true table_name: "table" -table_height: 0.1 -table_width: 0.5 -table_length: 0.4 -table_pos_x: 0.5 -table_pos_y: -0.25 - -# gripper grasp frame transform -grasp_offset_x: 0.1 # X -grasp_offset_y: 0.0 # Y -grasp_offset_z: 0.0 # Z -grasp_rotation_x: 0.75 # rotation X axis -grasp_rotation_y: 1.5 # rotation Y axis - -# place pose metrics (x/y position on table_top) -place_pos_x: 0.1 -place_pos_y: 0.1 +table_dimensions: [0.4, 0.5, 0.1] # [length, width, height] +table_pose: [0.5, -0.25, 0, 0, 0, 0] + +# Gripper grasp frame transform [x,y,z,r,p,y] +grasp_frame_transform: [0, 0, 0.1, 1.571, 0.785, 1.571] + +# Place pose [x,y,z,r,p,y] +place_pose: [0.6, -0.15, 0, 0, 0, 0] place_surface_offset: 0.0001 # place offset from table -# valid distance range when approaching an object for picking +# Valid distance range when approaching an object for picking approach_object_min_dist: 0.1 approach_object_max_dist: 0.15 -# valid height range when lifting an object after pick +# Valid height range when lifting an object after pick lift_object_min_dist: 0.01 lift_object_max_dist: 0.1 diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h index 17535a278..e081762e2 100644 --- a/demo/include/moveit_task_constructor_demo/pick_place_task.h +++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h @@ -63,8 +63,7 @@ #include -#ifndef MOVEIT_TASK_CONSTRUCTOR_DEMO_PICK_PLACE_TASK_H -#define MOVEIT_TASK_CONSTRUCTOR_DEMO_PICK_PLACE_TASK_H +# pragma once namespace moveit_task_constructor_demo { using namespace moveit::task_constructor; @@ -72,61 +71,54 @@ using namespace moveit::task_constructor; class PickPlaceTask { public: - PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); - ~PickPlaceTask() = default; + PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); + ~PickPlaceTask() = default; - void loadParameters(); + void loadParameters(); - void init(); + void init(); - bool plan(); + bool plan(); - bool execute(); + bool execute(); private: - ros::NodeHandle nh_; - - std::string task_name_; - moveit::task_constructor::TaskPtr task_; - - // planning group properties - std::string arm_group_name_; - std::string eef_name_; - std::string hand_name_; - std::string hand_frame_; - - // object + surface - std::vector support_surfaces_; - std::string table_refrence_frame_; - std::string object_refrence_frame_; - std::string surface_link_; - std::string object_name_; - std::string table_name_; - std::string world_frame_; - double object_height_; - double object_radius_; - double table_height_; - double table_length_; - double table_width_; - - // pose_names - std::string open_gripper_pose_; - std::string close_gripper_pose_; - std::string home_pose_; - - // execution - actionlib::SimpleActionClient execute_; - - // ros params - double approach_object_min_dist_; - double approach_object_max_dist_; - double lift_object_min_dist_; - double lift_object_max_dist_; - double place_pos_x_; - double place_pos_y_; - double place_surface_offset_; - - Eigen::Isometry3d grasp_frame_transform_; + ros::NodeHandle nh_; + + std::string task_name_; + moveit::task_constructor::TaskPtr task_; + + // planning group properties + std::string arm_group_name_; + std::string eef_name_; + std::string hand_group_name_; + std::string hand_frame_; + + // object + surface + std::vector support_surfaces_; + std::string object_reference_frame_; + std::string surface_link_; + std::string object_name_; + std::string world_frame_; + std::vector object_dimensions_; + + // Predefined pose targets + std::string hand_open_pose_; + std::string hand_close_pose_; + std::string arm_home_pose_; + + // Execution + actionlib::SimpleActionClient execute_; + + // Pick metrics + Eigen::Isometry3d grasp_frame_transform_; + double approach_object_min_dist_; + double approach_object_max_dist_; + double lift_object_min_dist_; + double lift_object_max_dist_; + + // Place metrics + geometry_msgs::Pose place_pose_; + double place_surface_offset_; }; } // moveit_task_constructor_demo -#endif // MOVEIT_TASK_CONSTRUCTOR_DEMO_PICK_PLACE_TASK_H diff --git a/demo/launch/demo.launch b/demo/launch/demo.launch index d52cf9c70..b7150aa49 100644 --- a/demo/launch/demo.launch +++ b/demo/launch/demo.launch @@ -5,7 +5,6 @@ - @@ -25,7 +24,6 @@ - diff --git a/demo/package.xml b/demo/package.xml index e2d66e8d3..9a4dcb736 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -4,8 +4,9 @@ 0.0.1 The moveit_task_constructor_demo package - simon Goldstein - Simon Goldstein + simon Goldstein + Henning Kayser + Henning Kayser @@ -17,10 +18,5 @@ moveit_visual_tools moveit_ros_planning_interface moveit_core - - - - - - + rosparam_shortcuts diff --git a/demo/src/moveit_task_constructor_demo.cpp b/demo/src/moveit_task_constructor_demo.cpp index e4e1cefe6..0db36debf 100644 --- a/demo/src/moveit_task_constructor_demo.cpp +++ b/demo/src/moveit_task_constructor_demo.cpp @@ -42,6 +42,10 @@ #include #include +#include +#include + +constexpr char LOGNAME[] = "moveit_task_constructor_demo"; void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) { if (!psi.applyCollisionObject(object)) @@ -50,74 +54,75 @@ void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject createTable() { ros::NodeHandle pnh("~"); - std::string table_name = pnh.param("table_name", "table_name"); - std::string table_refrence_frame = pnh.param("table_refrence_frame", "frame_table_is_in"); - double height = pnh.param("table_height", 0.0); - double width = pnh.param("table_width", 0.0); - double length = pnh.param("table_length", 0.0); - double position_x = pnh.param("table_pos_x", 0.0); - double position_y = pnh.param("table_pos_y", 0.0); + std::string table_name, table_reference_frame; + std::vector table_dimensions; + geometry_msgs::Pose pose; + std::size_t error = 0; + error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose); + rosparam_shortcuts::shutdownIfError(LOGNAME, error); moveit_msgs::CollisionObject object; - object.id = table_name = table_name; - object.header.frame_id = table_refrence_frame; + object.id = table_name; + object.header.frame_id = table_reference_frame; object.primitives.resize(1); object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; - object.primitives[0].dimensions = { length, width, height }; - object.primitive_poses.resize(1); - object.primitive_poses[0].position.x = position_x; - object.primitive_poses[0].position.y = position_y; - object.primitive_poses[0].position.z = 0.5 * height; - object.primitive_poses[0].orientation.w = 1.0; - return object; + object.primitives[0].dimensions = table_dimensions; + pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world + object.primitive_poses.push_back(pose); + return object; } moveit_msgs::CollisionObject createObject() { ros::NodeHandle pnh("~"); - std::string object_name = pnh.param("object_name", "name_of_moved_object"); - std::string object_refrence_frame = pnh.param("object_refrence_frame", "frame_object_lays_upon"); - double height = pnh.param("object_height", 0.0); - double radius = pnh.param("object_radius", 0.0); - double position_x = pnh.param("object_pos_x", 0.0); - double position_y = pnh.param("object_pos_y", 0.0); - double table_height = pnh.param("table_height", 0.0); - double place_surface_offset = pnh.param("place_surface_offset", 0.0); + std::string object_name, object_reference_frame; + std::vector object_dimensions; + geometry_msgs::Pose pose; + std::size_t error = 0; + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose); + rosparam_shortcuts::shutdownIfError(LOGNAME, error); moveit_msgs::CollisionObject object; - object.id = object_name = object_name; - object.header.frame_id = object_refrence_frame; + object.id = object_name; + object.header.frame_id = object_reference_frame; object.primitives.resize(1); object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; - object.primitives[0].dimensions = { height, radius }; - object.primitive_poses.resize(1); - object.primitive_poses[0].position.x = position_x; - object.primitive_poses[0].position.y = position_y; - object.primitive_poses[0].position.z = 0.5 * (height + table_height) + place_surface_offset; - object.primitive_poses[0].orientation.w = 1.0; + object.primitives[0].dimensions = object_dimensions; + pose.position.z += 0.5 * object_dimensions[0]; + object.primitive_poses.push_back(pose); return object; } int main(int argc, char** argv) { - ROS_INFO("Init moveit_task_constructor_demo"); + ROS_INFO_NAMED(LOGNAME, "Init moveit_task_constructor_demo"); ros::init(argc, argv, "moveit_task_constructor_demo"); ros::NodeHandle nh; ros::AsyncSpinner spinner(1); spinner.start(); // Add table and object to planning scene + ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service moveit::planning_interface::PlanningSceneInterface psi; - spawnObject(psi, createTable()); - spawnObject(psi, createObject()); + ros::NodeHandle pnh("~"); + if (pnh.param("spawn_table", true)) + spawnObject(psi, createTable()); + spawnObject(psi, createObject()); // Construct and run pick/place task moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh); pick_place_task.loadParameters(); pick_place_task.init(); - if (pick_place_task.plan()) { - ROS_INFO("Planning succeded"); - pick_place_task.execute(); - } - ROS_INFO("Planning failed"); + if (pick_place_task.plan()) { + ROS_INFO_NAMED(LOGNAME, "Planning succeded"); + pick_place_task.execute(); + } else { + ROS_INFO_NAMED(LOGNAME, "Planning failed"); + } // Keep introspection alive ros::waitForShutdown(); diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index abbdfab55..22d1fc1ee 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -35,11 +35,12 @@ */ #include +#include namespace moveit_task_constructor_demo { +constexpr char LOGNAME[] = "pick_place_task"; PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh) : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) { - ROS_INFO("waiting for task execution"); } void PickPlaceTask::loadParameters() { @@ -48,60 +49,42 @@ void PickPlaceTask::loadParameters() { * Load Parameters * * * ***************************************************/ - ros::NodeHandle pnh("~"); - - // Planning group properties - arm_group_name_ = pnh.param("arm_group_name", "arm_group_name"); - hand_name_ = pnh.param("hand_name", "hand_group_name"); - eef_name_ = pnh.param("eef_name", "eef_name"); - hand_frame_ = pnh.param("hand_frame", "hand_frame_name"); - world_frame_ = pnh.param("world_frame", "world_frame_name"); - - // poses - open_gripper_pose_ = pnh.param("open_gripper_pose", "open_pose"); - close_gripper_pose_ = pnh.param("close_gripper_pose", "close_pose"); - home_pose_ = pnh.param("home_pose", "home_pose"); - - // Object + surface - table_name_ = pnh.param("table_name", "table_name"); - table_height_ = pnh.param("table_height", 0.0); - table_length_ = pnh.param("table_length", 0.0); - table_width_ = pnh.param("table_width", 0.0); - - object_name_ = pnh.param("object_name", "name_of_moved_object"); - object_height_ = pnh.param("object_height", 0.0); - object_radius_ = pnh.param("object_radius", 0.0); - - table_refrence_frame_ = pnh.param("table_refrence_frame", "frame_table_is_in"); - object_refrence_frame_ = pnh.param("object_refrence_frame", "frame_object_lays_upon"); - surface_link_ = pnh.param("surface_link", "name_of_table"); - support_surfaces_ = { surface_link_ }; - - // Pick - approach_object_min_dist_ = pnh.param("approach_object_min_dist", 0.0); - approach_object_max_dist_ = pnh.param("approach_object_max_dist", 0.0); - - // Lift - lift_object_min_dist_ = pnh.param("lift_object_min_dist", 0.0); - lift_object_max_dist_ = pnh.param("lift_object_max_dist", 0.0); - - // Place - place_surface_offset_ = pnh.param("place_surface_offset", 0.0); - place_pos_x_ = pnh.param("place_pos_x", 0.0); - place_pos_y_ = pnh.param("place_pos_y", 0.0); - - // compute hand grasp frame - double rotationy = pnh.param("grasp_rotation_y", 0.0); - double rotationx = pnh.param("grasp_rotation_x", 0.0); - double grasp_offset_x = pnh.param("grasp_offset_x", 0.0); - double grasp_offset_y = pnh.param("grasp_offset_y", 0.0); - double grasp_offset_z = pnh.param("prasp_offset_z", 0.0); - grasp_frame_transform_ = Eigen::AngleAxisd(M_PI * rotationy, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(M_PI * rotationx, Eigen::Vector3d::UnitX()) * - Eigen::Translation3d(grasp_offset_x, grasp_offset_y, grasp_offset_z); + ROS_INFO_NAMED(LOGNAME, "Loading task parameters"); + ros::NodeHandle pnh("~"); + + // Planning group properties + size_t error = 0; + error += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_); + + // Predefined pose targets + error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_); + + // Target object + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_); + support_surfaces_ = { surface_link_ }; + + // Pick/Place metrics + error += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_); + rosparam_shortcuts::shutdownIfError(LOGNAME, error); } void PickPlaceTask::init() { + ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); const std::string object = "object"; // Reset ROS introspection before constructing the new object @@ -124,7 +107,7 @@ void PickPlaceTask::init() { // Set task properties t.setProperty("group", arm_group_name_); t.setProperty("eef", eef_name_); - t.setProperty("hand", hand_name_); + t.setProperty("hand", hand_group_name_); t.setProperty("hand_grasping_frame", hand_frame_); t.setProperty("ik_frame", hand_frame_); @@ -159,8 +142,8 @@ void PickPlaceTask::init() { ***************************************************/ { // Open Hand auto stage = std::make_unique("open hand", sampling_planner); - stage->setGroup(hand_name_); - stage->setGoal(open_gripper_pose_); + stage->setGroup(hand_group_name_); + stage->setGoal(hand_open_pose_); t.add(std::move(stage)); } @@ -214,7 +197,7 @@ void PickPlaceTask::init() { auto stage = std::make_unique("generate grasp pose"); stage->properties().configureInitFrom(Stage::PARENT); stage->properties().set("marker_ns", "grasp_pose"); - stage->setPreGraspPose(open_gripper_pose_); + stage->setPreGraspPose(hand_open_pose_); stage->setObject(object); stage->setAngleDelta(M_PI / 12); stage->setMonitoredStage(current_state); // Hook into current state @@ -224,7 +207,7 @@ void PickPlaceTask::init() { wrapper->setMaxIKSolutions(8); wrapper->setMinSolutionDistance(1.0); wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); - wrapper->properties().configureInitFrom(Stage::PARENT, { "eef" }); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); grasp->insert(std::move(wrapper)); } @@ -235,7 +218,7 @@ void PickPlaceTask::init() { { auto stage = std::make_unique("allow collision (hand,object)"); stage->allowCollisions( - object, t.getRobotModel()->getJointModelGroup(hand_name_)->getLinkModelNamesWithCollisionGeometry(), true); + object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), true); grasp->insert(std::move(stage)); } @@ -244,8 +227,8 @@ void PickPlaceTask::init() { ***************************************************/ { auto stage = std::make_unique("close hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_name_); - stage->setGoal(close_gripper_pose_); + stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); + stage->setGoal(hand_close_pose_); grasp->insert(std::move(stage)); } @@ -352,11 +335,9 @@ void PickPlaceTask::init() { // Set target pose geometry_msgs::PoseStamped p; - p.header.frame_id = object_refrence_frame_; - p.pose.orientation.w = 1; - p.pose.position.x = place_pos_x_; - p.pose.position.y = place_pos_y_; - p.pose.position.z = 0.5 * (object_height_ + table_height_) + place_surface_offset_; + p.header.frame_id = object_reference_frame_; + p.pose = place_pose_; + p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; stage->setPose(p); stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage @@ -374,8 +355,8 @@ void PickPlaceTask::init() { *****************************************************/ { auto stage = std::make_unique("open hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_name_); - stage->setGoal(open_gripper_pose_); + stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); + stage->setGoal(hand_open_pose_); place->insert(std::move(stage)); } @@ -385,7 +366,7 @@ void PickPlaceTask::init() { { auto stage = std::make_unique("forbid collision (hand,object)"); stage->allowCollisions( - object_name_, t.getRobotModel()->getJointModelGroup(hand_name_)->getLinkModelNamesWithCollisionGeometry(), + object_name_, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), false); place->insert(std::move(stage)); } @@ -427,43 +408,40 @@ void PickPlaceTask::init() { { auto stage = std::make_unique("move home", sampling_planner); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setGoal(home_pose_); + stage->setGoal(arm_home_pose_); stage->restrictDirection(stages::MoveTo::FORWARD); t.add(std::move(stage)); } } bool PickPlaceTask::plan() { + ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); ros::NodeHandle pnh("~"); - int number_of_plans = pnh.param("number_of_plans", 10); + int planning_attempts = pnh.param("planning_attempts", 10); try { - task_->plan(number_of_plans); + task_->plan(planning_attempts); } catch (InitStageException& e) { - ROS_ERROR_STREAM("Initialization failed: " << e); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e); return false; } if (task_->numSolutions() == 0) { - ROS_ERROR("Planning failed"); + ROS_ERROR_NAMED(LOGNAME, "Planning failed"); return false; } return true; } bool PickPlaceTask::execute() { - moveit_task_constructor_msgs::Solution solution; - task_->solutions().front()->fillMessage(solution); - - ROS_INFO_STREAM("last trajectory in solution:\n" << solution.sub_trajectory.back().trajectory); - + ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory"); moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; - execute_goal.solution = solution; + task_->solutions().front()->fillMessage(execute_goal.solution); execute_.sendGoal(execute_goal); execute_.waitForResult(); moveit_msgs::MoveItErrorCodes execute_result = execute_.getResult()->error_code; if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { - ROS_ERROR_STREAM("task execution failed and returned: " << execute_.getState().toString()); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_.getState().toString()); return false; } From ac294d54a61aca7c61e7d44bc5c839795c8e7d12 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 10 Sep 2019 18:13:42 +0200 Subject: [PATCH 16/18] Clang-format demo package --- .../pick_place_task.h | 88 +++++++++---------- demo/src/moveit_task_constructor_demo.cpp | 60 ++++++------- demo/src/pick_place_task.cpp | 84 +++++++++--------- 3 files changed, 116 insertions(+), 116 deletions(-) diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h index e081762e2..56aee8f80 100644 --- a/demo/include/moveit_task_constructor_demo/pick_place_task.h +++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h @@ -63,7 +63,7 @@ #include -# pragma once +#pragma once namespace moveit_task_constructor_demo { using namespace moveit::task_constructor; @@ -71,54 +71,54 @@ using namespace moveit::task_constructor; class PickPlaceTask { public: - PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); - ~PickPlaceTask() = default; + PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); + ~PickPlaceTask() = default; - void loadParameters(); + void loadParameters(); - void init(); + void init(); - bool plan(); + bool plan(); - bool execute(); + bool execute(); private: - ros::NodeHandle nh_; - - std::string task_name_; - moveit::task_constructor::TaskPtr task_; - - // planning group properties - std::string arm_group_name_; - std::string eef_name_; - std::string hand_group_name_; - std::string hand_frame_; - - // object + surface - std::vector support_surfaces_; - std::string object_reference_frame_; - std::string surface_link_; - std::string object_name_; - std::string world_frame_; - std::vector object_dimensions_; - - // Predefined pose targets - std::string hand_open_pose_; - std::string hand_close_pose_; - std::string arm_home_pose_; - - // Execution - actionlib::SimpleActionClient execute_; - - // Pick metrics - Eigen::Isometry3d grasp_frame_transform_; - double approach_object_min_dist_; - double approach_object_max_dist_; - double lift_object_min_dist_; - double lift_object_max_dist_; - - // Place metrics - geometry_msgs::Pose place_pose_; - double place_surface_offset_; + ros::NodeHandle nh_; + + std::string task_name_; + moveit::task_constructor::TaskPtr task_; + + // planning group properties + std::string arm_group_name_; + std::string eef_name_; + std::string hand_group_name_; + std::string hand_frame_; + + // object + surface + std::vector support_surfaces_; + std::string object_reference_frame_; + std::string surface_link_; + std::string object_name_; + std::string world_frame_; + std::vector object_dimensions_; + + // Predefined pose targets + std::string hand_open_pose_; + std::string hand_close_pose_; + std::string arm_home_pose_; + + // Execution + actionlib::SimpleActionClient execute_; + + // Pick metrics + Eigen::Isometry3d grasp_frame_transform_; + double approach_object_min_dist_; + double approach_object_max_dist_; + double lift_object_min_dist_; + double lift_object_max_dist_; + + // Place metrics + geometry_msgs::Pose place_pose_; + double place_surface_offset_; }; } // moveit_task_constructor_demo diff --git a/demo/src/moveit_task_constructor_demo.cpp b/demo/src/moveit_task_constructor_demo.cpp index 0db36debf..49d2422fd 100644 --- a/demo/src/moveit_task_constructor_demo.cpp +++ b/demo/src/moveit_task_constructor_demo.cpp @@ -54,15 +54,15 @@ void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject createTable() { ros::NodeHandle pnh("~"); - std::string table_name, table_reference_frame; - std::vector table_dimensions; - geometry_msgs::Pose pose; - std::size_t error = 0; - error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name); + std::string table_name, table_reference_frame; + std::vector table_dimensions; + geometry_msgs::Pose pose; + std::size_t error = 0; + error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name); error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose); - rosparam_shortcuts::shutdownIfError(LOGNAME, error); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose); + rosparam_shortcuts::shutdownIfError(LOGNAME, error); moveit_msgs::CollisionObject object; object.id = table_name; @@ -70,22 +70,22 @@ moveit_msgs::CollisionObject createTable() { object.primitives.resize(1); object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; object.primitives[0].dimensions = table_dimensions; - pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world - object.primitive_poses.push_back(pose); - return object; + pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world + object.primitive_poses.push_back(pose); + return object; } moveit_msgs::CollisionObject createObject() { ros::NodeHandle pnh("~"); - std::string object_name, object_reference_frame; - std::vector object_dimensions; - geometry_msgs::Pose pose; - std::size_t error = 0; + std::string object_name, object_reference_frame; + std::vector object_dimensions; + geometry_msgs::Pose pose; + std::size_t error = 0; error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name); error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame); error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose); - rosparam_shortcuts::shutdownIfError(LOGNAME, error); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose); + rosparam_shortcuts::shutdownIfError(LOGNAME, error); moveit_msgs::CollisionObject object; object.id = object_name; @@ -93,8 +93,8 @@ moveit_msgs::CollisionObject createObject() { object.primitives.resize(1); object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; object.primitives[0].dimensions = object_dimensions; - pose.position.z += 0.5 * object_dimensions[0]; - object.primitive_poses.push_back(pose); + pose.position.z += 0.5 * object_dimensions[0]; + object.primitive_poses.push_back(pose); return object; } @@ -106,23 +106,23 @@ int main(int argc, char** argv) { spinner.start(); // Add table and object to planning scene - ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service + ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service moveit::planning_interface::PlanningSceneInterface psi; - ros::NodeHandle pnh("~"); - if (pnh.param("spawn_table", true)) - spawnObject(psi, createTable()); - spawnObject(psi, createObject()); + ros::NodeHandle pnh("~"); + if (pnh.param("spawn_table", true)) + spawnObject(psi, createTable()); + spawnObject(psi, createObject()); // Construct and run pick/place task moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh); pick_place_task.loadParameters(); pick_place_task.init(); - if (pick_place_task.plan()) { - ROS_INFO_NAMED(LOGNAME, "Planning succeded"); - pick_place_task.execute(); - } else { - ROS_INFO_NAMED(LOGNAME, "Planning failed"); - } + if (pick_place_task.plan()) { + ROS_INFO_NAMED(LOGNAME, "Planning succeded"); + pick_place_task.execute(); + } else { + ROS_INFO_NAMED(LOGNAME, "Planning failed"); + } // Keep introspection alive ros::waitForShutdown(); diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 22d1fc1ee..88d66f06d 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -40,8 +40,7 @@ namespace moveit_task_constructor_demo { constexpr char LOGNAME[] = "pick_place_task"; PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh) - : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) { -} + : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) {} void PickPlaceTask::loadParameters() { /**************************************************** @@ -49,42 +48,42 @@ void PickPlaceTask::loadParameters() { * Load Parameters * * * ***************************************************/ - ROS_INFO_NAMED(LOGNAME, "Loading task parameters"); - ros::NodeHandle pnh("~"); - - // Planning group properties - size_t error = 0; - error += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_); - - // Predefined pose targets - error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_); - - // Target object - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_); - support_surfaces_ = { surface_link_ }; - - // Pick/Place metrics - error += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_); - rosparam_shortcuts::shutdownIfError(LOGNAME, error); + ROS_INFO_NAMED(LOGNAME, "Loading task parameters"); + ros::NodeHandle pnh("~"); + + // Planning group properties + size_t error = 0; + error += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_); + + // Predefined pose targets + error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_); + + // Target object + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_); + support_surfaces_ = { surface_link_ }; + + // Pick/Place metrics + error += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_); + rosparam_shortcuts::shutdownIfError(LOGNAME, error); } void PickPlaceTask::init() { - ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); + ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); const std::string object = "object"; // Reset ROS introspection before constructing the new object @@ -218,7 +217,8 @@ void PickPlaceTask::init() { { auto stage = std::make_unique("allow collision (hand,object)"); stage->allowCollisions( - object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), true); + object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), + true); grasp->insert(std::move(stage)); } @@ -336,8 +336,8 @@ void PickPlaceTask::init() { // Set target pose geometry_msgs::PoseStamped p; p.header.frame_id = object_reference_frame_; - p.pose = place_pose_; - p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; + p.pose = place_pose_; + p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; stage->setPose(p); stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage @@ -366,8 +366,8 @@ void PickPlaceTask::init() { { auto stage = std::make_unique("forbid collision (hand,object)"); stage->allowCollisions( - object_name_, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), - false); + object_name_, + t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), false); place->insert(std::move(stage)); } @@ -415,7 +415,7 @@ void PickPlaceTask::init() { } bool PickPlaceTask::plan() { - ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); + ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); ros::NodeHandle pnh("~"); int planning_attempts = pnh.param("planning_attempts", 10); @@ -433,7 +433,7 @@ bool PickPlaceTask::plan() { } bool PickPlaceTask::execute() { - ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory"); + ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory"); moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; task_->solutions().front()->fillMessage(execute_goal.solution); execute_.sendGoal(execute_goal); From d9626a7d611d4cf2bd53c4beeca28103e5cf4ccd Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 18 Sep 2019 10:00:29 +0200 Subject: [PATCH 17/18] Add rosparam_shortcuts to .rosinstall --- .rosinstall | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.rosinstall b/.rosinstall index 830e279d6..d279436d3 100644 --- a/.rosinstall +++ b/.rosinstall @@ -1,4 +1,4 @@ -# This file is intended for users who want to build MoveIt! from source. +# This file is intended for users who want to build MoveIt from source. # Used with wstool, users can download source of all packages of MoveIt!. - git: local-name: moveit_task_constructor @@ -12,3 +12,7 @@ local-name: mtc_pour uri: https://github.com/TAMS-Group/mtc_pour.git version: master +- git: + local-name: rosparam-shortcuts + uri: https://github.com/PickNikRobotics/rosparam_shortcuts.git + version: melodic-devel From 2bd282697580c60b11234693beb2542017185652 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 7 Oct 2019 22:22:17 +0200 Subject: [PATCH 18/18] Cleanup tutorial demo (#3) - fix dependencies - moveit_visual_tools is not actually used - panda_moveit_config is required in a recent version - use more specific lib/binary names --- .rosinstall | 8 +--- demo/CMakeLists.txt | 28 +++++-------- demo/config/mtc.rviz | 48 +++++++++++------------ demo/launch/demo.launch | 2 +- demo/package.xml | 5 +-- demo/src/moveit_task_constructor_demo.cpp | 12 +++--- demo/src/pick_place_task.cpp | 42 ++++++++++---------- 7 files changed, 65 insertions(+), 80 deletions(-) diff --git a/.rosinstall b/.rosinstall index d279436d3..abfb40d37 100644 --- a/.rosinstall +++ b/.rosinstall @@ -1,5 +1,5 @@ -# This file is intended for users who want to build MoveIt from source. -# Used with wstool, users can download source of all packages of MoveIt!. +# This file is intended for users who want to build MTC and its demos from source. +# Using wstool, users can download relevant MTC-related source packages. - git: local-name: moveit_task_constructor uri: https://github.com/ros-planning/moveit_task_constructor.git @@ -12,7 +12,3 @@ local-name: mtc_pour uri: https://github.com/TAMS-Group/mtc_pour.git version: master -- git: - local-name: rosparam-shortcuts - uri: https://github.com/PickNikRobotics/rosparam_shortcuts.git - version: melodic-devel diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 99c2c0871..a32d2d8af 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -7,7 +7,6 @@ find_package(catkin REQUIRED COMPONENTS roscpp moveit_core moveit_task_constructor_core - moveit_visual_tools moveit_ros_planning_interface rosparam_shortcuts ) @@ -20,24 +19,19 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} - src/pick_place_task.cpp -) - -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -add_executable(${PROJECT_NAME}_moveit_task_constructor_demo src/moveit_task_constructor_demo.cpp) - -set_target_properties(${PROJECT_NAME}_moveit_task_constructor_demo PROPERTIES OUTPUT_NAME moveit_task_constructor_demo PREFIX "") - -add_dependencies(${PROJECT_NAME}_moveit_task_constructor_demo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +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}) -target_link_libraries(${PROJECT_NAME}_moveit_task_constructor_demo - ${PROJECT_NAME} +add_executable(${PROJECT_NAME}_demo src/moveit_task_constructor_demo.cpp) +target_link_libraries(${PROJECT_NAME}_demo + ${PROJECT_NAME}_lib ${catkin_LIBRARIES} ) +set_target_properties(${PROJECT_NAME}_demo PROPERTIES OUTPUT_NAME moveit_task_constructor_demo) +add_dependencies(${PROJECT_NAME}_demo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_moveit_task_constructor_demo +install(TARGETS ${PROJECT_NAME}_lib ${PROJECT_NAME}_demo ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -46,10 +40,8 @@ install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_moveit_task_constructor_demo install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE ) -install(FILES - launch/demo.launch +install(DIRECTORY launch config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/demo/config/mtc.rviz b/demo/config/mtc.rviz index e3a9fc5ab..83505a9b4 100644 --- a/demo/config/mtc.rviz +++ b/demo/config/mtc.rviz @@ -5,8 +5,9 @@ Panels: Property Tree Widget: Expanded: - /Motion Planning Tasks1 - Splitter Ratio: 0.7425600290298462 - Tree Height: 270 + - /Motion Planning Tasks1/Markers1 + Splitter Ratio: 0.5393258333206177 + Tree Height: 533 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -14,8 +15,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz_visual_tools/RvizVisualToolsGui - Name: RvizVisualToolsGui - Class: moveit_task_constructor/Motion Planning Tasks Global Settings: Task View Settings: @@ -23,17 +22,20 @@ Panels: Name: Motion Planning Tasks Tasks View: property_splitter: - - 87 - - 87 + - 541 + - 0 solution_sorting: column: 0 order: 1 solutions_splitter: - - 302 - - 70 - solutions_view_columns: ~ + - 328 + - 76 + solutions_view_columns: + - 38 + - 0 + - 0 tasks_view_columns: - - 153 + - 250 - 38 - 38 Preferences: @@ -68,6 +70,7 @@ Visualization Manager: Markers: All at once?: false Value: true + approach_object: true Name: Motion Planning Tasks Robot: Fixed Robot Color: 150; 50; 150 @@ -153,7 +156,7 @@ Visualization Manager: State Display Time: 0.05 s Task Solution Topic: /moveit_task_constructor_demo/pick_place_task/solution Tasks: - {} + pick_place_task: 10 Trail Step Size: 1 Value: true Enabled: true @@ -168,35 +171,34 @@ Visualization Manager: Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - - Class: rviz_visual_tools/KeyTool Value: true Views: Current: Class: rviz/XYOrbit - Distance: 2.827594041824341 + Distance: 1.9269260168075562 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.11356700211763382 - Y: 0.10592000186443329 + X: 0.8292451500892639 + Y: -0.2452867180109024 Z: 2.2351800055275817e-7 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.47020357847213745 + Pitch: 0.3197975158691406 Target Frame: panda_link0 Value: XYOrbit (rviz) - Yaw: 6.06496000289917 + Yaw: 6.239960670471191 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 741 + Height: 768 Help: collapsed: false Hide Left Dock: false @@ -205,11 +207,9 @@ Window Geometry: collapsed: false Motion Planning Tasks - Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001780000028bfc020000000ffb000000100044006900730070006c006100790073010000003d0000019f000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069000000027d0000004b0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000120020002d00200053006c00690064006500720000000000ffffffff0000004100fffffffb0000002a004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b007301000001e2000000e6000000e600ffffff000003d80000028b00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RvizVisualToolsGui: - collapsed: false + QMainWindow State: 000000ff00000000fd000000020000000000000166000002a6fc020000000efb000000100044006900730070006c006100790073010000003d000002a6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069000000027d0000004b0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000120020002d00200053006c00690064006500720000000000ffffffff00000000000000000000000100000198000002a6fc0200000002fb0000002a004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b0073010000003d00000254000000e500fffffffb0000003c004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b00730020002d00200053006c006900640065007201000002970000004c0000004100ffffff0000028f000002a600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1366 - X: 544 - Y: 315 + Width: 1433 + X: 472 + Y: 25 diff --git a/demo/launch/demo.launch b/demo/launch/demo.launch index b7150aa49..d6de926c0 100644 --- a/demo/launch/demo.launch +++ b/demo/launch/demo.launch @@ -18,7 +18,7 @@ - + diff --git a/demo/package.xml b/demo/package.xml index 9a4dcb736..27ef23921 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -8,15 +8,12 @@ Henning Kayser Henning Kayser - - - BSD catkin moveit_task_constructor_core - moveit_visual_tools moveit_ros_planning_interface moveit_core rosparam_shortcuts + panda_moveit_config diff --git a/demo/src/moveit_task_constructor_demo.cpp b/demo/src/moveit_task_constructor_demo.cpp index 49d2422fd..89442b13d 100644 --- a/demo/src/moveit_task_constructor_demo.cpp +++ b/demo/src/moveit_task_constructor_demo.cpp @@ -57,12 +57,12 @@ moveit_msgs::CollisionObject createTable() { std::string table_name, table_reference_frame; std::vector table_dimensions; geometry_msgs::Pose pose; - std::size_t error = 0; - error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose); - rosparam_shortcuts::shutdownIfError(LOGNAME, error); + std::size_t errors = 0; + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose); + rosparam_shortcuts::shutdownIfError(LOGNAME, errors); moveit_msgs::CollisionObject object; object.id = table_name; diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 88d66f06d..6d9452447 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -52,34 +52,34 @@ void PickPlaceTask::loadParameters() { ros::NodeHandle pnh("~"); // Planning group properties - size_t error = 0; - error += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_); + size_t errors = 0; + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_); // Predefined pose targets - error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_); // Target object - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_); support_surfaces_ = { surface_link_ }; // Pick/Place metrics - error += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_); - rosparam_shortcuts::shutdownIfError(LOGNAME, error); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_); + rosparam_shortcuts::shutdownIfError(LOGNAME, errors); } void PickPlaceTask::init() {