diff --git a/.catkin_tools/VERSION b/.catkin_tools/VERSION index 7deb86fee42803fdce673cb574b8186909c17806..899f24fc754a14b89617093824d258aaa3aa2943 100644 --- a/.catkin_tools/VERSION +++ b/.catkin_tools/VERSION @@ -1 +1 @@ -0.7.1 \ No newline at end of file +0.9.0 \ No newline at end of file diff --git a/.catkin_tools/profiles/default/config.yaml b/.catkin_tools/profiles/default/config.yaml new file mode 100644 index 0000000000000000000000000000000000000000..0967ef424bce6791893e9a57bb952f80fd536e93 --- /dev/null +++ b/.catkin_tools/profiles/default/config.yaml @@ -0,0 +1 @@ +{} diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index 11f581c86b4fd6b6f1fc110e7a6e41d3a9116def..91f6f8aa6bff048bfe96c8122f6d32ba43bbfadd 100644 --- a/.catkin_tools/profiles/default/devel_collisions.txt +++ b/.catkin_tools/profiles/default/devel_collisions.txt @@ -1,2 +1,4 @@ /home/matteo/ws_panda/devel/./cmake.lock 42 -/home/matteo/reachability/devel/./cmake.lock 6278 +/home/matteo/reachability/devel/./cmake.lock 7314 +/home/matteo/reachability/devel/lib/libmoveit_grasps.so 66 +/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 66 diff --git a/.catkin_tools/profiles/default/packages/moveit_grasps/devel_manifest.txt b/.catkin_tools/profiles/default/packages/moveit_grasps/devel_manifest.txt new file mode 100644 index 0000000000000000000000000000000000000000..34be7681767050b3986660d3a998b4f2bb577d2b --- /dev/null +++ b/.catkin_tools/profiles/default/packages/moveit_grasps/devel_manifest.txt @@ -0,0 +1,12 @@ +moveit_grasps +/home/matteo/reachability/devel/.private/moveit_grasps/cmake.lock /home/matteo/reachability/devel/./cmake.lock +/home/matteo/reachability/devel/.private/moveit_grasps/share/moveit_grasps/cmake/moveit_graspsConfig.cmake /home/matteo/reachability/devel/share/moveit_grasps/cmake/moveit_graspsConfig.cmake +/home/matteo/reachability/devel/.private/moveit_grasps/share/moveit_grasps/cmake/moveit_graspsConfig-version.cmake /home/matteo/reachability/devel/share/moveit_grasps/cmake/moveit_graspsConfig-version.cmake +/home/matteo/reachability/devel/.private/moveit_grasps/lib/libmoveit_grasps.so /home/matteo/reachability/devel/lib/libmoveit_grasps.so +/home/matteo/reachability/devel/.private/moveit_grasps/lib/libmoveit_grasps_filter.so /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so +/home/matteo/reachability/devel/.private/moveit_grasps/lib/moveit_grasps/moveit_grasps_grasp_pipeline_demo /home/matteo/reachability/devel/lib/moveit_grasps/moveit_grasps_grasp_pipeline_demo +/home/matteo/reachability/devel/.private/moveit_grasps/lib/moveit_grasps/moveit_grasps_grasp_filter_demo /home/matteo/reachability/devel/lib/moveit_grasps/moveit_grasps_grasp_filter_demo +/home/matteo/reachability/devel/.private/moveit_grasps/lib/moveit_grasps/moveit_grasps_grasp_poses_visualizer_demo /home/matteo/reachability/devel/lib/moveit_grasps/moveit_grasps_grasp_poses_visualizer_demo +/home/matteo/reachability/devel/.private/moveit_grasps/lib/moveit_grasps/moveit_grasps_grasp_generator_demo /home/matteo/reachability/devel/lib/moveit_grasps/moveit_grasps_grasp_generator_demo +/home/matteo/reachability/devel/.private/moveit_grasps/lib/moveit_grasps/moveit_grasps_suction_grasp_pipeline_demo /home/matteo/reachability/devel/lib/moveit_grasps/moveit_grasps_suction_grasp_pipeline_demo +/home/matteo/reachability/devel/.private/moveit_grasps/lib/pkgconfig/moveit_grasps.pc /home/matteo/reachability/devel/lib/pkgconfig/moveit_grasps.pc diff --git a/.catkin_tools/profiles/default/packages/moveit_grasps/package.xml b/.catkin_tools/profiles/default/packages/moveit_grasps/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..2569b4a68c3450fb9b5276dec357aa71816699f3 --- /dev/null +++ b/.catkin_tools/profiles/default/packages/moveit_grasps/package.xml @@ -0,0 +1,50 @@ +<package> + <name>moveit_grasps</name> + <version>2.0.0</version> + <description> + A basic grasp generator for objects such as blocks or cylinders for use with the MoveIt! pick and place pipeline. + Does not consider friction cones or other dynamics. + </description> + <maintainer email="davetcoleman@gmail.com">Dave Coleman</maintainer> + <maintainer email="mcevoy.andy@gmail.com">Andy McEvoy</maintainer> + + <license>BSD</license> + + <url type="website">https://github.com/PickNikRobotics/moveit_grasps/</url> + <url type="bugtracker">https://github.com/PickNikRobotics/moveit_grasps/issues</url> + <url type="repository">https://github.com/PickNikRobotics/moveit_grasps</url> + + <author email="davetcoleman@gmail.com">Dave Coleman</author> + <author email="mcevoy.andy@gmail.com">Andy McEvoy</author> + + <buildtool_depend>catkin</buildtool_depend> + + <build_depend>roscpp</build_depend> + <build_depend>roslint</build_depend> + <build_depend>eigen_conversions</build_depend> + <build_depend>std_msgs</build_depend> + <build_depend>trajectory_msgs</build_depend> + <build_depend>tf</build_depend> + <build_depend>tf_conversions</build_depend> + <build_depend>moveit_ros_planning</build_depend> + <build_depend>moveit_ros_planning_interface</build_depend> + <build_depend>moveit_core</build_depend> + <build_depend>moveit_visual_tools</build_depend> + <build_depend>rosparam_shortcuts</build_depend> + <build_depend>message_generation</build_depend> + <build_depend>moveit_msgs</build_depend> + <build_depend>geometry_msgs</build_depend> + + <run_depend>std_msgs</run_depend> + <run_depend>trajectory_msgs</run_depend> + <run_depend>moveit_msgs</run_depend> + <run_depend>geometry_msgs</run_depend> + <run_depend>message_runtime</run_depend> + <run_depend>moveit_visual_tools</run_depend> + <run_depend>rosparam_shortcuts</run_depend> + + <test_depend>rostest</test_depend> + <test_depend>rosunit</test_depend> + <test_depend>panda_moveit_config</test_depend> + +</package> diff --git a/.catkin_tools/profiles/default/packages/mtc/devel_manifest.txt b/.catkin_tools/profiles/default/packages/mtc/devel_manifest.txt new file mode 100644 index 0000000000000000000000000000000000000000..79543793de7e2029e1c65c2c5b9f5ac83e981eb6 --- /dev/null +++ b/.catkin_tools/profiles/default/packages/mtc/devel_manifest.txt @@ -0,0 +1,14 @@ +mtc +/home/matteo/reachability/devel/.private/mtc/cmake.lock /home/matteo/reachability/devel/./cmake.lock +/home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtc-msg-paths.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtc-msg-paths.cmake +/home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtcConfig-version.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtcConfig-version.cmake +/home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtcConfig.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtcConfig.cmake +/home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtc-msg-extras.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtc-msg-extras.cmake +/home/matteo/reachability/devel/.private/mtc/share/roseus/ros/mtc/manifest.l /home/matteo/reachability/devel/share/roseus/ros/mtc/manifest.l +/home/matteo/reachability/devel/.private/mtc/lib/libmoveit_grasps.so /home/matteo/reachability/devel/lib/libmoveit_grasps.so +/home/matteo/reachability/devel/.private/mtc/lib/libmoveit_grasps_filter.so /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so +/home/matteo/reachability/devel/.private/mtc/lib/mtc/base_routine /home/matteo/reachability/devel/lib/mtc/base_routine +/home/matteo/reachability/devel/.private/mtc/lib/mtc/moveit_grasps_grasp_pipeline /home/matteo/reachability/devel/lib/mtc/moveit_grasps_grasp_pipeline +/home/matteo/reachability/devel/.private/mtc/lib/mtc/mtc2taskspace /home/matteo/reachability/devel/lib/mtc/mtc2taskspace +/home/matteo/reachability/devel/.private/mtc/lib/python3/dist-packages/mtc/__init__.py /home/matteo/reachability/devel/lib/python3/dist-packages/mtc/__init__.py +/home/matteo/reachability/devel/.private/mtc/lib/pkgconfig/mtc.pc /home/matteo/reachability/devel/lib/pkgconfig/mtc.pc diff --git a/.catkin_tools/profiles/default/packages/mtc/package.xml b/.catkin_tools/profiles/default/packages/mtc/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..1213557c52d3af5afd9af5b3363c849eb40c63af --- /dev/null +++ b/.catkin_tools/profiles/default/packages/mtc/package.xml @@ -0,0 +1,54 @@ +<package> + <name>mtc</name> + <version>2.0.0</version> + <description> + A basic grasp generator for objects such as blocks or cylinders for use with the MoveIt! pick and place pipeline. + Does not consider friction cones or other dynamics. + </description> + <maintainer email="davetcoleman@gmail.com">Dave Coleman</maintainer> + <maintainer email="mcevoy.andy@gmail.com">Andy McEvoy</maintainer> + + <license>BSD</license> + + <url type="website">https://github.com/PickNikRobotics/moveit_grasps/</url> + <url type="bugtracker">https://github.com/PickNikRobotics/moveit_grasps/issues</url> + <url type="repository">https://github.com/PickNikRobotics/moveit_grasps</url> + + <author email="davetcoleman@gmail.com">Dave Coleman</author> + <author email="mcevoy.andy@gmail.com">Andy McEvoy</author> + + <buildtool_depend>catkin</buildtool_depend> + + <build_depend>roscpp</build_depend> + <build_depend>roslint</build_depend> + <build_depend>eigen_conversions</build_depend> + <build_depend>std_msgs</build_depend> + <build_depend>trajectory_msgs</build_depend> + <build_depend>tf</build_depend> + <build_depend>tf_conversions</build_depend> + <build_depend>moveit_ros_planning</build_depend> + <build_depend>moveit_ros_planning_interface</build_depend> + <build_depend>moveit_core</build_depend> + <build_depend>moveit_visual_tools</build_depend> + <build_depend>rosparam_shortcuts</build_depend> + <build_depend>message_generation</build_depend> + <build_depend>moveit_msgs</build_depend> + <build_depend>geometry_msgs</build_depend> + <build_depend>YAML_CPP</build_depend> + + <run_depend>YAML_CPP</run_depend> + + <run_depend>std_msgs</run_depend> + <run_depend>trajectory_msgs</run_depend> + <run_depend>moveit_msgs</run_depend> + <run_depend>geometry_msgs</run_depend> + <run_depend>message_runtime</run_depend> + <run_depend>moveit_visual_tools</run_depend> + <run_depend>rosparam_shortcuts</run_depend> + + <!-- The export tag contains other, unspecified, tags --> + <export> + <!-- Other tools can request additional information be placed here --> + + </export> +</package> diff --git a/.catkin_tools/profiles/default/packages/panda_moveit_config2/devel_manifest.txt b/.catkin_tools/profiles/default/packages/panda_moveit_config2/devel_manifest.txt new file mode 100644 index 0000000000000000000000000000000000000000..6142ce29e188e7f9009a999cd3a4eb59f02ce93a --- /dev/null +++ b/.catkin_tools/profiles/default/packages/panda_moveit_config2/devel_manifest.txt @@ -0,0 +1,5 @@ +panda_moveit_config2 +/home/matteo/reachability/devel/.private/panda_moveit_config2/cmake.lock /home/matteo/reachability/devel/./cmake.lock +/home/matteo/reachability/devel/.private/panda_moveit_config2/share/panda_moveit_config2/cmake/panda_moveit_config2Config.cmake /home/matteo/reachability/devel/share/panda_moveit_config2/cmake/panda_moveit_config2Config.cmake +/home/matteo/reachability/devel/.private/panda_moveit_config2/share/panda_moveit_config2/cmake/panda_moveit_config2Config-version.cmake /home/matteo/reachability/devel/share/panda_moveit_config2/cmake/panda_moveit_config2Config-version.cmake +/home/matteo/reachability/devel/.private/panda_moveit_config2/lib/pkgconfig/panda_moveit_config2.pc /home/matteo/reachability/devel/lib/pkgconfig/panda_moveit_config2.pc diff --git a/.catkin_tools/profiles/default/packages/panda_moveit_config2/package.xml b/.catkin_tools/profiles/default/packages/panda_moveit_config2/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..ad33dee20a6f0b7c72101dfc259cc343b08f1924 --- /dev/null +++ b/.catkin_tools/profiles/default/packages/panda_moveit_config2/package.xml @@ -0,0 +1,32 @@ +<package> + <name>panda_moveit_config2</name> + <version>0.7.3</version> + <description> + An automatically generated package with all the configuration and launch files for using the panda with the MoveIt! Motion Planning Framework + </description> + <author email="mike@picknik.ai">Mike Lautman</author> + <maintainer email="mike@picknik.ai">Mike Lautman</maintainer> + + <license>BSD</license> + + <url type="website">http://moveit.ros.org/</url> + <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url> + <url type="repository">https://github.com/ros-planning/moveit</url> + + <buildtool_depend>catkin</buildtool_depend> + + <run_depend>franka_description</run_depend> + <run_depend>moveit_ros_move_group</run_depend> + <run_depend>moveit_fake_controller_manager</run_depend> + <run_depend>moveit_kinematics</run_depend> + <run_depend>moveit_planners_ompl</run_depend> + <run_depend>moveit_ros_visualization</run_depend> + <run_depend>joint_state_publisher</run_depend> + <run_depend>joint_state_publisher_gui</run_depend> + <run_depend>robot_state_publisher</run_depend> + <run_depend>xacro</run_depend> + <run_depend>topic_tools</run_depend> + <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. + Commented the dependency until this works. --> + <!-- <run_depend>warehouse_ros_mongo</run_depend> --> +</package> diff --git a/.catkin_tools/profiles/default/packages/reachability/devel_manifest.txt b/.catkin_tools/profiles/default/packages/reachability/devel_manifest.txt index acce48a20d50a582181471c6364ca1bf648a984f..bd2e016b375d9c3743cef1ccd43fa18d830afa80 100644 --- a/.catkin_tools/profiles/default/packages/reachability/devel_manifest.txt +++ b/.catkin_tools/profiles/default/packages/reachability/devel_manifest.txt @@ -6,7 +6,6 @@ reachability /home/matteo/reachability/devel/.private/reachability/share/reachability/cmake/reachability-msg-paths.cmake /home/matteo/reachability/devel/share/reachability/cmake/reachability-msg-paths.cmake /home/matteo/reachability/devel/.private/reachability/share/roseus/ros/reachability/manifest.l /home/matteo/reachability/devel/share/roseus/ros/reachability/manifest.l /home/matteo/reachability/devel/.private/reachability/lib/reachability/map_analyse /home/matteo/reachability/devel/lib/reachability/map_analyse -/home/matteo/reachability/devel/.private/reachability/lib/reachability/publisher_node /home/matteo/reachability/devel/lib/reachability/publisher_node /home/matteo/reachability/devel/.private/reachability/lib/reachability/visualization /home/matteo/reachability/devel/lib/reachability/visualization /home/matteo/reachability/devel/.private/reachability/lib/reachability/single_pick /home/matteo/reachability/devel/lib/reachability/single_pick /home/matteo/reachability/devel/.private/reachability/lib/reachability/reach_map /home/matteo/reachability/devel/lib/reachability/reach_map @@ -14,6 +13,5 @@ reachability /home/matteo/reachability/devel/.private/reachability/lib/reachability/pick_and_place /home/matteo/reachability/devel/lib/reachability/pick_and_place /home/matteo/reachability/devel/.private/reachability/lib/reachability/task_viewer /home/matteo/reachability/devel/lib/reachability/task_viewer /home/matteo/reachability/devel/.private/reachability/lib/reachability/eval_analyse /home/matteo/reachability/devel/lib/reachability/eval_analyse -/home/matteo/reachability/devel/.private/reachability/lib/reachability/base_position /home/matteo/reachability/devel/lib/reachability/base_position /home/matteo/reachability/devel/.private/reachability/lib/python3/dist-packages/reachability/__init__.py /home/matteo/reachability/devel/lib/python3/dist-packages/reachability/__init__.py /home/matteo/reachability/devel/.private/reachability/lib/pkgconfig/reachability.pc /home/matteo/reachability/devel/lib/pkgconfig/reachability.pc diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index c2e1bd9bb62f5f6138ada867504d0e20d660cdad..6842f49097f10bbd82d966dea0e104d77f31cc8a 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -1,19 +1,20 @@ { - "configurations": [ - { - "browse": { - "databaseFilename": "", - "limitSymbolsToIncludedHeaders": true - }, - "includePath": [ - "/opt/ros/noetic/include/**", - "/home/matteo/catkin_ws/devel/include/**", - "/home/matteo/catkin_ws/src/beginner_tutorials/include/**", - "/home/matteo/catkin_ws/src/komplette_zeitverschwendung/include/**", - "/usr/include/**" - ], - "name": "ROS" - } - ], - "version": 4 + "configurations": [ + { + "browse": { + "databaseFilename": "", + "limitSymbolsToIncludedHeaders": true + }, + "includePath": [ + "/opt/ros/noetic/include/**", + "/home/matteo/catkin_ws/devel/include/**", + "/home/matteo/catkin_ws/src/beginner_tutorials/include/**", + "/home/matteo/catkin_ws/src/komplette_zeitverschwendung/include/**", + "/home/matteo/reachability/src/mtc/include/**", + "/usr/include/**" + ], + "name": "ROS" + } + ], + "version": 4 } \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index e0f87089cf20993e636bd9ad82960f91554fcbbd..40057ab04b1b13167eb38ff9d40fcafacad8c57e 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -79,7 +79,8 @@ "variant": "cpp", "bit": "cpp", "valarray": "cpp", - "regex": "cpp" + "regex": "cpp", + "any": "cpp" }, "cmake.sourceDirectory": "${workspaceFolder}/src" } \ No newline at end of file diff --git a/src/.rosinstall b/src/.rosinstall new file mode 100644 index 0000000000000000000000000000000000000000..97dd12a43027c4fa0a399abf615865e047d2c168 --- /dev/null +++ b/src/.rosinstall @@ -0,0 +1 @@ +# THIS IS AN AUTOGENERATED FILE, LAST GENERATED USING wstool ON 2022-07-25 diff --git a/src/moveit_grasps b/src/moveit_grasps new file mode 160000 index 0000000000000000000000000000000000000000..e70b88f97ed223dbcaecd3343316b6900029dacd --- /dev/null +++ b/src/moveit_grasps @@ -0,0 +1 @@ +Subproject commit e70b88f97ed223dbcaecd3343316b6900029dacd diff --git a/src/mtc/CMakeLists.txt b/src/mtc/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..d65f948ecfbd023f06f919e83b0a10792dea856e --- /dev/null +++ b/src/mtc/CMakeLists.txt @@ -0,0 +1,110 @@ +cmake_minimum_required(VERSION 3.0.2) +project(mtc) +# C++ 11 +add_compile_options(-std=c++11) + +# Warnings +add_definitions(-W -Wall -Wextra + -Wwrite-strings -Wunreachable-code -Wpointer-arith + -Winit-self -Wredundant-decls + -Wno-unused-parameter -Wno-unused-function) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) +find_package(Boost REQUIRED thread system) +find_package(yaml-cpp REQUIRED) + +find_package(OpenMP) + +# Load catkin and all dependencies required for this package +find_package(catkin REQUIRED COMPONENTS + eigen_conversions + geometry_msgs + message_generation + moveit_core + moveit_msgs + moveit_ros_planning + moveit_ros_planning_interface + moveit_visual_tools + roscpp + roslint + rosparam_shortcuts + std_msgs + tf + tf_conversions + trajectory_msgs +) + +# Catkin +catkin_package( + LIBRARIES + moveit_grasps + moveit_grasps_filter + CATKIN_DEPENDS + geometry_msgs + message_runtime + moveit_msgs + moveit_visual_tools + rosparam_shortcuts + std_msgs + trajectory_msgs + INCLUDE_DIRS + include + DEPENDS + EIGEN3 +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +# Grasp Library +add_library(moveit_grasps + src/grasp_candidate.cpp + src/grasp_data.cpp + src/grasp_generator.cpp + src/grasp_scorer.cpp + src/two_finger_grasp_data.cpp + src/two_finger_grasp_generator.cpp + src/two_finger_grasp_scorer.cpp +) +target_link_libraries(moveit_grasps + ${catkin_LIBRARIES} ${Boost_LIBRARIES} +) + +# Grasp Filter Library +add_library(moveit_grasps_filter + src/grasp_filter.cpp + src/two_finger_grasp_filter.cpp + src/grasp_planner.cpp +) +target_link_libraries(moveit_grasps_filter + moveit_grasps + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) +set_target_properties(moveit_grasps_filter PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") # for threading +set_target_properties(moveit_grasps_filter PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + +# Demo grasp pipeline +add_executable(base_routine src/base_routine.cpp) +add_executable(moveit_grasps_grasp_pipeline src/grasp_pipeline.cpp) + + +add_dependencies(base_routine ${base_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(moveit_grasps_grasp_pipeline +moveit_grasps moveit_grasps_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES} yaml-cpp +) + +target_link_libraries(base_routine + ${catkin_LIBRARIES} + ${OCTOMAP_LIBRARIES} + yaml-cpp +) diff --git a/src/mtc/config/moveit_grasps_config.yaml b/src/mtc/config/moveit_grasps_config.yaml new file mode 100644 index 0000000000000000000000000000000000000000..38c33d08e174a8c7c34951a0c12ec633c8e47a32 --- /dev/null +++ b/src/mtc/config/moveit_grasps_config.yaml @@ -0,0 +1,55 @@ +moveit_grasps: + #################################### + # These settings are used by the grasp generator which generates GraspCandidates + #################################### + generator: + # Other verbose stuff + verbose: true + + # Show hand of generated grasps + show_prefiltered_grasps: true + show_prefiltered_grasps_speed: 0.001 # (sec / grasp) + + # -------------------------------- + # Finger gripper specific settings + # -------------------------------- + # None yet + + # -------------------------------- + # Suction gripper specific settings + # -------------------------------- + # Visual debug options + debug_top_grasps: false + # Show suction gripper overhang + show_grasp_overhang: false + + #################################### + # The grasp filter filters out unreachable GraspCandidates and populates valid GraspCandidates' pregrasp_ik_solution_ and grasp_ik_solution_'s + #################################### + filter: + # Show summary statistics + statistics_verbose: true + # Visualize the planes used by the filter + show_cutting_planes: true + # Collision checking in verbose. Note: this will cause filtering to happen on a single thread rather than in parallel + collision_verbose: true + collision_verbose_speed: 0.01 # (sec/ solution) + # Collision checking in verbose on second pass (after all failed) + show_grasp_filter_collision_if_failed: true + # Show post-filter arrows + show_filtered_grasps: true + # Show pose-filter arm ik solutions + show_filtered_arm_solutions: true + show_filtered_arm_solutions_pregrasp_speed: 0.01 # (sec / solution) + show_filtered_arm_solutions_speed: 0.01 # (sec / solution) + + #################################### + # The GraspPlanner generates approach, lift and retreat paths for a GraspCandidate. + # If the GraspPlanner is unable to plan 100% of the approach path and at least ~90% of the lift and retreat paths, then it considers the GraspCandidate to be infeasible + #################################### + planner: + # Cartesian planning + statistics_verbose: true + verbose_cartesian_filtering: false + show_cartesian_waypoints: false + collision_checking_verbose: false diff --git a/src/mtc/config/panda_grasp_data.yaml b/src/mtc/config/panda_grasp_data.yaml new file mode 100644 index 0000000000000000000000000000000000000000..9c7f45cc20f6877a6a32a7c0e5762ccb40e32db3 --- /dev/null +++ b/src/mtc/config/panda_grasp_data.yaml @@ -0,0 +1,88 @@ +base_link: 'world' +# ===================================================== +# Note: these parameters are copied from Robotiq gripper without tweaking +hand: + end_effector_name: 'hand' # ee group name + end_effector_type: 'finger' # 'finger' or 'suction' + + # x/y translation and angle orientation grasp resolution parameters used for sampling grasp candidates + grasp_resolution : 1 + angle_resolution : 90 # degrees + + # min/max values of the grasp depth range in meter. + # This should fit to the distance from finger tip to inner palm or the suction cup stroke + grasp_max_depth : 0.035 + grasp_min_depth : 0.0 + # grasp_depth_resolution is the discretization distance in meter used for sampling the grasp depth value. + # Idealy this should be a small fraction of the depth range (i.e. 0.005). + # If the value exceeds the depth distance range grasp_max_depth is used by default. + grasp_depth_resolution : 0.035 + + # A grasp motion is composed of three linear paths: approach, retreat, lift + # The values below define the desired distances of each path. + # Length of approach path (this is in addition to the grasp_max_depth) + approach_distance_desired: 0.05 # approach path (in addition to grasp_max_depth) + retreat_distance_desired: 0.05 # retreat path (in addition to grasp_max_depth) + lift_distance_desired: 0.01 # lift path (used as MINIMUM safety distance from surface) + + # minimum padding on each side of the object on approach + grasp_padding_on_approach: 0.005 # meter + + # We need to know the transform from the arm IK link to the grasp point. By default we define the transform manually with + # `tcp_to_eef_mount_transform`. Alternatively, we can set this to a frame in the robot model and compute the transform + # automatically. this is done by setting `define_tcp_by_name: true` and providing a value for `tcp_name`. + # + # Distance from the eef mount to the palm of end effector [x, y, z, r, p, y] + # z-axis pointing toward object to grasp + # x-axis perp. to movement of grippers + # y-axis parallel to movement of grippers + # If we have a tool center point frame, we would set: + define_tcp_by_name: true + tcp_name: "tool_center_point" + # Alternatively, if we cannot add a frame for the tool center point, we can define tcp by transform + # tcp_to_eef_mount_transform : [0, 0, -0.105, 0, 0, -0.7853] # NOTE: Imaginary point in the middle + + # Target durations in seconds for reaching pregrasp and grasp positions in end effector motions + pregrasp_time_from_start : 0.0 + grasp_time_from_start : 0.0 + + # Joint names and state values for the end effector. + # The names should match the values of pregrasp_posture and grasp_posture. + # These values can be left empty if end_effector_type is 'suction' + pregrasp_posture : [0.04] # open position + grasp_posture : [0.0] # close position + joints : ['panda_finger_joint1'] + + ############################## + ## Finger gripper parameters + ############################## + + # Distances between fingers matching to pregrasp_posture and grasp_posture, in meters + max_finger_width: 0.085 + min_finger_width: 0.06 + + # Maximum allowed finger width for a grasp. + # This value should be considerably smaller than max_finger_width + # to allow padded collision checks + max_grasp_width: 0.08 + + # width of gripper fingers to ensure the finger tips fully overlap the object + gripper_finger_width : 0.015 + + ############################## + ## Suction gripper parameters + ############################## + # The width of the suction gripper along the specified axes. (Assumes that Z points in the direction of the suction cups) + active_suction_range_x : 0.22 + active_suction_range_y : 0.12 + + # Some suction grippers can selectively enable and disable regions of suction. If you have more than one + # region that can be selectively activated or de-activated, set those values here and moveit_grasps + # will prioritize grasp candidates which use full suction regions rather than partial regions. + # it will also set the suction_regions_in_contact_ variable in the grasp candidates so that you are able + # to only enable suction on regions that are in contact with the grasp target. + # Note that this metric assumes that suction regions are equally sized + # ie. width = active_suction_range_x / suction_cols_count + # + suction_rows_count: 2 + suction_cols_count: 2 diff --git a/src/mtc/descriptions/dummy.yaml b/src/mtc/descriptions/dummy.yaml new file mode 100644 index 0000000000000000000000000000000000000000..a2fcd6d098bc55190970c7572ec152bd247c2d63 --- /dev/null +++ b/src/mtc/descriptions/dummy.yaml @@ -0,0 +1,284 @@ +task: + groups: + robot_x: + - + - -0.3 + - -0.7 + - 0.89 + - + - -0.3 + - -0.6 + - 0.89 + - + - -0.2 + - -0.7 + - 0.89 + - + - -0.2 + - -0.6 + - 0.89 + - + - 0.1 + - -0.7 + - 0.89 + - + - 0.1 + - -0.6 + - 0.89 + - + - 0.1 + - -0.3 + - 0.89 + - + - 0.1 + - -0.2 + - 0.89 + - + - 0.1 + - -0.1 + - 0.89 + - + - 0.2 + - -0.3 + - 0.89 + - + - 0.2 + - -0.2 + - 0.89 + - + - 0.2 + - -0.1 + - 0.89 + - + - 0.3 + - -0.3 + - 0.89 + - + - 0.3 + - -0.2 + - 0.89 + - + - 0.3 + - -0.1 + - 0.89 + - + - 0.1 + - 0.1 + - 0.89 + - + - 0.1 + - 0.2 + - 0.89 + - + - 0.1 + - 0.3 + - 0.89 + - + - 0.2 + - 0.1 + - 0.89 + - + - 0.2 + - 0.2 + - 0.89 + - + - 0.2 + - 0.3 + - 0.89 + - + - 0.3 + - 0.1 + - 0.89 + - + - 0.3 + - 0.2 + - 0.89 + - + - 0.3 + - 0.3 + - 0.89 + - + - -0.3 + - 0.6 + - 0.89 + - + - -0.3 + - 0.7 + - 0.89 + - + - -0.2 + - 0.6 + - 0.89 + - + - -0.2 + - 0.7 + - 0.89 + - + - 0.1 + - 0.6 + - 0.89 + - + - 0.1 + - 0.7 + - 0.89 + robot_y: + - + - 0.1 + - 1.01 + - 0.89 + - + - 0.1 + - 1.11 + - 0.89 + - + - 0.1 + - 1.21 + - 0.89 + - + - 0.2 + - 1.01 + - 0.89 + - + - 0.2 + - 1.11 + - 0.89 + - + - 0.2 + - 1.21 + - 0.89 + - + - 0.3 + - 1.01 + - 0.89 + - + - 0.3 + - 1.11 + - 0.89 + - + - 0.3 + - 1.21 + - 0.89 + - + - 0.1 + - 1.41 + - 0.89 + - + - 0.1 + - 1.51 + - 0.89 + - + - 0.1 + - 1.61 + - 0.89 + - + - 0.2 + - 1.41 + - 0.89 + - + - 0.2 + - 1.51 + - 0.89 + - + - 0.2 + - 1.61 + - 0.89 + - + - 0.3 + - 1.41 + - 0.89 + - + - 0.3 + - 1.51 + - 0.89 + - + - 0.3 + - 1.61 + - 0.89 + - + - -0.3 + - 1.91 + - 0.89 + - + - -0.3 + - 2.01 + - 0.89 + - + - -0.2 + - 1.91 + - 0.89 + - + - -0.2 + - 2.01 + - 0.89 + - + - 0.1 + - 1.91 + - 0.89 + - + - 0.1 + - 2.01 + - 0.89 + - + - 0 + - 0.6525 + - 1.49 + - + - 0.1 + - -0.7 + - 0.9355 + - + - 0.2 + - 0.3 + - 0.9355 + - + - 0.2 + - -0.1 + - 0.9355 + - + - 0.2 + - -0.3 + - 0.9355 + - + - 0.1 + - 1.91 + - 0.9355 + - + - -0.3 + - -0.6 + - 0.9355 + - + - 0.3 + - 1.41 + - 0.9355 + - + - -0.22 + - 0 + - 0.89 + - + - -0.22 + - 1.31 + - 0.89 + - + - -0.3 + - 0.6 + - 0.89 + - + - -0.3 + - 0.7 + - 0.89 + - + - -0.2 + - 0.6 + - 0.89 + - + - -0.2 + - 0.7 + - 0.89 + - + - 0.1 + - 0.6 + - 0.89 + - + - 0.1 + - 0.7 + - 0.89 \ No newline at end of file diff --git a/src/mtc/include/grasp_candidate.h b/src/mtc/include/grasp_candidate.h new file mode 100644 index 0000000000000000000000000000000000000000..e954038fbffbc54851d8e91b389edc0d72aa3ccb --- /dev/null +++ b/src/mtc/include/grasp_candidate.h @@ -0,0 +1,142 @@ +#ifndef MOVEIT_GRASPS__GRASP_CANDIDATE_ +#define MOVEIT_GRASPS__GRASP_CANDIDATE_ + +// ROS +#include <ros/ros.h> +#include <moveit_msgs/Grasp.h> + +// Grasping +#include "../include/grasp_data.h" + +// MoveIt +#include <moveit/robot_state/robot_state.h> +#include <moveit_msgs/Grasp.h> + +namespace moveit_grasps +{ +typedef std::vector<std::vector<moveit::core::RobotStatePtr> > GraspTrajectories; +enum GraspTrajectorySegments +{ + APPROACH = 0, + LIFT = 1, + RETREAT = 2 +}; + +struct GraspFilterCode +{ + enum + { + NOT_FILTERED = 0, + GRASP_FILTERED_BY_IK, // Ik solution at grasp failed + GRASP_FILTERED_BY_CUTTING_PLANE, // grasp pose is in an unreachable part of the environment (eg: behind a wall) + GRASP_FILTERED_BY_ORIENTATION, // grasp pose is not desireable + GRASP_FILTERED_BY_IK_CLOSED, // ik solution was fine with grasp opened, but failed with grasp closed + PREGRASP_FILTERED_BY_IK, // Ik solution before approach failed + GRASP_INVALID, // An error occured while processing the grasp + LAST // Used to track last value in the base class when inheriting + }; +}; + +/** + * \brief Contains collected data for each potential grasp after it has been verified / filtered + * This includes the pregrasp and grasp IK solution + */ +class GraspCandidate +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GraspCandidate(const moveit_msgs::Grasp& grasp, const GraspDataPtr& grasp_data, const Eigen::Isometry3d& cuboid_pose); + + bool getPreGraspState(moveit::core::RobotStatePtr& robot_state); + + bool getGraspStateOpen(moveit::core::RobotStatePtr& robot_state); + + bool getGraspStateOpenEEOnly(moveit::core::RobotStatePtr& robot_state); + + bool getGraspStateClosed(moveit::core::RobotStatePtr& robot_state); + + bool getGraspStateClosedEEOnly(moveit::core::RobotStatePtr& robot_state); + + virtual bool isValid(); + + moveit_msgs::Grasp grasp_; + + /*# Contents of moveit_msgs::Grasp for reference + + # A name for this grasp + string id + + # The internal posture of the hand for the pre-grasp + # only positions are used + trajectory_msgs/JointTrajectory pre_grasp_posture + + # The internal posture of the hand for the grasp + # positions and efforts are used + trajectory_msgs/JointTrajectory grasp_posture + + # The position of the end-effector for the grasp. This is the pose of + # the "parent_link" of the end-effector, not actually the pose of any + # link *in* the end-effector. Typically this would be the pose of the + # most distal wrist link before the hand (end-effector) links began. + geometry_msgs/PoseStamped grasp_pose + + # The estimated probability of success for this grasp, or some other + # measure of how "good" it is. + float64 grasp_quality + + # The approach direction to take before picking an object + GripperTranslation pre_grasp_approach + { + # defines a translation for the gripper, used in pickup or place tasks + # for example for lifting an object off a table or approaching the table for placing + + # the direction of the translation + geometry_msgs/Vector3Stamped direction + + # the desired translation distance + float32 desired_distance + + # the min distance that must be considered feasible before the + # grasp is even attempted + float32 min_distance + } + + # The retreat direction to take after a grasp has been completed (object is attached) + GripperTranslation post_grasp_retreat + + # The retreat motion to perform when releasing the object; this information + # is not necessary for the grasp itself, but when releasing the object, + # the information will be necessary. The grasp used to perform a pickup + # is returned as part of the result, so this information is available for + # later use. + GripperTranslation post_place_retreat + + # the maximum contact force to use while grasping (<=0 to disable) + float32 max_contact_force + + # an optional list of obstacles that we have semantic information about + # and that can be touched/pushed/moved in the course of grasping + string[] allowed_touch_objects + + # The minimum opening that the fingers can have as it approaches an object + float64 min_finger_open_on_approach + */ + + const GraspDataPtr grasp_data_; + // TODO(davetcoleman): possibly remove + Eigen::Isometry3d cuboid_pose_; // pose of original object to grasp + + int grasp_filtered_code_; // All codes defined in enum + + std::vector<double> grasp_ik_solution_; + std::vector<double> pregrasp_ik_solution_; + + // Store pregrasp, grasp, lifted, and retreat trajectories + GraspTrajectories segmented_cartesian_traj_; +}; // class + +typedef std::shared_ptr<GraspCandidate> GraspCandidatePtr; + +} // namespace moveit_grasps + +#endif diff --git a/src/mtc/include/grasp_data.h b/src/mtc/include/grasp_data.h new file mode 100644 index 0000000000000000000000000000000000000000..874f1da15b6771408ca9afdef8fb9dd9e454ff86 --- /dev/null +++ b/src/mtc/include/grasp_data.h @@ -0,0 +1,102 @@ +#ifndef MOVEIT_GRASPS__GRASP_DATA_H_ +#define MOVEIT_GRASPS__GRASP_DATA_H_ + +// Ros +#include <ros/node_handle.h> + +// Msgs +#include <geometry_msgs/Pose.h> +#include <trajectory_msgs/JointTrajectory.h> + +// MoveIt +#include <moveit/macros/class_forward.h> +#include <moveit/robot_state/robot_state.h> +#include <moveit/robot_model/link_model.h> + +// moveit grasps +#include "suction_voxel_matrix.h" + +namespace moveit_grasps +{ +MOVEIT_CLASS_FORWARD(GraspData); + +// Map various arms to end effector grasp datas +typedef std::map<const robot_model::JointModelGroup*, moveit_grasps::GraspDataPtr> GraspDatas; + +struct GraspData +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /** + * \brief Loads grasp data from a yaml file (load from roslaunch) + * \param node handle - allows for namespacing + * \param end effector name - which side of a two handed robot to load data for. should correspond to SRDF EE names + */ + GraspData(const ros::NodeHandle& nh, const std::string& end_effector, + const moveit::core::RobotModelConstPtr& robot_model); + + /** + * \brief Helper function for constructor + * \return true on success + */ + virtual bool loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector); + + /** + * \brief Alter a robot state so that the end effector corresponding to this grasp data is in pre-grasp state (OPEN) + * \param joint state of robot + * \return true on success + */ + bool setRobotStatePreGrasp(robot_state::RobotStatePtr& robot_state); + + /** + * \brief Alter a robot state so that the end effector corresponding to this grasp data is in grasp state (CLOSED) + * \param joint state of robot + * \return true on success + */ + bool setRobotStateGrasp(robot_state::RobotStatePtr& robot_state); + + /** + * \brief Alter a robot state so that the end effector corresponding to this grasp data is in a grasp posture + * \param joint state of robot + * \param posture - what state to set the end effector + * \return true on success + */ + bool setRobotState(robot_state::RobotStatePtr& robot_state, const trajectory_msgs::JointTrajectory& posture); + + /** + * \brief Debug data to console + */ + virtual void print(); + + // The (possibly fictional) center point of a grasp + std::string tcp_name_; + Eigen::Isometry3d tcp_to_eef_mount_; // Convert generic grasp pose to the parent arm's eef_mount frame of reference + + trajectory_msgs::JointTrajectory pre_grasp_posture_; // when the end effector is in "open" position + trajectory_msgs::JointTrajectory grasp_posture_; // when the end effector is in "close" position + std::string base_link_; // name of global frame with z pointing up + + const robot_model::JointModelGroup* ee_jmg_; // this end effector + const robot_model::JointModelGroup* arm_jmg_; // the arm that attaches to this end effector + const robot_model::RobotModelConstPtr robot_model_; + + // Duplicate end effector data copied from RobotModel + // the last link in the kinematic chain before the end effector, e.g. "/gripper_roll_link" class + const robot_model::LinkModel* parent_link_; + + int angle_resolution_; // generate grasps at increments of: angle_resolution * pi / 180 + + double grasp_resolution_; + double grasp_depth_resolution_; // generate grasps at this depth resolution along grasp_max_depth_ + double grasp_min_depth_; // minimum amount fingers must overlap object + double grasp_max_depth_; // Maximum distance from tip of end effector inwords that an object can be for a grasp + + // grasp approach and retreat parameters + double approach_distance_desired_; // this is in addition to the grasp_max_depth + double retreat_distance_desired_; // this is in addition to the grasp_max_depth + double lift_distance_desired_; + double grasp_padding_on_approach_; +}; + +} // namespace moveit_grasps + +#endif diff --git a/src/mtc/include/grasp_filter.h b/src/mtc/include/grasp_filter.h new file mode 100644 index 0000000000000000000000000000000000000000..a3ece959f2e7e36132244f190ee37ebe7bfa98b1 --- /dev/null +++ b/src/mtc/include/grasp_filter.h @@ -0,0 +1,391 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman <dave@picknik.ai> + Desc: Filters grasps based on kinematic feasibility and collision +*/ + +#ifndef MOVEIT_GRASPS__GRASP_FILTER_ +#define MOVEIT_GRASPS__GRASP_FILTER_ + +// ROS +#include <ros/ros.h> +#include <tf_conversions/tf_eigen.h> +#include <geometry_msgs/PoseArray.h> +#include <geometry_msgs/PoseStamped.h> + +// Grasping +#include "../include/grasp_generator.h" +#include "../include/grasp_candidate.h" + +// Rviz +#include <moveit_visual_tools/moveit_visual_tools.h> + +// MoveIt +#include <moveit/robot_state/robot_state.h> +#include <moveit_msgs/Grasp.h> +#include <moveit/kinematics_plugin_loader/kinematics_plugin_loader.h> +#include <moveit/planning_scene_monitor/planning_scene_monitor.h> + +// C++ +#include <boost/thread.hpp> +#include <cmath> + +namespace moveit_grasps +{ +enum GraspParallelPlane +{ + XY, + XZ, + YZ +}; + +/** + * \brief Contains information to filter grasps by a cutting plane + */ +struct CuttingPlane +{ + Eigen::Isometry3d pose_; + GraspParallelPlane plane_; + int direction_; + + CuttingPlane(Eigen::Isometry3d pose, GraspParallelPlane plane, int direction) + : pose_(pose), plane_(plane), direction_(direction) + { + } +}; +typedef std::shared_ptr<CuttingPlane> CuttingPlanePtr; + +/** + * \brief Contains information to filter grasps by orientation + */ +struct DesiredGraspOrientation +{ + Eigen::Isometry3d pose_; + double max_angle_offset_; + + DesiredGraspOrientation(Eigen::Isometry3d pose, double max_angle_offset) + : pose_(pose), max_angle_offset_(max_angle_offset) + { + } +}; +typedef std::shared_ptr<DesiredGraspOrientation> DesiredGraspOrientationPtr; + +/** + * \brief Struct for passing parameters to threads, for cleaner code + */ +struct IkThreadStruct +{ + IkThreadStruct(std::vector<GraspCandidatePtr>& grasp_candidates, // the input + const planning_scene::PlanningScenePtr& planning_scene, Eigen::Isometry3d& link_transform, + std::size_t grasp_id, kinematics::KinematicsBaseConstPtr kin_solver, + const robot_state::RobotStatePtr& robot_state, double timeout, bool filter_pregrasp, bool visual_debug, + std::size_t thread_id, const std::string& grasp_target_object_id) + : grasp_candidates_(grasp_candidates) + , planning_scene_(planning_scene::PlanningScene::clone(planning_scene)) + , link_transform_(link_transform) + , grasp_id(grasp_id) + , kin_solver_(kin_solver) + , robot_state_(std::make_shared<robot_state::RobotState>(*robot_state)) + , timeout_(timeout) + , filter_pregrasp_(filter_pregrasp) + , visual_debug_(visual_debug) + , thread_id_(thread_id) + , grasp_target_object_id_(grasp_target_object_id) + { + } + + std::vector<GraspCandidatePtr>& grasp_candidates_; + planning_scene::PlanningScenePtr planning_scene_; + Eigen::Isometry3d link_transform_; + std::size_t grasp_id; + kinematics::KinematicsBaseConstPtr kin_solver_; + robot_state::RobotStatePtr robot_state_; + double timeout_; + bool filter_pregrasp_; + bool visual_debug_; + std::size_t thread_id_; + // The name of the target grasp object to be used in the planning scene. + // NOTE: The ik filtering checks if the user has already added the grasp object + // to the planning scene. If it hasn't then GraspFilter will not modify the planning scene allowed collision matrix + std::string grasp_target_object_id_; + + // Used within processing function + geometry_msgs::PoseStamped ik_pose_; // Set from grasp candidate + std::vector<double> ik_seed_state_; +}; +typedef std::shared_ptr<IkThreadStruct> IkThreadStructPtr; + +class GraspFilter +{ +public: + // Constructor + GraspFilter(const robot_state::RobotStatePtr& robot_state, + const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools); + + /** + * \brief Return grasps that are kinematically feasible + * \param grasp_candidates - all possible grasps that this will test. this vector is returned modified + * \param arm_jmg - the arm to solve the IK problem on + * \param filter_pregrasp -whether to also check ik feasibility for the pregrasp position + * \param target_object_id - The name of the target grasp object in the planning scene if it exists + * \return true on successful filter + */ + virtual bool filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const robot_model::JointModelGroup* arm_jmg, const moveit::core::RobotStatePtr& seed_state, + bool filter_pregrasp = false, const std::string& target_object_id = ""); + + virtual bool filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates, + const planning_scene::PlanningScenePtr& planning_scene, + const robot_model::JointModelGroup* arm_jmg, const moveit::core::RobotStatePtr& seed_state, + bool filter_pregrasp = false, const std::string& target_object_id = ""); + /** + * \brief Return grasps that are kinematically feasible + * \param grasp_candidates - all possible grasps that this will test. this vector is returned modified + * \param arm_jmg - the arm to solve the IK problem on + * \param seed_state - A robot state to be used for IK. Ideally this will be close to the desired goal configuration. + * \param filter_pregrasp - Whether to also check ik feasibility for the pregrasp position + * \param visualize - visualize IK filtering + * \param target_object_id - The name of the target grasp object in the planning scene if it exists + * \return true on successful filter + */ + virtual std::size_t filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const robot_model::JointModelGroup* arm_jmg, + const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, + bool visualize, const std::string& grasp_target_object_id = ""); + + virtual std::size_t filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates, + const planning_scene::PlanningScenePtr& planning_scene, + const robot_model::JointModelGroup* arm_jmg, + const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, + bool visualize, const std::string& grasp_target_object_id = ""); + + /** + * \brief Print grasp filtering statistics + */ + virtual void printFilterStatistics(const std::vector<GraspCandidatePtr>& grasp_candidates) const; + + /** + * \brief Method for checking part of the possible grasps list. MUST BE THREAD SAFE + */ + virtual bool processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct); + + /** + * \brief Used for sorting an array of CandidateGrasps + * \return true if A is less than B + */ + static bool compareGraspScores(const GraspCandidatePtr& grasp_a, const GraspCandidatePtr& grasp_b) + { + // Determine if A or B has higher quality + return (grasp_a->grasp_.grasp_quality > grasp_b->grasp_.grasp_quality); + } + + /** + * \brief Of an array of grasps, sort the valid ones from best score to worse score + * \return false if no grasps remain + */ + bool removeInvalidAndFilter(std::vector<GraspCandidatePtr>& grasp_candidates) const; + + /** + * \brief Add a cutting plane filter for a shelf bin + * \return true on success + */ + bool addCuttingPlanesForBin(const Eigen::Isometry3d& world_to_bin, const Eigen::Isometry3d& bin_to_product, + const double& bin_width, const double& bin_height); + + /* \brief Set the ACM entry to ignore collisions between the ee_link_names and the object in the planning_scene */ + void setACMFingerEntry(const std::string& object_name, bool allowed, const std::vector<std::string>& ee_link_names, + const planning_scene::PlanningScenePtr& planning_scene); + +protected: + /** + * \brief Filter grasps by cutting plane + * \param grasp_candidates - all possible grasps that this will test. this vector is returned modified + * \param filter_pose - pose of filter that will define cutting plane + * \param plane - the cutting plane (XY, XZ, or YZ) + * \param direction - which side of this plane to cut (+/- 1) + * \return true if grasp is filtered by operation + */ + bool filterGraspByPlane(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& filter_pose, + GraspParallelPlane plane, int direction) const; + + /** + * \brief Filter grasps by desired orientation. Think of reaching into a small opening, you can only rotate your hand + * a tiny amount and still grasp an object. If there's empty space behind an object, grasps behind the object aren't + * removed by the cutting plane operations. We know we'll never get to them because they deviate too much from the + * desired grasping pose... straight in. + * \param grasp_candidates - a grasp candidate that this will test. + * \param desired_pose - the desired grasp pose ( using standard grasping orientation ) + * \param max_angular_offset - maximum angle allowed between the grasp pose and the desired pose + * \return true if grasp is filtered by operation + */ + bool filterGraspByOrientation(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& desired_pose, + double max_angular_offset) const; + + /** + * \brief Filter grasps by feasablity of the grasp pose. + * \param grasp_candidates - a grasp candidate that this will test + * \param grasp_ik_solution - the IK solution if one exists + * \param ik_thread_struct - a struct containing the planning_scene + * \return true if grasp is filtered by operation + */ + bool filterGraspByGraspIK(const GraspCandidatePtr& grasp_candidate, std::vector<double>& grasp_ik_solution, + const IkThreadStructPtr& ik_thread_struct) const; + + /** + * \brief Filter grasps by feasablity of the pre grasp pose. + * \param grasp_candidates - a grasp candidate that this will test + * \param grasp_ik_solution - the IK solution if one exists + * \param ik_thread_struct - a struct containing the planning_scene + * \return true if grasp is filtered by operation + */ + bool filterGraspByPreGraspIK(const GraspCandidatePtr& grasp_candidate, std::vector<double>& pregrasp_ik_solution, + const IkThreadStructPtr& ik_thread_struct) const; + + /** + * \brief Helper for the thread function to find IK solutions + * \return true on success + */ + bool findIKSolution(std::vector<double>& ik_solution, const IkThreadStructPtr& ik_thread_struct, + const GraspCandidatePtr& grasp_candidate, + const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const; + + /** + * \brief add a cutting plane + * \param pose - pose describing the cutting plane + * \param plane - which plane to use as the cutting plane + * \param direction - on which side of the plane the grasps will be removed + */ + void addCuttingPlane(const Eigen::Isometry3d& pose, GraspParallelPlane plane, int direction); + + /** + * \brief Show all cutting planes that are currently enables + * \return true on success + */ + bool visualizeCuttingPlanes(); + + /** + * \brief clear all cutting planes + */ + void clearCuttingPlanes(); + + /** + * \brief add a desired grasp orientation + * \param pose - the desired grasping pose + * \param max_angle_offset - maximum amount a generated grasp can deviate from the desired pose + */ + void addDesiredGraspOrientation(const Eigen::Isometry3d& pose, double max_angle_offset); + + /** + * \brief clear all desired orientations + */ + void clearDesiredGraspOrientations(); + + /** + * \brief Show grasps after being filtered + * \return true on success + */ + bool visualizeGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates, + const moveit::core::JointModelGroup* arm_jmg); + + /** + * \brief Show IK solutions of entire arm + * \return true on success + */ + bool visualizeIKSolutions(const std::vector<GraspCandidatePtr>& grasp_candidates, + const moveit::core::JointModelGroup* arm_jmg, double animation_speed); + + /** + * \brief Show solutions of entire arm + * \return true on success + */ + bool visualizeCandidateGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates); + + /** + * \brief Show an entire planning scene + */ + void publishPlanningScene(const planning_scene::PlanningScenePtr& ps) const; + +protected: + // logging name + const std::string name_; + + // Allow a writeable robot state + robot_state::RobotStatePtr robot_state_; + + // Keep a robot state for every thread + std::vector<robot_state::RobotStatePtr> robot_states_; + + // Threaded kinematic solvers + std::map<std::string, std::vector<kinematics::KinematicsBaseConstPtr> > kin_solvers_; + + // Class for publishing stuff to rviz + moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; + // for rviz visualization of the planning scene + ros::Publisher planning_scene_publisher_; + + // Number of degrees of freedom for the IK solver to find + std::size_t num_variables_; + + // Time to allow IK solver to run + double solver_timeout_; + + // Visualization levels + bool collision_verbose_; + bool statistics_verbose_; + double collision_verbose_speed_; + bool show_filtered_grasps_; + bool show_filtered_arm_solutions_; + bool show_cutting_planes_; + double show_filtered_arm_solutions_speed_; + double show_filtered_arm_solutions_pregrasp_speed_; + bool show_grasp_filter_collision_if_failed_; + + // Shared node handle + ros::NodeHandle nh_; + + // Cutting planes and orientation filter + std::vector<CuttingPlanePtr> cutting_planes_; + std::vector<DesiredGraspOrientationPtr> desired_grasp_orientations_; + +}; // end of class + +typedef std::shared_ptr<GraspFilter> GraspFilterPtr; +typedef std::shared_ptr<const GraspFilter> GraspFilterConstPtr; + +} // namespace moveit_grasps + +#endif diff --git a/src/mtc/include/grasp_generator.h b/src/mtc/include/grasp_generator.h new file mode 100644 index 0000000000000000000000000000000000000000..4d8a3f55b7d4af2e6fbcc4610b98bd8d14d3bfee --- /dev/null +++ b/src/mtc/include/grasp_generator.h @@ -0,0 +1,248 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy + Desc: Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches +*/ + +#ifndef MOVEIT_GRASPS__GRASP_GENERATOR_H_ +#define MOVEIT_GRASPS__GRASP_GENERATOR_H_ + +// ROS +#include <ros/ros.h> + +// TF +#include <tf_conversions/tf_eigen.h> + +// Msgs +#include <geometry_msgs/PoseArray.h> + +// MoveIt +#include <moveit_msgs/Grasp.h> + +// geometric_shapes +#include <geometric_shapes/shape_operations.h> +#include <geometric_shapes/bodies.h> + +// Eigen +#include <Eigen/Core> +#include <Eigen/Geometry> +#include <Eigen/Eigenvalues> +#include <eigen_conversions/eigen_msg.h> + +// Visualization +#include <moveit_visual_tools/moveit_visual_tools.h> + +// moveit_grasps +#include "grasp_candidate.h" +#include "grasp_scorer.h" + +// bounding_box +//#include <bounding_box/bounding_box.h> + +// C++ +#include <cstdlib> +#include <string> +#include <cmath> +#include <limits> + +#include "grasp_data.h" + +namespace moveit_grasps +{ +static const double RAD2DEG = 57.2957795; +static const double MIN_GRASP_DISTANCE = 0.001; // m between grasps + +// Grasp axis orientation +enum grasp_axis_t +{ + X_AXIS, + Y_AXIS, + Z_AXIS +}; + +class GraspGenerator +{ +public: + // Eigen requires 128-bit alignment for the Eigen::Vector2d's array (of 2 doubles). + // With GCC, this is done with a attribute ((aligned(16))). + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * \brief Constructor + */ + GraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, bool verbose = false); + + /** + * \brief Create possible grasp positions around a cuboid + * \param cuboid_pose - centroid of object to grasp in world frame + * \param depth length of cuboid along local x-axis + * \param width length of cuboid along local y-axis + * \param height length of cuboid along local z-axis + * \param grasp_data data describing end effector + * \param grasp_candidate_config parameter for selectively enabling and disabling different grasp types + * \param grasp_candidates possible grasps generated + * \return true if successful + */ + virtual bool generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height, + const GraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates) = 0; + + /** + * \brief Get the grasp direction vector relative to the world frame + * \param grasp + * \param name of parent link + * \return the approach direction + */ + static Eigen::Vector3d getPreGraspDirection(const moveit_msgs::Grasp& grasp, const std::string& ee_parent_link); + // static Eigen::Vector3d getPostGraspDirection(const moveit_msgs::Grasp &grasp, const std::string &ee_parent_link); + + /** + * \brief Using an input grasp description, get the pregrasp pose. The pregrasp pose is the grasp pose translated + * backwards in the grasp frame away from the object being grasped. + * \param grasp + * \param name of parent link + * \return pregrasp pose + */ + static geometry_msgs::PoseStamped getPreGraspPose(const GraspCandidatePtr& grasp_candidate, + const std::string& ee_parent_link); + /** + * \brief Compute the pre-grasp, grasp, lift and retreat poses for a grasp candidate + * \param grasp_candidate - the grasp candidate + * \param grasp_waypoints - a reference to a vector that will be populated with the pre-grasp, grasp, lift and retreat + * poses in that order. + */ + static void getGraspWaypoints(const GraspCandidatePtr& grasp_candidate, EigenSTL::vector_Isometry3d& grasp_waypoints); + + /** + * \brief Helper to convert a robot-specific grasp to an arrow pointed in the right direction + * \param grasp - the grasp to show + * \param arm - the planning group of the arm we want to display + * \return true on success + */ + void publishGraspArrow(const geometry_msgs::Pose& grasp, const GraspDataPtr& grasp_data, + const rviz_visual_tools::colors& color, double approach_length = 0.1); + + /** + * \brief Getter for Verbose + */ + bool getVerbose() + { + return verbose_; + } + + /** + * \brief Getter for ideal grasp pose + */ + Eigen::Isometry3d getIdealTCPGraspPose() + { + return ideal_grasp_pose_; + } + + /** + * \brief Setter for ideal grasp pose for scoring + */ + void setIdealTCPGraspPose(Eigen::Isometry3d ideal_pose) + { + ideal_grasp_pose_ = ideal_pose; + } + + /** + * \brief Setter for the roll pitch yall ideal grasp pose for scoring + */ + void setIdealTCPGraspPoseRPY(const std::vector<double>& ideal_grasp_orientation_rpy); + + /** + * \brief Setter for grasp score weights + */ + void setGraspScoreWeights(const GraspScoreWeightsPtr& grasp_score_weights) + { + grasp_score_weights_ = std::make_shared<GraspScoreWeights>(*grasp_score_weights); + } + + /** + * \brief Setter for grasp score weights + */ + const GraspScoreWeightsPtr& getGraspScoreWeights() + { + return grasp_score_weights_; + } + + /** + * \brief Setter for Verbose + */ + void setVerbose(bool verbose) + { + verbose_ = verbose; + } + + /** + * \brief Visualize animated grasps + * \return true on success + */ + bool visualizeAnimatedGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates, + const moveit::core::JointModelGroup* ee_jmg, double animation_speed); + + // Ideal grasp pose for scoring purposes + Eigen::Isometry3d ideal_grasp_pose_; + +protected: + // class for publishing stuff to rviz + moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; + + // Display more output both in console + bool verbose_; + + // Visual debug settings + bool show_prefiltered_grasps_; + double show_prefiltered_grasps_speed_; + + // Shared node handle + ros::NodeHandle nh_; + + // Transform from frame of box to global frame + Eigen::Isometry3d object_global_transform_; + + double min_grasp_distance_, max_grasp_distance_; + Eigen::Vector3d min_translations_, max_translations_; + + GraspScoreWeightsPtr grasp_score_weights_; + +}; // end of class + +typedef std::shared_ptr<GraspGenerator> GraspGeneratorPtr; +typedef std::shared_ptr<const GraspGenerator> GraspGeneratorConstPtr; + +} // namespace moveit_grasps + +#endif diff --git a/src/mtc/include/grasp_planner copy.h b/src/mtc/include/grasp_planner copy.h new file mode 100644 index 0000000000000000000000000000000000000000..a5271665b775d7b2a073298ef58f5eb2754db64e --- /dev/null +++ b/src/mtc/include/grasp_planner copy.h @@ -0,0 +1,163 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman <dave@picknik.ai> + Desc: Find the approach, lift, and retreat path for a candidate grasp (if a valid one exists) +*/ + +#ifndef MOVEIT_GRASPS__GRASP_PLANNER_ +#define MOVEIT_GRASPS__GRASP_PLANNER_ + +// ROS +#include <ros/ros.h> + +// moveit_grasps +#include "../include/grasp_candidate.h" +#include "../include/grasp_generator.h" + +// moveit +#include <moveit_visual_tools/moveit_visual_tools.h> + +namespace moveit_grasps +{ +// Allow an interrupt to be called that waits for user input, useful for debugging +typedef boost::function<void(std::string message)> WaitForNextStepCallback; + +class GraspPlanner +{ +public: + /** + * \brief Constructor + */ + GraspPlanner(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools); + + /** + * \brief Plan entire cartesian manipulation sequence + * \param grasp_candidates - GraspCandidates for which we will compute apprach, lift and retreat paths + * \param robot_state - robot_state to be used for computeCartesianPath + * \param planning_scene_monitor - Current state of the world + * \param grasp_data - robot gripper configuration + * \param grasp_object_id - The name of the target grasp object to be used in the planning scene. + * \return true on success + */ + bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates, + const robot_state::RobotStatePtr& robot_state, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::string& grasp_object_id = ""); + + bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates, + const robot_state::RobotStatePtr& robot_state, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const std::string& grasp_object_id = ""); + + /** + * \brief Plan entire cartesian manipulation sequence + * \param input - description + * \return true on success + */ + bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + bool verbose_cartesian_filtering, const std::string& grasp_object_id = ""); + + bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state, + const planning_scene::PlanningSceneConstPtr& planning_scene, + bool verbose_cartesian_filtering, const std::string& grasp_object_id = ""); + + /** + * \brief Compute a cartesian path along waypoints + * \return true on success + */ + bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const moveit::core::RobotStatePtr& start_state, + const EigenSTL::vector_Isometry3d& waypoints, + const std::string& grasp_object_id = ""); + + bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const moveit::core::RobotStatePtr& start_state, + const EigenSTL::vector_Isometry3d& waypoints, + const std::string& grasp_object_id = ""); + + /** + * \brief Wait for user input to proceeed + * \param message - text to display to user when waiting + */ + void waitForNextStep(const std::string& message); + + /** + * \brief Allow an interrupt to be called that waits for user input, useful for debugging + * \param message - text to display to user when waiting + */ + // TODO(davetcoleman): Ideally all the classes in moveit_grasps would leverage this approach + void setWaitForNextStepCallback(WaitForNextStepCallback callback); + + /** + * \brief Load parameter settings of the server under ~/debug_level + * \param parent_name - only used for debugging, allows one to see what paremeters are loaded in what namespace + * \param setting_namespace - where on this node's namespace to load settings + * e.g. /this_name/setting_namespace/some_parameter + * \return true on success + */ + bool loadEnabledSettings(); + + /** + * \brief Check if a setting is enabled + * \param setting_name - name of key on the parameter server as loaded in the 'setting_namespace' + * \return true if setting is enabled + */ + bool isEnabled(const std::string& setting_name); + +private: + // A shared node handle + ros::NodeHandle nh_; + + // Class for publishing stuff to rviz + moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; + + WaitForNextStepCallback wait_for_next_step_callback_; + + // Visualization settings + bool enabled_settings_loaded_ = false; + std::map<std::string, bool> enabled_setting_; + +}; // end class + +// Create smart pointers for this class +typedef std::shared_ptr<GraspPlanner> GraspPlannerPtr; +typedef std::shared_ptr<const GraspPlanner> GraspPlannerConstPtr; + +} // namespace moveit_grasps + +#endif diff --git a/src/mtc/include/grasp_planner.h b/src/mtc/include/grasp_planner.h new file mode 100644 index 0000000000000000000000000000000000000000..9b5d922f63884718faa38b90076d0b9df4bad31a --- /dev/null +++ b/src/mtc/include/grasp_planner.h @@ -0,0 +1,163 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman <dave@picknik.ai> + Desc: Find the approach, lift, and retreat path for a candidate grasp (if a valid one exists) +*/ + +#ifndef MOVEIT_GRASPS__GRASP_PLANNER_ +#define MOVEIT_GRASPS__GRASP_PLANNER_ + +// ROS +#include <ros/ros.h> + +// moveit_grasps +#include "grasp_candidate.h" +#include "grasp_generator.h" + +// moveit +#include <moveit_visual_tools/moveit_visual_tools.h> + +namespace moveit_grasps +{ +// Allow an interrupt to be called that waits for user input, useful for debugging +typedef boost::function<void(std::string message)> WaitForNextStepCallback; + +class GraspPlanner +{ +public: + /** + * \brief Constructor + */ + GraspPlanner(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools); + + /** + * \brief Plan entire cartesian manipulation sequence + * \param grasp_candidates - GraspCandidates for which we will compute apprach, lift and retreat paths + * \param robot_state - robot_state to be used for computeCartesianPath + * \param planning_scene_monitor - Current state of the world + * \param grasp_data - robot gripper configuration + * \param grasp_object_id - The name of the target grasp object to be used in the planning scene. + * \return true on success + */ + bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates, + const robot_state::RobotStatePtr& robot_state, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::string& grasp_object_id = ""); + + bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates, + const robot_state::RobotStatePtr& robot_state, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const std::string& grasp_object_id = ""); + + /** + * \brief Plan entire cartesian manipulation sequence + * \param input - description + * \return true on success + */ + bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + bool verbose_cartesian_filtering, const std::string& grasp_object_id = ""); + + bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state, + const planning_scene::PlanningSceneConstPtr& planning_scene, + bool verbose_cartesian_filtering, const std::string& grasp_object_id = ""); + + /** + * \brief Compute a cartesian path along waypoints + * \return true on success + */ + bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const moveit::core::RobotStatePtr& start_state, + const EigenSTL::vector_Isometry3d& waypoints, + const std::string& grasp_object_id = ""); + + bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const moveit::core::RobotStatePtr& start_state, + const EigenSTL::vector_Isometry3d& waypoints, + const std::string& grasp_object_id = ""); + + /** + * \brief Wait for user input to proceeed + * \param message - text to display to user when waiting + */ + void waitForNextStep(const std::string& message); + + /** + * \brief Allow an interrupt to be called that waits for user input, useful for debugging + * \param message - text to display to user when waiting + */ + // TODO(davetcoleman): Ideally all the classes in moveit_grasps would leverage this approach + void setWaitForNextStepCallback(WaitForNextStepCallback callback); + + /** + * \brief Load parameter settings of the server under ~/debug_level + * \param parent_name - only used for debugging, allows one to see what paremeters are loaded in what namespace + * \param setting_namespace - where on this node's namespace to load settings + * e.g. /this_name/setting_namespace/some_parameter + * \return true on success + */ + bool loadEnabledSettings(); + + /** + * \brief Check if a setting is enabled + * \param setting_name - name of key on the parameter server as loaded in the 'setting_namespace' + * \return true if setting is enabled + */ + bool isEnabled(const std::string& setting_name); + +private: + // A shared node handle + ros::NodeHandle nh_; + + // Class for publishing stuff to rviz + moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; + + WaitForNextStepCallback wait_for_next_step_callback_; + + // Visualization settings + bool enabled_settings_loaded_ = false; + std::map<std::string, bool> enabled_setting_; + +}; // end class + +// Create smart pointers for this class +typedef std::shared_ptr<GraspPlanner> GraspPlannerPtr; +typedef std::shared_ptr<const GraspPlanner> GraspPlannerConstPtr; + +} // namespace moveit_grasps + +#endif diff --git a/src/mtc/include/grasp_scorer.h b/src/mtc/include/grasp_scorer.h new file mode 100644 index 0000000000000000000000000000000000000000..c1b5f671ecdaa1c0799787ef439549911776db23 --- /dev/null +++ b/src/mtc/include/grasp_scorer.h @@ -0,0 +1,126 @@ +/********************************************************************* + * Software License Agreement ("Modified BSD License") + * + * Copyright (c) 2014, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/** + * Authors : Andy McEvoy + * Desc : Functions for scoring generated grasps + */ + +#ifndef MOVEIT_GRASPS__GRASP_SCORER_ +#define MOVEIT_GRASPS__GRASP_SCORER_ + +#include <cmath> + +#include <ros/ros.h> + +#include "../include/grasp_data.h" +#include <moveit_visual_tools/moveit_visual_tools.h> + +#include <Eigen/Core> +#include <Eigen/Geometry> + +namespace moveit_grasps +{ +struct GraspScoreWeights +{ + GraspScoreWeights() + : orientation_x_score_weight_(1.0) + , orientation_y_score_weight_(1.0) + , orientation_z_score_weight_(1.0) + , translation_x_score_weight_(1.0) + , translation_y_score_weight_(1.0) + , translation_z_score_weight_(1.0) + { + } + + /* \brief Compute the weighted score given the orientation and translation scores */ + double computeScore(const Eigen::Vector3d& orientation_scores, const Eigen::Vector3d& translation_scores, + bool verbose = false) const; + + /* \brief returns the sum of the grasp score weights*/ + virtual double getWeightTotal() const; + + double orientation_x_score_weight_; + double orientation_y_score_weight_; + double orientation_z_score_weight_; + double translation_x_score_weight_; + double translation_y_score_weight_; + double translation_z_score_weight_; +}; +// Create smart pointers for this struct +typedef std::shared_ptr<GraspScoreWeights> GraspScoreWeightsPtr; + +class GraspScorer +{ +public: + /** + * \brief Scores each axis of the grasp based on its angle to the desired pose axis. + * \param grasp_pose_tcp - the pose of the end effector + * \param ideal_pose - the ideal grasp pose (ex: straight into the bin) + * \return the unweighted scores: + * 1.0 -> 0 degrees between grasp axis and desired axis, + * 0.0 -> 180 degrees + */ + static Eigen::Vector3d scoreRotationsFromDesired(const Eigen::Isometry3d& grasp_pose_tcp, + const Eigen::Isometry3d& ideal_pose); + + /** + * \brief Score the grasp based on the translation values of the grasp pose + * \param grasp_pose_tcp - the pose of the end effector (not the eef mount) + * \param min_translations - the minimum translation values for all grasp poses + * \param max_translations - the maximum translation values for all grasp poses + * \return the unweighted scores: + * 0.0 -> pose is at the minimum translation in that axis + * 1.0 -> pose is at the maximum translation in that axis + */ + static Eigen::Vector3d scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp, + const Eigen::Vector3d& min_translations, + const Eigen::Vector3d& max_translations); + + /** + * \brief Score the grasp based on the translation values of the grasp pose + * \param grasp_pose_tcp - the pose of the end effector (not the eef mount) + * \param ideal_pose - the ideal pose location + * \param object_pose - the pose of the object being grasped + * \param object_size - the size of the object represented as a vector [x,y,z] + * \param visual_tools - set to a moveit_visual_tools pointer to enable visual debugging + * \return the unweighted scores: + * 0.0 -> pose is at the ideal translation in that axis + */ + static Eigen::Vector3d scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp, + const Eigen::Isometry3d& ideal_pose); +}; + +} // namespace moveit_grasps + +#endif diff --git a/src/mtc/include/impl/abstract_map_loader.h b/src/mtc/include/impl/abstract_map_loader.h new file mode 100644 index 0000000000000000000000000000000000000000..58939fdb81de67a21f8d94f80fdbabc8a9c6855a --- /dev/null +++ b/src/mtc/include/impl/abstract_map_loader.h @@ -0,0 +1,33 @@ +#ifndef ABSTRACT_MAP_LOADER_ +#define ABSTRACT_MAP_LOADER_ + +#include "ros/ros.h" +#include "geometry_msgs/Pose.h" +#include "geometry_msgs/Point.h" +#include <xmlrpcpp/XmlRpc.h> +#include <tf2/LinearMath/Transform.h> + +#include "impl/abstract_strategy.h" + + +class Abstract_map_loader{ + private: + std::vector<geometry_msgs::Pose> _inv_map; + std::vector<std::vector<geometry_msgs::Point>> _targets_grasps; + Abstract_strategy *_strategy; + + public: + Abstract_map_loader() = default; + ~Abstract_map_loader() = default; + + virtual void base_calculation()=0; + inline std::vector<geometry_msgs::Pose> get_inv_map() {return _inv_map;} + inline void set_inv_map(std::vector<geometry_msgs::Pose>& list) {_inv_map = list;} + inline void set_task(std::vector<std::vector<geometry_msgs::Point>>& lists_in_list) { _targets_grasps = lists_in_list;} + inline void set_strategy(Abstract_strategy* some_stratergy) {_strategy = some_stratergy;} + inline Abstract_strategy* get_strategy() {return _strategy;} + +}; + + +#endif diff --git a/src/mtc/include/impl/abstract_strategy.h b/src/mtc/include/impl/abstract_strategy.h new file mode 100644 index 0000000000000000000000000000000000000000..367fd55844322d21bf5d7e6a862f177cd3097a1f --- /dev/null +++ b/src/mtc/include/impl/abstract_strategy.h @@ -0,0 +1,12 @@ +#ifndef ABSTRACT_STRATEGY_ +#define ABSTRACT_STRATEGY_ + +#include <ros/ros.h> + +class Abstract_strategy { + public: + Abstract_strategy() = default; + ~Abstract_strategy() = default; + virtual void base_algorithm()=0; +}; +#endif \ No newline at end of file diff --git a/src/mtc/include/impl/base_by_rotation.h b/src/mtc/include/impl/base_by_rotation.h new file mode 100644 index 0000000000000000000000000000000000000000..2915cc105c49823621197fc368cf64afaf32ad9a --- /dev/null +++ b/src/mtc/include/impl/base_by_rotation.h @@ -0,0 +1,16 @@ +#ifndef BASE_BY_ROTATION_ +#define BASE_BY_ROTATION_ + +#include <ros/ros.h> + +#include "impl/abstract_strategy.h" + +class Base_by_rotation : public Abstract_strategy { + public: + Base_by_rotation()= default; + ~Base_by_rotation()= default; + + void base_algorithm() override; +}; + +#endif \ No newline at end of file diff --git a/src/mtc/include/impl/map_loader.h b/src/mtc/include/impl/map_loader.h new file mode 100644 index 0000000000000000000000000000000000000000..8489227d335ea88619bd17fa67cb30efee42071a --- /dev/null +++ b/src/mtc/include/impl/map_loader.h @@ -0,0 +1,18 @@ +#ifndef MAP_LOADER_ +#define MAP_LOADER_ + +#include "ros/ros.h" +#include "abstract_map_loader.h" +#include "geometry_msgs/Pose.h" +#include <regex> + + +class Map_loader : public Abstract_map_loader { + public: + Map_loader(XmlRpc::XmlRpcValue& map_data, XmlRpc::XmlRpcValue& target_data); + void base_calculation() override; + +}; + + +#endif \ No newline at end of file diff --git a/src/mtc/include/mtc2taskspace.h b/src/mtc/include/mtc2taskspace.h new file mode 100644 index 0000000000000000000000000000000000000000..af645f13e38a7d4bd29c52fbe38366ada9f65d9c --- /dev/null +++ b/src/mtc/include/mtc2taskspace.h @@ -0,0 +1,14 @@ +#ifndef MTC2TASKSPACE_H +#define MTC2TASKSPACE_H + +#include "ros/ros.h" +#include <ros/package.h> +#include "std_msgs/String.h" +#include <tf/transform_broadcaster.h> +#include <regex> +#include "yaml-cpp/yaml.h" +#include <xmlrpcpp/XmlRpc.h> +#include <fstream> + + +#endif diff --git a/src/mtc/include/state_validity_callback.h b/src/mtc/include/state_validity_callback.h new file mode 100644 index 0000000000000000000000000000000000000000..575beb89ad141215474389e0b5fc6dd78f65b9d7 --- /dev/null +++ b/src/mtc/include/state_validity_callback.h @@ -0,0 +1,87 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman <dave@picknik.ai> + Desc: Callback for checking if a state is in collision +*/ + +#ifndef MOVEIT_GRASPS__STATE_VALIDITY_CALLBACK +#define MOVEIT_GRASPS__STATE_VALIDITY_CALLBACK + +// Rviz +#include <moveit_visual_tools/moveit_visual_tools.h> + +// MoveIt +#include <moveit/planning_scene_monitor/planning_scene_monitor.h> +#include <moveit/robot_state/robot_state.h> + +namespace +{ +bool isGraspStateValid(const planning_scene::PlanningScene* planning_scene, bool visual_debug, double verbose_speed, + const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, + robot_state::RobotState* robot_state, const robot_state::JointModelGroup* group, + const double* ik_solution) +{ + robot_state->setJointGroupPositions(group, ik_solution); + robot_state->update(); + if (!robot_state->satisfiesBounds(group)) + { + ROS_DEBUG_STREAM_NAMED("is_grasp_state_valid", "Ik solution invalid"); + return false; + } + + if (!planning_scene) + { + ROS_ERROR_STREAM_NAMED("is_grasp_state_valid", "No planning scene provided"); + return false; + } + + if (!planning_scene->isStateColliding(*robot_state, group->getName())) + return true; // not in collision + + // Display more info about the collision + if (visual_debug && visual_tools) + { + visual_tools->publishRobotState(*robot_state, rviz_visual_tools::RED); + planning_scene->isStateColliding(*robot_state, group->getName(), true); + visual_tools->publishContactPoints(*robot_state, planning_scene); + visual_tools->trigger(); + ros::Duration(verbose_speed).sleep(); + } + return false; +} + +} // namespace + +#endif diff --git a/src/mtc/include/suction_voxel_matrix.h b/src/mtc/include/suction_voxel_matrix.h new file mode 100644 index 0000000000000000000000000000000000000000..9bb35a6a59657097cdb1bef148e70ff80e378fb7 --- /dev/null +++ b/src/mtc/include/suction_voxel_matrix.h @@ -0,0 +1,188 @@ +/* + * Software License Agreement (Modified BSD License) + * + * Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L. + * 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 Univ of CO, Boulder, PAL Robotics, S.L. + * nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Authors: Mike Lautman + Description: Data class used by grasp data. +*/ + +#ifndef MOVEIT_GRASPS__SUCTION_VOXEL_MATRIX_H_ +#define MOVEIT_GRASPS__SUCTION_VOXEL_MATRIX_H_ + +// ROS +#include <ros/ros.h> + +// Eigen +#include <Eigen/Core> +#include <Eigen/Geometry> + +namespace moveit_grasps +{ +struct SuctionVoxel +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /* \brief + @param center_point - position of voxel center point in tcp frame + @param x_width - width of voxel along x dim in tcp frame + @param y_width - width of voxel along y dim in tcp frame + */ + SuctionVoxel(const Eigen::Vector3d& center_point, double x_width, double y_width) + : center_point_(center_point), x_width_(x_width), y_width_(y_width) + { + top_left_ = center_point + Eigen::Vector3d(-x_width / 2.0, y_width / 2.0, 0); + top_right_ = center_point + Eigen::Vector3d(x_width / 2.0, y_width / 2.0, 0); + bottom_left_ = center_point + Eigen::Vector3d(-x_width / 2.0, -y_width / 2.0, 0); + bottom_right_ = center_point + Eigen::Vector3d(x_width / 2.0, -y_width / 2.0, 0); + } + + // Voxel center point in tcp frame + Eigen::Vector3d center_point_; + double x_width_; + double y_width_; + // Voxel corners in tcp frame + Eigen::Vector3d top_left_; + Eigen::Vector3d top_right_; + Eigen::Vector3d bottom_left_; + Eigen::Vector3d bottom_right_; +}; + +class SuctionVoxelMatrix +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + SuctionVoxelMatrix(double suction_rows_count, double suction_cols_count, double total_suction_range_y, + double total_suction_range_x) + : suction_rows_count_(suction_rows_count) + , suction_cols_count_(suction_cols_count) + , active_suction_range_x_(total_suction_range_x) + , active_suction_range_y_(total_suction_range_y) + { + voxel_x_width_ = active_suction_range_x_ / suction_cols_count_; + voxel_y_width_ = active_suction_range_y_ / suction_rows_count_; + suction_voxels_.resize(suction_rows_count_); + // We store the voxels starting bottom left and moving right then up + for (std::size_t voxel_y = 0; voxel_y < suction_rows_count_; ++voxel_y) + { + suction_voxels_[voxel_y].resize(suction_cols_count_); + for (std::size_t voxel_x = 0; voxel_x < suction_cols_count_; ++voxel_x) + { + suction_voxels_[voxel_y][voxel_x] = std::make_shared<SuctionVoxel>( + Eigen::Vector3d(-active_suction_range_x_ / 2.0 + voxel_x_width_ * (voxel_x + 0.5), + -active_suction_range_y_ / 2.0 + voxel_y_width_ * (voxel_y + 0.5), 0), + voxel_x_width_, voxel_y_width_); + } + } + } + + // \brief - get the voxel at the index location [row, col] with [0, 0] being the bottom left + bool getSuctionVoxel(std::size_t row, std::size_t col, std::shared_ptr<SuctionVoxel>& voxel) + { + if (row >= suction_rows_count_) + { + ROS_DEBUG_STREAM_NAMED("suction_voxel_matrix", "Invalid row " << row << "/" << suction_rows_count_ - 1); + return false; + } + + if (col >= suction_cols_count_) + { + ROS_DEBUG_STREAM_NAMED("suction_voxel_matrix", "Invalid col " << col << "/" << suction_cols_count_ - 1); + return false; + } + + voxel = suction_voxels_[row][col]; + return true; + } + + /** \brief Get the voxel at the index i where columns are the minor axis and rows are the major axes. + * @param index - the index of the suction voxel where index / #cols is the row and index % #cols is the col + * index 0 is bottom left + */ + bool getSuctionVoxel(std::size_t index, std::shared_ptr<SuctionVoxel>& voxel) + { + return getSuctionVoxel(index / suction_cols_count_, index % suction_cols_count_, voxel); + } + + std::size_t getNumRows() + { + return suction_rows_count_; + } + + std::size_t getNumCols() + { + return suction_cols_count_; + } + + std::size_t getNumVoxels() + { + return suction_cols_count_ * suction_rows_count_; + } + + double getVoxelArea() + { + return voxel_x_width_ * voxel_y_width_; + } + + double getVoxelWidthX() + { + return voxel_x_width_; + } + + double getVoxelWidthY() + { + return voxel_y_width_; + } + + double getActiveSuctionWidthX() + { + return active_suction_range_x_; + } + + double getActiveSuctionWidthY() + { + return active_suction_range_y_; + } + +protected: + std::size_t suction_rows_count_; + std::size_t suction_cols_count_; + double voxel_x_width_; + double voxel_y_width_; + double active_suction_range_x_; + double active_suction_range_y_; + std::vector<std::vector<std::shared_ptr<SuctionVoxel>>> suction_voxels_; +}; + +} // namespace moveit_grasps + +#endif diff --git a/src/mtc/include/two_finger_grasp_data.h b/src/mtc/include/two_finger_grasp_data.h new file mode 100644 index 0000000000000000000000000000000000000000..3b9a00f7493977e5c8bd2040b07eb83bdfdd6bfc --- /dev/null +++ b/src/mtc/include/two_finger_grasp_data.h @@ -0,0 +1,120 @@ +/* + * Software License Agreement (Modified BSD License) + * + * Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L. + * 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 Univ of CO, Boulder, PAL Robotics, S.L. + * nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Authors: Dave Coleman, Bence Magyar + Description: Data class used by the grasp generator. +*/ + +#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_DATA_H_ +#define MOVEIT_GRASPS__TWO_FINGER_GRASP_DATA_H_ + +// moveit grasps +#include "../include/grasp_data.h" + +namespace moveit_grasps +{ +MOVEIT_CLASS_FORWARD(TwoFingerGraspData); + +class TwoFingerGraspData : public GraspData +{ +public: + /** + * \brief Creates a two finger grasp data object + * \param node_handle - allows for namespacing + * \param end_effector name - which side of a two handed robot to load data for. should correspond to SRDF EE names + * \param robot_model - The robot model + */ + TwoFingerGraspData(const ros::NodeHandle& nh, const std::string& end_effector, + const moveit::core::RobotModelConstPtr& robot_model); + + /** + * \brief Helper function for constructor, loads grasp data from a yaml file (load from roslaunch) + * \param nh - node handle allows for namespacing + * \param end_effector - The end effector joint group name + * \return true on success + */ + bool loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector) override; + + /** + * \brief Set the width between fingers as a percentage of object size and max finger width + * \param fraction_open - [0,1] the 0->closed 1->open + * \return true on success + */ + bool setGraspWidth(double fraction_open, double min_finger_width, trajectory_msgs::JointTrajectory& grasp_posture); + + /** + * \brief Convert width between fingers to joint positions + * \param distance_btw_fingers - (meters) The target distance between the fingers + * \param grasp_posture - output. A joint trajectory with the values for the end effector filled in. + * Note: we interpolate to get the joint value assuming a linear relationship between min and max + * finger distance and min and max finger joint value. This could likely be improved on a per-robot basis. + * \param end_effector - The end effector joint group name + * \return true on success + */ + bool fingerWidthToGraspPosture(double distance_btw_fingers, trajectory_msgs::JointTrajectory& grasp_posture); + + /** + * \brief Convert joint positions to full grasp posture + * \param joint_positions - the full joint state as a vector of doubles + * \param grasp_posture - output. The full grasp posture + * \return true on success + */ + bool jointPositionsToGraspPosture(const std::vector<double>& joint_positions, + trajectory_msgs::JointTrajectory& grasp_posture); + + /** + * \brief Debug data to console + */ + void print() override; + +public: + ///////////////////////////////////// + // Finger gripper specific parameters + ///////////////////////////////////// + // Maximum allowed finger width for a grasp. + // This value should be considerably smaller than max_finger_width + // to allow padded collision checks + double max_grasp_width_; + // Maximum / Minimum distance between fingers + // For calculating the ratio between the distance between fingers and the joint values + double max_finger_width_; + double min_finger_width_; + // Parameter used to ensure generated grasps will overlap object + double gripper_finger_width_; +}; + +} // namespace moveit_grasps + +#endif diff --git a/src/mtc/include/two_finger_grasp_filter.h b/src/mtc/include/two_finger_grasp_filter.h new file mode 100644 index 0000000000000000000000000000000000000000..1ec06ce7ee6e92956ecb24422cbcaf2541ac64a1 --- /dev/null +++ b/src/mtc/include/two_finger_grasp_filter.h @@ -0,0 +1,81 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman <dave@picknik.ai> + Desc: Filters grasps based on kinematic feasibility and collision +*/ + +#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_FILTER_ +#define MOVEIT_GRASPS__TWO_FINGER_GRASP_FILTER_ + +// Parent class +#include "../include/grasp_filter.h" + +// Grasping +#include "../include/two_finger_grasp_generator.h" + +namespace moveit_grasps +{ +class TwoFingerGraspFilter : public GraspFilter +{ +public: + // Constructor + TwoFingerGraspFilter(const robot_state::RobotStatePtr& robot_state, + const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools); + + /** + * \brief Thread for checking part of the possible grasps list + */ + bool processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct) override; + + /** + * \brief Check if ik solution is in collision with fingers closed + * \return true on success + */ + bool checkFingersClosedIK(std::vector<double>& ik_solution, const IkThreadStructPtr& ik_thread_struct, + GraspCandidatePtr& grasp_candidate, + const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const; + +protected: + // Name for logging + const std::string name_; + +}; // end of class + +typedef std::shared_ptr<TwoFingerGraspFilter> TwoFingerGraspFilterPtr; +typedef std::shared_ptr<const TwoFingerGraspFilter> TwoFingerGraspFilterConstPtr; + +} // namespace moveit_grasps + +#endif diff --git a/src/mtc/include/two_finger_grasp_generator.h b/src/mtc/include/two_finger_grasp_generator.h new file mode 100644 index 0000000000000000000000000000000000000000..454510aac1cef921e5677c09f7b3d09773738389 --- /dev/null +++ b/src/mtc/include/two_finger_grasp_generator.h @@ -0,0 +1,257 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy + Desc: Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches +*/ + +#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_GENERATOR_H_ +#define MOVEIT_GRASPS__TWO_FINGER_GRASP_GENERATOR_H_ + +#include "../include/grasp_generator.h" +#include "../include/grasp_candidate.h" +#include "../include/two_finger_grasp_scorer.h" +#include "../include/two_finger_grasp_data.h" + +// Testing +#include <gtest/gtest.h> + +namespace moveit_grasps +{ +struct TwoFingerGraspCandidateConfig +{ + TwoFingerGraspCandidateConfig(); + void enableAllGraspTypes(); + void enableAllGraspAxes(); + void enableAll(); + void disableAllGraspTypes(); + void disableAllGraspAxes(); + void disableAll(); + + /////////////////////////////// + // Finger Gripper config values + /////////////////////////////// + bool enable_corner_grasps_; + bool enable_face_grasps_; + bool enable_variable_angle_grasps_; + bool enable_edge_grasps_; + bool generate_x_axis_grasps_; + bool generate_y_axis_grasps_; + bool generate_z_axis_grasps_; +}; + +class TwoFingerGraspGenerator : public GraspGenerator +{ +public: + // Eigen requires 128-bit alignment for the Eigen::Vector2d's array (of 2 doubles). + // With GCC, this is done with a attribute ((aligned(16))). + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * \brief Constructor + */ + TwoFingerGraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, bool verbose = false); + + /* brief sets internal grasp_candidate_config_ variable + * \param grasp_candidate_config - a config describing the grasps to be generated + */ + void setGraspCandidateConfig(const TwoFingerGraspCandidateConfig& grasp_candidate_config); + + /** + * \brief Create possible grasp positions around a cuboid + * \param cuboid_pose - centroid of object to grasp in world frame + * \param depth length of cuboid along local x-axis + * \param width length of cuboid along local y-axis + * \param height length of cuboid along local z-axis + * \param grasp_data data describing end effector + * \param grasp_candidate_config parameter for selectively enabling and disabling different grasp types + * \param grasp_candidates possible grasps generated + * \return true if successful + */ + bool generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height, + const GraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates) override; + + bool generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height, + const TwoFingerGraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates); + + /** + * \brief Setter for grasp score weights + */ + void setGraspScoreWeights(const TwoFingerGraspScoreWeightsPtr& grasp_score_weights) + { + auto two_finger_grasp_score_weights = std::make_shared<TwoFingerGraspScoreWeights>(*grasp_score_weights); + grasp_score_weights_ = std::dynamic_pointer_cast<GraspScoreWeights>(two_finger_grasp_score_weights); + } + + /** + * \brief Setter for grasp score weights + */ + const TwoFingerGraspScoreWeightsPtr getGraspScoreWeights() + { + return std::dynamic_pointer_cast<TwoFingerGraspScoreWeights>(grasp_score_weights_); + } + +protected: + /** + * \brief creates grasp messages from the generated grasp poses + * \param grasp_pose_eef_mount - the grasp pose. (Note: this is the pose of the eef mount not the position of the tcp) + * \param grasp_data data describing the end effector + * \param object_pose - pose of object to grasp + * \param object_size - size of object to grasp + * \param object_width - In the case of finger grippers, the width of the object in the dimension betwen the fingers + * \param grasp_candidates - list possible grasps with new grasp appended + * \return true on success + */ + bool addGrasp(const Eigen::Isometry3d& grasp_pose_eef_mount, const TwoFingerGraspDataPtr& grasp_data, + const Eigen::Isometry3d& object_pose, const Eigen::Vector3d& object_size, double object_width, + std::vector<GraspCandidatePtr>& grasp_candidates); + + /** + * \brief Create grasp positions around one axis of a cuboid + * \param cuboid_pose: centroid of object to grasp in world frame + * \param depth: length of cuboid along local x-axis + * \param width: length of cuboid along local y-axis + * \param height: length of cuboid along local z-axis + * \param axis: axis of cuboid to generate grasps along + * \param grasp_data: data describing end effector + * \param grasp_candidates: possible grasps generated + * \param only_edge_grasps: set to true if object is too wide to grap the face in this axis + * \return true if successful + */ + bool generateCuboidAxisGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height, + grasp_axis_t axis, const TwoFingerGraspDataPtr& grasp_data, + const TwoFingerGraspCandidateConfig& grasp_candidate_config, + std::vector<GraspCandidatePtr>& grasp_candidates); + + /** + * \brief helper function for adding grasps at corner of cuboid + * \param pose - pose of the object to grasp + * \param rotation_angles - rotation angles to go from cuboid pose to standard grasping pose + * \param translation - translation to go from cuboid centroid to grasping location + * \param corner_rotation - extra rotatation needed to align grasp pose as you move around the cuboid + * \param num_radial_grasps - the number of grasps to generate around the corner + * \param grasp_poses_tcp - list of grasp poses generated + * \return the number of poses generated + */ + std::size_t addCornerGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3], + const Eigen::Vector3d& translation, double corner_rotation, + std::size_t num_radial_grasps, EigenSTL::vector_Isometry3d& grasp_poses_tcp); + + /** + * \brief helper function for adding grasps along the face of a cuboid + * \param pose - pose of the object to grasp + * \param rotation_angles - rotation angles to go from cuboid pose to standard grasping pose + * \param delta - distance to move away from cuboid at each step + * \param translation - translation to go from cuboid centroid to grasping location + * \param alignment_rotation - extra rotatation needed to align grasp pose as you move around the cuboid + * \param num_grasps - the number of grasps to generate around the corner + * \param grasp_poses_tcp - list of grasp poses generated + * \return the number of poses generated + */ + std::size_t addFaceGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3], + const Eigen::Vector3d& translation, const Eigen::Vector3d& delta, + double alignment_rotation, std::size_t num_grasps, + EigenSTL::vector_Isometry3d& grasp_poses_tcp); + + /** + * \brief helper function for adding grasps along the edges of the cuboid + * \param pose - pose of the object to grasp + * \param rotation_angles - rotation angles to go from cuboid pose to standard grasping pose + * \param delta - distance to move away from cuboid at each step + * \param translation - translation to go from cuboid centroid to grasping location + * \param alignment_rotation - extra rotatation needed to align grasp pose as you move around the cuboid + * \param num_grasps - the number of grasps to generate around the corner + * \param grasp_poses_tcp - list of grasp poses generated + * \return the number of poses generated + */ + std::size_t addEdgeGraspsHelper(const Eigen::Isometry3d& cuboid_pose, double rotation_angles[3], + const Eigen::Vector3d& translation, const Eigen::Vector3d& delta, + double alignment_rotation, std::size_t num_grasps, + EigenSTL::vector_Isometry3d& grasp_poses_tcp, double corner_rotation); + + /** + * \brief helper function for determining if the grasp will intersect the cuboid + * \param cuboid_pose - centroid of object to grasp in world frame + * \param depth - size of cuboid along x axis + * \param width - size of cuboid along y axis + * \param height - size of cuboid along z axis + * \param grasp_pose_tcp - pose of grasp + * \param grasp_data - data describing end effector + * \return true if the grasp intersects the cuboid + */ + bool graspIntersectionHelper(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height, + const Eigen::Isometry3d& grasp_pose_tcp, const TwoFingerGraspDataPtr& grasp_data); + + /** + * \brief helper function to test intersection of a line with a plane + * \param t - parametric distance along grasp line + * \param u1, v1, u2, v2 - (u,v) coordinates of the line + * \param a, b - length and width of the plane area in which to test for intersection + * \param u, v - location of intersection + * \return true if the line intersects the plane + */ + bool intersectionHelper(double t, double u1, double v1, double u2, double v2, double a, double b, double& u, + double& v); + + /** + * \brief Score the generated finger grasp poses + * \param grasp_pose_tcp - the grasp pose of the tcp + * \param grasp_data - data describing the end effector + * \param object_pose - the pose of the object being grasped + * \param percent_open - percentage that the grippers are open. 0.0 -> grippers are at object width + padding + * \return a score with positive being better + */ + double scoreFingerGrasp(const Eigen::Isometry3d& grasp_pose_tcp, const TwoFingerGraspDataPtr& grasp_data, + const Eigen::Isometry3d& object_pose, double percent_open); + bool + generateFingerGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height, + const TwoFingerGraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates, + const TwoFingerGraspCandidateConfig& grasp_candidate_config = TwoFingerGraspCandidateConfig()); + +protected: + TwoFingerGraspCandidateConfig grasp_candidate_config_; + + // Tests + FRIEND_TEST(TwoFingerGraspGeneratorTest, GenerateFaceGrasps); + FRIEND_TEST(TwoFingerGraspGeneratorTest, GenerateEdgeGrasps); + FRIEND_TEST(TwoFingerGraspGeneratorTest, GenerateCornerGrasps); + +}; // end of class + +typedef std::shared_ptr<TwoFingerGraspGenerator> TwoFingerGraspGeneratorPtr; +typedef std::shared_ptr<const TwoFingerGraspGenerator> TwoFingerGraspGeneratorConstPtr; + +} // namespace moveit_grasps + +#endif diff --git a/src/mtc/include/two_finger_grasp_scorer.h b/src/mtc/include/two_finger_grasp_scorer.h new file mode 100644 index 0000000000000000000000000000000000000000..629cc9d55696a13240b7ffa58ea92c3398597995 --- /dev/null +++ b/src/mtc/include/two_finger_grasp_scorer.h @@ -0,0 +1,104 @@ +/********************************************************************* + * Software License Agreement ("Modified BSD License") + * + * Copyright (c) 2014, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/** + * Authors : Andy McEvoy + * Desc : Functions for scoring generated grasps + */ + +#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_SCORER_ +#define MOVEIT_GRASPS__TWO_FINGER_GRASP_SCORER_ + +#include "../include/two_finger_grasp_data.h" +#include "../include/grasp_scorer.h" + +namespace moveit_grasps +{ +struct TwoFingerGraspScoreWeights : public GraspScoreWeights +{ + TwoFingerGraspScoreWeights() : GraspScoreWeights(), depth_score_weight_(1.0), width_score_weight_(1.0) + { + } + + /* \brief Compute the weighted score given the orientation and translation scores */ + double computeScore(const Eigen::Vector3d& orientation_scores, const Eigen::Vector3d& translation_scores, + double depth_score, double width_score, bool verbose = false) const; + + /* \brief returns the sum of the grasp score weights*/ + double getWeightTotal() const override; + + // Finger gripper specific weights + double depth_score_weight_; + double width_score_weight_; +}; +// Create smart pointers for this class +typedef std::shared_ptr<TwoFingerGraspScoreWeights> TwoFingerGraspScoreWeightsPtr; + +class TwoFingerGraspScorer : public GraspScorer +{ +public: + /** + * \brief Scores the grasp on how wide the fingers are on approach, the more open the better + * \param grasp_data - pointer to grasp info + * \param percent_open - amount the gripper is open + * 0.0 -> gripper is open to the object width + minimum padding + * 1.0 -> gripper is in full open position + * \return the unweighted score: + * 1.0 -> gripper is wide open, + * 0.0 -> gripper is at minimum position. + */ + static double scoreGraspWidth(const TwoFingerGraspDataPtr& grasp_data, double percent_open); + + /** + * \brief Score the grasp based on how far the object is from the palm of the hand + * \param grasp_pose_tcpgrasp_pose_tcp - the pose of the end effector (not the eef mount) + * \param grasp_data - pointer to grasp info + * \param object_pose - the pose of the object being grasped + * \param max_grasp_distance - the maximum acceptable distance from palm + * \return the unweighted score: + * 1.0 -> object pose and grasp pose have same translation values + * 0.0 -> object is at max distanct + * < 0.0 -> object is beyond the max_grasp_distance + */ + // DEV NOTE: when this function is called we've lost the references to the acutal size of the object. + // max_distance should be the length of the fingers minus some minimum amount that the fingers need to grip an object + // since we don't know the distance from the centoid of the object to the edge of the object, this is set as an + // arbitrary number given our target object set (i.e. I based it off of the cheese it box) + static double scoreDistanceToPalm(const Eigen::Isometry3d& grasp_pose_tcp, const TwoFingerGraspDataPtr& grasp_data, + const Eigen::Isometry3d& object_pose, double min_grasp_distance, + double max_grasp_distance); +}; + +} // namespace moveit_grasps + +#endif diff --git a/src/mtc/launch/base_routine.launch b/src/mtc/launch/base_routine.launch new file mode 100644 index 0000000000000000000000000000000000000000..a021de08a8fb9410e8753334f40ecf5e16779687 --- /dev/null +++ b/src/mtc/launch/base_routine.launch @@ -0,0 +1,6 @@ +<launch> + <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/> + <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/> + + <node pkg="mtc" type="base_routine" name="base_routine" output="screen" required="true"/> +</launch> \ No newline at end of file diff --git a/src/mtc/launch/gdb_settings.gdb b/src/mtc/launch/gdb_settings.gdb new file mode 100644 index 0000000000000000000000000000000000000000..698fff8efb266732eff05bc9b707f16333ca24fc --- /dev/null +++ b/src/mtc/launch/gdb_settings.gdb @@ -0,0 +1,8 @@ +## This file allows developers to set breakpoints in the CPP_EXECUTABLE_NAME.launch file +## To set a breakpoint, compile with debug flag set to true. Add a line below with the source file name, +## a colon, then the line number to break on. Then launch CPP_EXECUTABLE_NAME with argument debug:=true + +set breakpoint pending on + +## Example break point: +# break moveit_visual_tools.cpp:322 diff --git a/src/mtc/launch/grasp_pipeline.launch b/src/mtc/launch/grasp_pipeline.launch new file mode 100644 index 0000000000000000000000000000000000000000..c5019dc44dd230118356336696ce87ee62016623 --- /dev/null +++ b/src/mtc/launch/grasp_pipeline.launch @@ -0,0 +1,49 @@ +<launch> + <!-- Launch rviz.launch before running this demo to see visalizations --> + + <!-- Suction or two finger --> + <arg name="gripper" default="two_finger" doc="must be one of 'suction' or 'two_finger'" /> + + <!-- GDB Debug Arguments --> + <arg name="debug" default="false" /> + <arg unless="$(arg debug)" name="launch_prefix" value="" /> + <arg if="$(arg debug)" name="launch_prefix" + value="gdb -x $(find mtc)/launch/gdb_settings.gdb --ex run --args" /> + + <!-- Verbose mode --> + <arg name="verbose" default="false" /> + <arg unless="$(arg verbose)" name="command_args" value="" /> + <arg if="$(arg verbose)" name="command_args" value="--verbose" /> + + <!-- Callgrind Arguments --> + <arg name="callgrind" default="false" /> + <arg unless="$(arg callgrind)" name="launch_prefix2" value="" /> + <arg if="$(arg callgrind)" name="launch_prefix2" value="valgrind --tool=callgrind --collect-atstart=no" /> + + <!-- Valgrind Arguments --> + <arg name="valgrind" default="false" /> + <arg unless="$(arg valgrind)" name="launch_prefix3" value="" /> + <arg if="$(arg valgrind)" name="launch_prefix3" value="valgrind --tool=memcheck --leak-check=full --show-leak-kinds=all --track-origins=yes -v" /> + + <!-- PANDA --> + <include file="$(find mtc)/launch/load_panda.launch"> + <arg name="gripper" value="$(arg gripper)"/> + </include> + + <!-- Start the demo --> + <node name="moveit_grasps" launch-prefix="$(arg launch_prefix) $(arg launch_prefix2) $(arg launch_prefix3)" pkg="mtc" + type="moveit_grasps_grasp_pipeline" output="screen" args="$(arg command_args)"> + <param name="gripper" value="$(arg gripper)"/> + <param name="ee_group_name" value="hand"/> + <param name="planning_group_name" value="panda_arm"/> + <rosparam command="load" file="$(find mtc)/config/panda_grasp_data.yaml"/> + <rosparam command="load" file="$(find mtc)/config/moveit_grasps_config.yaml"/> + </node> + + <!-- Planning Functionality --> + <arg name="planner" default="ompl" /> + <include ns="moveit_grasps" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> + <arg name="pipeline" value="$(arg planner)" /> + </include> + +</launch> diff --git a/src/mtc/launch/load_panda.launch b/src/mtc/launch/load_panda.launch new file mode 100644 index 0000000000000000000000000000000000000000..2f2cac409a279f48a63f8d57cefccff35a638ae2 --- /dev/null +++ b/src/mtc/launch/load_panda.launch @@ -0,0 +1,22 @@ +<launch> + + <!-- Suction or two finger --> + <arg name="gripper" default="suction" doc="must be one of 'suction' or 'two_finger'" /> + + <!-- PANDA --> + <include file="$(find mtc)/launch/panda_planning_context.launch"> + <arg name="load_robot_description" value="true"/> + <arg name="gripper" value="$(arg gripper)"/> + </include> + + <!-- We do not have a robot connected, so publish fake joint states --> + <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"> + <param name="/use_gui" value="false"/> + <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam> + </node> + <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" /> + + <!-- Given the published joint states, publish tf for the robot links --> + <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" /> + +</launch> diff --git a/src/mtc/launch/mtc2taskspace.launch b/src/mtc/launch/mtc2taskspace.launch new file mode 100644 index 0000000000000000000000000000000000000000..c165f181ab10ac9af290c6f0d27a68518a921616 --- /dev/null +++ b/src/mtc/launch/mtc2taskspace.launch @@ -0,0 +1,4 @@ +<launch> + <rosparam command="load" file="$(find mtc)/resources/dummy.yaml"/> + <node pkg="mtc" type="mtc2taskspace" name="mtc2taskspace" output="screen" required="true"/> +</launch> \ No newline at end of file diff --git a/src/mtc/launch/panda_planning_context.launch b/src/mtc/launch/panda_planning_context.launch new file mode 100644 index 0000000000000000000000000000000000000000..bc394dca652f570c3087102af44d444142713c22 --- /dev/null +++ b/src/mtc/launch/panda_planning_context.launch @@ -0,0 +1,31 @@ +<launch> + <!-- Suction or two finger --> + <arg name="gripper" default="two_finger" doc="must be one of 'suction' or 'two_finger'" /> + + <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior --> + <arg name="load_robot_description" default="false"/> + + <!-- The name of the parameter under which the URDF is loaded --> + <arg name="robot_description" default="robot_description"/> + + <!-- Should the panda be loaded with the two finger gripper or the suction gripper --> + + <!-- Load universal robot description format (URDF) --> + <param if="$(eval arg('load_robot_description') and arg('gripper')=='two_finger')" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find moveit_grasps)/robots/panda_arm_two_finger_hand.urdf.xacro'"/> + <param if="$(eval arg('load_robot_description') and arg('gripper')=='suction')" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find moveit_grasps)/robots/panda_arm_suction_hand.urdf.xacro'"/> + + <!-- The semantic description that corresponds to the URDF --> + <param if="$(eval arg('gripper')=='two_finger')" name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find moveit_grasps)/robots/panda_arm_two_finger_hand.srdf.xacro'" /> + <param if="$(eval arg('gripper')=='suction')" name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find moveit_grasps)/robots/panda_arm_suction_hand.srdf.xacro'" /> + + <!-- Load updated joint limits (override information from URDF) --> + <group ns="$(arg robot_description)_planning"> + <rosparam command="load" file="$(find panda_moveit_config)/config/joint_limits.yaml"/> + </group> + + <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace --> + <group ns="$(arg robot_description)_kinematics"> + <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/> + </group> + +</launch> diff --git a/src/mtc/launch/rviz.launch b/src/mtc/launch/rviz.launch new file mode 100644 index 0000000000000000000000000000000000000000..7d8dcc957904118ab3e96aef3c8c81d3651068f9 --- /dev/null +++ b/src/mtc/launch/rviz.launch @@ -0,0 +1,25 @@ +<launch> + <!-- Debug --> + <arg name="debug" default="false" /> + <arg unless="$(arg debug)" name="launch_prefix" value="" /> + <arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" /> + + <!-- Suction or two finger --> + <arg name="gripper" default="two_finger" doc="must be one of 'suction' or 'two_finger'" /> + + <!-- Publish world frame --> + <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world panda_link0" /> + + <!-- PANDA --> + <include file="$(find panda_moveit_config)/launch/planning_context.launch"> + <arg name="load_robot_description" value="true"/> + <arg name="GOAL" value="0,0,0"/> + + </include> + + <!-- Load Rviz --> + <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" + args="-d $(find mtc)/launch/rviz/grasps.rviz" output="screen"> + </node> + +</launch> diff --git a/src/mtc/launch/rviz/grasps.rviz b/src/mtc/launch/rviz/grasps.rviz new file mode 100644 index 0000000000000000000000000000000000000000..be0d012ee8d190bb8560f47c27646da12163c621 --- /dev/null +++ b/src/mtc/launch/rviz/grasps.rviz @@ -0,0 +1,386 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /MarkerArray1/Namespaces1 + Splitter Ratio: 0.5704230070114136 + Tree Height: 545 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: rviz_visual_tools/RvizVisualToolsGui + Name: RvizVisualToolsGui +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: <Fixed Frame> + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + 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 + tool_center_point: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /rviz_visual_tools + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /grasp_visuals + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Attached Body Color: 150; 50; 150 + Class: moveit_rviz_plugin/RobotState + Collision Enabled: false + Enabled: true + 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 + tool_center_point: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotState + Robot Alpha: 1 + Robot Description: robot_description + Robot State Topic: /display_robot_state + Show All Links: true + Show Highlights: true + Value: true + Visual Enabled: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /moveit_grasps_demo/grasping_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body 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 + tool_center_point: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + - Class: rviz_visual_tools/KeyTool + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.91066575050354 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.6259773373603821 + Y: 0.3534509539604187 + Z: 0.4344802796840668 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.14539791643619537 + Target Frame: <Fixed Frame> + Value: Orbit (rviz) + Yaw: 4.575382709503174 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 845 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000164000002acfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073020000042c000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ac000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000220000001c00000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100ffffff000000010000010f000003f1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000003f1000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005fd00000041fc0100000003fb00000024005200760069007a00560069007300750061006c0054006f006f006c00730047007500690100000000000005fd0000016400fffffffb0000000800540069006d0065000000000000000638000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000493000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RvizVisualToolsGui: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1533 + X: 67 + Y: 27 diff --git a/src/mtc/maps/dummy.yaml b/src/mtc/maps/dummy.yaml new file mode 100644 index 0000000000000000000000000000000000000000..85ae62136502cfe89287c6002d65cd821866edab --- /dev/null +++ b/src/mtc/maps/dummy.yaml @@ -0,0 +1,44 @@ +data: + - -0.704647 -0.085013 0.103821 -0.063726 -0.039416 -0.249854 0.965380 + - -0.513205 -0.514270 0.103184 0.092404 -0.010381 -0.590406 0.801732 + - -0.499355 -0.290624 0.104579 -0.041985 -0.015622 -0.411819 0.910164 + - -0.459296 -0.182886 0.049981 -0.221514 0.461441 -0.859037 0.007668 + - -0.519670 0.093757 0.102952 0.057347 -0.080399 -0.305704 0.946991 + - -0.499105 0.404992 0.000930 -0.557591 0.429723 0.428737 0.566228 + - -0.489862 0.521042 0.102369 -0.020562 0.110022 0.800758 0.588437 + - -0.294706 -0.580002 0.067799 0.390187 -0.157800 0.394885 0.816651 + - -0.322003 -0.302647 0.102635 0.061764 -0.086307 -0.477962 0.871946 + - -0.297281 -0.032005 0.079964 -0.311869 -0.148179 -0.436303 0.830916 + - -0.323814 0.102339 0.102237 0.017354 -0.113384 -0.245576 0.962567 + - -0.221989 0.339610 0.058054 -0.434513 -0.186412 -0.879328 0.056848 + - -0.282236 0.566463 -0.079323 -0.842237 0.410323 0.066782 0.343238 + - -0.045863 -0.717152 0.088317 -0.256995 0.115736 -0.952974 0.111348 + - -0.017530 -0.503598 0.064890 -0.243688 0.362787 -0.533595 0.724071 + - -0.087573 -0.290814 0.103857 -0.066561 0.031855 -0.467458 0.880930 + - -0.162281 0.379039 0.029983 -0.367444 -0.471388 -0.191603 0.778503 + - -0.001871 0.499820 0.037360 -0.225484 0.520819 -0.328508 0.754975 + - 0.000220 0.731249 -0.002125 0.004754 0.714210 0.212787 0.666785 + - 0.097747 -0.713925 0.104048 0.058142 0.033945 -0.634190 0.770241 + - 0.000591 -0.480898 -0.027892 -0.540596 -0.583588 0.320502 0.514257 + - 0.177063 -0.326099 0.066371 -0.179291 0.389617 -0.620917 0.656137 + - 0.071622 -0.165081 0.077357 0.042348 -0.360331 0.804873 0.469626 + - 0.175867 0.259083 -0.059958 -0.622893 0.630490 -0.442872 0.135467 + - 0.071068 0.437589 0.079327 0.239442 0.254795 -0.889244 0.294944 + - 0.252831 -0.566984 0.065675 0.121811 -0.415237 0.561186 0.705557 + - 0.295272 -0.292902 0.104653 0.015052 -0.037756 -0.977591 0.206554 + - 0.284065 -0.076036 0.100979 -0.071881 -0.118235 -0.419818 0.896999 + - 0.382397 0.139887 0.051428 -0.304978 0.402606 -0.169316 0.846303 + - 0.238413 0.384767 0.006826 -0.655008 -0.196111 0.241573 0.688584 + - 0.237968 0.455913 0.072342 0.158007 -0.361317 0.187635 0.899598 + - 0.582562 -0.238750 0.021376 -0.629042 0.050133 -0.584335 0.510240 + - 0.515243 -0.097356 0.103854 -0.073570 0.006677 -0.963172 0.258540 + - 0.548987 0.125685 0.089250 -0.243039 0.126222 -0.550080 0.788930 + - 0.526708 0.200380 -0.019687 0.363156 -0.679607 0.620766 0.144572 + - 0.545807 0.477673 0.091806 -0.188434 0.165296 -0.933886 0.255002 + - 0.698731 -0.108383 0.104657 0.037677 0.014594 -0.496260 0.867233 + - 0.688405 0.103068 0.104313 0.013921 -0.055488 -0.482543 0.874002 +description: + system: Franka Emika 'Panda' + resolution: 0.2 + dimension: 0.900000 0.900000 0.000000 (m) + timestamp: 1712.052411 sec \ No newline at end of file diff --git a/src/mtc/package.xml b/src/mtc/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..1213557c52d3af5afd9af5b3363c849eb40c63af --- /dev/null +++ b/src/mtc/package.xml @@ -0,0 +1,54 @@ +<package> + <name>mtc</name> + <version>2.0.0</version> + <description> + A basic grasp generator for objects such as blocks or cylinders for use with the MoveIt! pick and place pipeline. + Does not consider friction cones or other dynamics. + </description> + <maintainer email="davetcoleman@gmail.com">Dave Coleman</maintainer> + <maintainer email="mcevoy.andy@gmail.com">Andy McEvoy</maintainer> + + <license>BSD</license> + + <url type="website">https://github.com/PickNikRobotics/moveit_grasps/</url> + <url type="bugtracker">https://github.com/PickNikRobotics/moveit_grasps/issues</url> + <url type="repository">https://github.com/PickNikRobotics/moveit_grasps</url> + + <author email="davetcoleman@gmail.com">Dave Coleman</author> + <author email="mcevoy.andy@gmail.com">Andy McEvoy</author> + + <buildtool_depend>catkin</buildtool_depend> + + <build_depend>roscpp</build_depend> + <build_depend>roslint</build_depend> + <build_depend>eigen_conversions</build_depend> + <build_depend>std_msgs</build_depend> + <build_depend>trajectory_msgs</build_depend> + <build_depend>tf</build_depend> + <build_depend>tf_conversions</build_depend> + <build_depend>moveit_ros_planning</build_depend> + <build_depend>moveit_ros_planning_interface</build_depend> + <build_depend>moveit_core</build_depend> + <build_depend>moveit_visual_tools</build_depend> + <build_depend>rosparam_shortcuts</build_depend> + <build_depend>message_generation</build_depend> + <build_depend>moveit_msgs</build_depend> + <build_depend>geometry_msgs</build_depend> + <build_depend>YAML_CPP</build_depend> + + <run_depend>YAML_CPP</run_depend> + + <run_depend>std_msgs</run_depend> + <run_depend>trajectory_msgs</run_depend> + <run_depend>moveit_msgs</run_depend> + <run_depend>geometry_msgs</run_depend> + <run_depend>message_runtime</run_depend> + <run_depend>moveit_visual_tools</run_depend> + <run_depend>rosparam_shortcuts</run_depend> + + <!-- The export tag contains other, unspecified, tags --> + <export> + <!-- Other tools can request additional information be placed here --> + + </export> +</package> diff --git a/src/mtc/resources/dummy.yaml b/src/mtc/resources/dummy.yaml new file mode 100644 index 0000000000000000000000000000000000000000..0cfc174df5feb31252e335ae3167d2c08333979a --- /dev/null +++ b/src/mtc/resources/dummy.yaml @@ -0,0 +1,140 @@ +# create json file using +# yq eval -o=json config_scene_ceti-table-placeworld.yaml > config_scene_ceti-table-placeworld.json +{ 'objects': [ + # table + # height: 12 cm wheels, 76 cm body, 1 cm table-top = 89 cm total + # wheels width/depth/height 12*12*12 + # body width/depth/height 69*69*76 + # table-top width/depth/height 80*80*1 + # panels are 50*70, with 0.5 cm distance to the table-top + # height of both is 12 + 76 + .5 = 88.5 + # their centre is at 40 + 0.25 + 25 = 65.25 + # a filled table and a backplate going all the way up is not possible, because of the oversized collision model of the robot + # { 'id': 'table_body','pos': { 'z': 0.50 },'size': { 'length': .69,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table1_wheel_1', 'pos': { 'x': 0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table1_wheel_2', 'pos': { 'x': -0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table1_wheel_3', 'pos': { 'x': -0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table1_wheel_4', 'pos': { 'x': 0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table1_body_front', 'pos': { 'x': .345, 'z': 0.50 },'size': { 'length': 0.005,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table1_body_back', 'pos': { 'x': -.345, 'z': 0.45 },'size': { 'length': 0.005,'width': 0.69,'height': 0.66 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table1_body_left', 'pos': { 'y': .345, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table1_body_right', 'pos': { 'y': -.345, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table1_table_top', 'pos': { 'z': 0.885 },'size': { 'length': 0.8,'width': 0.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table1_left_panel', 'pos': { 'z': 0.885, 'y': -0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table1_front_panel', 'pos': { 'z': 0.885, 'x': 0.6525 },'size': { 'length': 0.5,'width': 0.7,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + + { 'id': 'shared_panel', 'pos': { 'z': 0.885, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + + # table 2 is shifted in y direction by the size of the base, the size of a side panel and two 5mm spaces: + # 0.8 + 0.5 + 2*0.05 = 1.31 + + { 'id': 'table2_wheel_1', 'pos': { 'x': 0.28,'y': 1.59,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table2_wheel_2', 'pos': { 'x': -0.28,'y': 1.03,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table2_wheel_3', 'pos': { 'x': -0.28,'y': 1.59,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table2_wheel_4', 'pos': { 'x': 0.28,'y': 1.03,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table2_body_front', 'pos': { 'x': .345, y: 1.31, 'z': 0.50 },'size': { 'length': 0.005,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table2_body_back', 'pos': { 'x': -.345, y: 1.31, 'z': 0.45 },'size': { 'length': 0.005,'width': 0.69,'height': 0.66 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table2_body_left', 'pos': { 'y': 1.655, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table2_body_right', 'pos': { 'y': 0.965, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table2_table_top', 'pos': { 'y': 1.31, 'z': 0.885 },'size': { 'length': 0.8,'width': 0.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table2_front_panel', 'pos': { 'x': 0.6525, 'y': 1.31, 'z': 0.885 },'size': { 'length': 0.5,'width': 0.7,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table2_right_panel', 'pos': { 'z': 0.885, 'y': 1.9625 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + + + { 'id': 'A1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'A2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'A3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'A4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + + { 'id': 'B1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'B2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, +# { 'id': 'B3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, +# { 'id': 'B4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + + { 'id': 'C1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': -.3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'C2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': -.2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'C3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': -.1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + { 'id': 'C4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': -.3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'C5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': -.2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'C6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': -.1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + { 'id': 'C7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': -.3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'C8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': -.2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'C9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': -.1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + { 'id': 'D1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': .1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'D2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': .2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'D3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': .3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + { 'id': 'D4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': .1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'D5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': .2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'D6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': .3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + { 'id': 'D7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': .1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'D8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': .2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'D9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': .3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + { 'id': 'E1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'E2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'E3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'E4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + + { 'id': 'F1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'F2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, +# { 'id': 'F3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, +# { 'id': 'F4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + + { 'id': 'G1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'G2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.11, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'G3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.21, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + { 'id': 'G4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'G5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.11, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'G6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.21, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + { 'id': 'G7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': 1.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'G8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': 1.11, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'G9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': 1.21, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + { 'id': 'H1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.41, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'H2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.51, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'H3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.61, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + { 'id': 'H4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.41, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'H5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.51, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'H6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.61, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + { 'id': 'H7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': 1.41, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'H8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': 1.51, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'H9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .3, 'y': 1.61, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + { 'id': 'I1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': 1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'I2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': 2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'I3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': 1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'I4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': 2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + + { 'id': 'J1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + { 'id': 'J2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .1, 'y': 2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, +# { 'id': 'J3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, +# { 'id': 'J4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': .2, 'y': 2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } }, + + # the collaboration zone has height 120cm + { 'id': 'cz1', 'type': 'COLLABORATION_ZONE', 'pos': { 'z': 1.49, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.3,'height': 1.2 },'orientation': { 'w': 1 },'color': { 'r': 1,'g': 0.627,'b': 0 } }, + + { 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': .1, 'y': -.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B + { 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': .2, 'y': .3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B + { 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # impossible for the target B + { 'id': 'green1', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } }, + { 'id': 'green2', 'type': 'BOX', 'pos': { 'x': .1, 'y': 1.91, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } }, + { 'id': 'red1', 'type': 'BOX', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } }, + { 'id': 'red2', 'type': 'BOX', 'pos': { 'x': .3, 'y': 1.41, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } }, + + { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.22, 'z': 0.89 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, + { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.22, 'y': 1.31, 'z': 0.89 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } +] } diff --git a/src/mtc/src/base_routine.cpp b/src/mtc/src/base_routine.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c3a99d8b10c0554f0c74b2569007afcd801801c3 --- /dev/null +++ b/src/mtc/src/base_routine.cpp @@ -0,0 +1,31 @@ +#include "impl/abstract_map_loader.h" +#include "impl/map_loader.h" +#include "impl/abstract_strategy.h" +#include "impl/base_by_rotation.h" +#include <xmlrpcpp/XmlRpc.h> + +#include "impl/map_loader.cpp" +#include "impl/base_by_rotation.cpp" + +int main(int argc, char **argv){ + ros::init(argc, argv, "base_routine"); + ros::NodeHandle n; + + XmlRpc::XmlRpcValue map; + XmlRpc::XmlRpcValue task; + + n.getParam("/data",map); + n.getParam("/task/groups",task); + + Abstract_map_loader* map_loader = new Map_loader(map, task); + Abstract_strategy* strategy = new Base_by_rotation(); + + map_loader->set_strategy(strategy); + + map_loader->base_calculation(); + + free(map_loader); + free(strategy); + + return 0; +} \ No newline at end of file diff --git a/src/mtc/src/grasp_candidate.cpp b/src/mtc/src/grasp_candidate.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5482843520f118f662762f1e95c05e1afd0c0f2d --- /dev/null +++ b/src/mtc/src/grasp_candidate.cpp @@ -0,0 +1,116 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman <dave@picknik.ai> + Desc: Filters grasps based on kinematic feasibility and collision +*/ + +#include "../include/grasp_candidate.h" + +namespace moveit_grasps +{ +GraspCandidate::GraspCandidate(const moveit_msgs::Grasp& grasp, const GraspDataPtr& grasp_data, + const Eigen::Isometry3d& cuboid_pose) + : grasp_(grasp) + , grasp_data_(grasp_data) + , cuboid_pose_(cuboid_pose) + , grasp_filtered_code_(GraspFilterCode::NOT_FILTERED) +{ +} + +bool GraspCandidate::getPreGraspState(moveit::core::RobotStatePtr& robot_state) +{ + // Error check + if (pregrasp_ik_solution_.empty()) + { + ROS_ERROR_STREAM_NAMED("grasp_candidate", "No pregrasp ik solution available to set"); + return false; + } + + // Apply IK solved arm joints to state + robot_state->setJointGroupPositions(grasp_data_->arm_jmg_, pregrasp_ik_solution_); + + // Set end effector to correct configuration + grasp_data_->setRobotState(robot_state, grasp_.pre_grasp_posture); + + return true; +} + +bool GraspCandidate::getGraspStateOpen(moveit::core::RobotStatePtr& robot_state) +{ + // Error check + if (grasp_ik_solution_.empty()) + { + ROS_ERROR_STREAM_NAMED("grasp_candidate", "No grasp ik solution available to set"); + return false; + } + + // Apply IK solved arm joints to state + robot_state->setJointGroupPositions(grasp_data_->arm_jmg_, grasp_ik_solution_); + + // Set end effector to correct configuration + return getGraspStateOpenEEOnly(robot_state); +} + +bool GraspCandidate::getGraspStateOpenEEOnly(moveit::core::RobotStatePtr& robot_state) +{ + return grasp_data_->setRobotState(robot_state, grasp_.pre_grasp_posture); +} + +bool GraspCandidate::getGraspStateClosed(moveit::core::RobotStatePtr& robot_state) +{ + if (grasp_ik_solution_.empty()) + { + ROS_ERROR_STREAM_NAMED("grasp_candidate", "No grasp ik solution available to set"); + return false; + } + + // Apply IK solved arm joints to state + robot_state->setJointGroupPositions(grasp_data_->arm_jmg_, grasp_ik_solution_); + + // Set end effector to correct configuration + return getGraspStateClosedEEOnly(robot_state); +} + +bool GraspCandidate::getGraspStateClosedEEOnly(moveit::core::RobotStatePtr& robot_state) +{ + return grasp_data_->setRobotState(robot_state, grasp_.grasp_posture); +} + +bool GraspCandidate::isValid() +{ + return grasp_filtered_code_ == GraspFilterCode::NOT_FILTERED; +} + +} // namespace moveit_grasps diff --git a/src/mtc/src/grasp_data.cpp b/src/mtc/src/grasp_data.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e69114492a8726ce11da01ee908289a331e6c654 --- /dev/null +++ b/src/mtc/src/grasp_data.cpp @@ -0,0 +1,212 @@ +/* + * Software License Agreement (Modified BSD License) + * + * Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L. + * 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 Univ of CO, Boulder, PAL Robotics, S.L. + * nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Authors: Dave Coleman, Bence Magyar + Description: Data class used by the grasp generator. +*/ + +#include "../include/grasp_data.h" + +// Eigen +#include <Eigen/Core> +#include <Eigen/Geometry> +#include <eigen_conversions/eigen_msg.h> + +// C++ +#include <cmath> + +// Parameter loading +#include <rosparam_shortcuts/rosparam_shortcuts.h> + +// Pose conversion +#include <rviz_visual_tools/rviz_visual_tools.h> + +namespace moveit_grasps +{ +const std::string LOGNAME = "grasp_data"; + +GraspData::GraspData(const ros::NodeHandle& nh, const std::string& end_effector, + const moveit::core::RobotModelConstPtr& robot_model) + : base_link_("/base_link"), robot_model_(robot_model) +{ +} + +bool GraspData::loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector) +{ + std::vector<std::string> joint_names; + std::vector<double> pre_grasp_posture; // todo: remove all underscore post-fixes + std::vector<double> grasp_posture; + double pregrasp_time_from_start; + double grasp_time_from_start; + std::string end_effector_name; + + // Helper to let user know what is wrong + if (!nh.hasParam("base_link")) + { + ROS_ERROR_STREAM_NAMED("grasp_data", "Grasp configuration parameter `base_link` missing from rosparam " + "server. Did you load your end effector's configuration yaml file? " + "Searching in namespace: " + << nh.getNamespace()); + return false; + } + + // Load all other parameters + const std::string parent_name = "grasp_data"; // for namespacing logging messages + std::size_t error = 0; + error += !rosparam_shortcuts::get(parent_name, nh, "base_link", base_link_); + + // Search within the sub-namespace of this end effector name + ros::NodeHandle child_nh(nh, end_effector); + + error += !rosparam_shortcuts::get(parent_name, child_nh, "pregrasp_time_from_start", pregrasp_time_from_start); + error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_time_from_start", grasp_time_from_start); + error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_resolution", grasp_resolution_); + error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_min_depth", grasp_min_depth_); + error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_max_depth", grasp_max_depth_); + error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_depth_resolution", grasp_depth_resolution_); + error += !rosparam_shortcuts::get(parent_name, child_nh, "approach_distance_desired", approach_distance_desired_); + error += !rosparam_shortcuts::get(parent_name, child_nh, "retreat_distance_desired", retreat_distance_desired_); + error += !rosparam_shortcuts::get(parent_name, child_nh, "lift_distance_desired", lift_distance_desired_); + error += !rosparam_shortcuts::get(parent_name, child_nh, "angle_resolution", angle_resolution_); + error += !rosparam_shortcuts::get(parent_name, child_nh, "end_effector_name", end_effector_name); + error += !rosparam_shortcuts::get(parent_name, child_nh, "joints", joint_names); + error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_padding_on_approach", grasp_padding_on_approach_); + error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_posture", grasp_posture); + error += !rosparam_shortcuts::get(parent_name, child_nh, "pregrasp_posture", pre_grasp_posture); + + bool define_tcp_by_name; + child_nh.param<bool>("define_tcp_by_name", define_tcp_by_name, false); + if (define_tcp_by_name) + error += !rosparam_shortcuts::get(parent_name, child_nh, "tcp_name", tcp_name_); + else + error += !rosparam_shortcuts::get(parent_name, child_nh, "tcp_to_eef_mount_transform", tcp_to_eef_mount_); + + rosparam_shortcuts::shutdownIfError(parent_name, error); + + // Create pre-grasp and grasp postures + pre_grasp_posture_.header.frame_id = base_link_; + pre_grasp_posture_.header.stamp = ros::Time::now(); + // Name of joints: + pre_grasp_posture_.joint_names = joint_names; + // Position of joints + pre_grasp_posture_.points.resize(1); + pre_grasp_posture_.points[0].positions = pre_grasp_posture; + pre_grasp_posture_.points[0].time_from_start = ros::Duration(pregrasp_time_from_start); + + // Create grasp posture + grasp_posture_.header.frame_id = base_link_; + grasp_posture_.header.stamp = ros::Time::now(); + // Name of joints: + grasp_posture_.joint_names = joint_names; + // Position of joints + grasp_posture_.points.resize(1); + grasp_posture_.points[0].positions = grasp_posture; + grasp_posture_.points[0].time_from_start = ros::Duration(grasp_time_from_start); + + // Copy values from RobotModel + ee_jmg_ = robot_model_->getJointModelGroup(end_effector_name); + arm_jmg_ = robot_model_->getJointModelGroup(ee_jmg_->getEndEffectorParentGroup().first); + parent_link_ = robot_model_->getLinkModel(ee_jmg_->getEndEffectorParentGroup().second); + + ROS_INFO_NAMED("grasp_data", "ee_name: %s, arm_jmg: %s, parent_link: %s", ee_jmg_->getName().c_str(), + arm_jmg_->getName().c_str(), parent_link_->getName().c_str()); + + if (define_tcp_by_name) + { + robot_state::RobotState state(robot_model_); + state.setToDefaultValues(); + state.update(); + if (!state.knowsFrameTransform(parent_link_->getName())) + { + ROS_ERROR_NAMED("grasp_data", + "Robot Model does not know the frame transform for the end effector group parent " + "frame: %s. Did you set a parent link in the srdf?", + parent_link_->getName().c_str()); + } + if (!state.knowsFrameTransform(tcp_name_)) + { + ROS_ERROR_NAMED("grasp_data", + "Robot Model does not know the frame transform for the tcp frame: %s. Is it " + "available in the urdf?", + tcp_name_.c_str()); + } + Eigen::Isometry3d eef_mount_pose = state.getGlobalLinkTransform(parent_link_); + Eigen::Isometry3d tcp_mount_pose = state.getGlobalLinkTransform(tcp_name_); + tcp_to_eef_mount_ = tcp_mount_pose.inverse() * eef_mount_pose; + } + + return true; +} + +bool GraspData::setRobotStatePreGrasp(robot_state::RobotStatePtr& robot_state) +{ + ROS_WARN_STREAM_NAMED("grasp_data", "setRobotStatePreGrasp is probably wrong"); + return setRobotState(robot_state, pre_grasp_posture_); +} + +bool GraspData::setRobotStateGrasp(robot_state::RobotStatePtr& robot_state) +{ + ROS_WARN_STREAM_NAMED("grasp_data", "setRobotStateGrasp is probably wrong"); + return setRobotState(robot_state, grasp_posture_); +} + +bool GraspData::setRobotState(robot_state::RobotStatePtr& robot_state, const trajectory_msgs::JointTrajectory& posture) +{ + // TODO(davetcoleman): make this more efficient + // Do for every joint in end effector + for (std::size_t i = 0; i < posture.joint_names.size(); ++i) + { + // Set joint position + robot_state->setJointPositions(posture.joint_names[i], posture.points[0].positions); + } + return true; +} + +void GraspData::print() +{ + ROS_WARN_STREAM_NAMED("grasp_data", "Debug Grasp Data variable values:"); + std::cout << "tcp_to_eef_mount_: \n" + << tcp_to_eef_mount_.translation() << "\n" + << tcp_to_eef_mount_.rotation() << std::endl; + std::cout << "pre_grasp_posture_: \n" << pre_grasp_posture_ << std::endl; + std::cout << "grasp_posture_: \n" << grasp_posture_ << std::endl; + std::cout << "base_link_: " << base_link_ << std::endl; + std::cout << "ee_group_: " << ee_jmg_->getName() << std::endl; + std::cout << "angle_resolution_: " << angle_resolution_ << std::endl; + std::cout << "grasp_max_depth_: " << grasp_max_depth_ << std::endl; + std::cout << "grasp_padding_on_approach_: " << grasp_padding_on_approach_ << std::endl; +} + +} // namespace moveit_grasps diff --git a/src/mtc/src/grasp_filter.cpp b/src/mtc/src/grasp_filter.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fd7ec8f9e4d9fec29605cafcbc4640e5fb1061c2 --- /dev/null +++ b/src/mtc/src/grasp_filter.cpp @@ -0,0 +1,884 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman <dave@picknik.ai> + Desc: Filters grasps based on kinematic feasibility and collision +*/ + +// moveit_grasps +#include "../include/grasp_filter.h" +#include "../include/state_validity_callback.h" + +// moveit +#include <moveit/transforms/transforms.h> +#include <moveit/collision_detection/collision_tools.h> + +// Conversions +#include <eigen_conversions/eigen_msg.h> + +// Parameter loading +#include <rosparam_shortcuts/rosparam_shortcuts.h> + +namespace moveit_grasps +{ +// Constructor +GraspFilter::GraspFilter(const robot_state::RobotStatePtr& robot_state, + const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools) + : name_("grasp_filter"), visual_tools_(visual_tools), nh_("~/moveit_grasps/filter") +{ + // Make a copy of the robot state so that we are sure outside influence does not break our grasp filter + robot_state_ = std::make_shared<moveit::core::RobotState>(*robot_state); + robot_state_->update(); // make sure transforms are computed + + // Load visulization settings + const std::string parent_name = "grasp_filter"; // for namespacing logging messages + std::size_t error = 0; + // clang-format off + error += !rosparam_shortcuts::get(parent_name, nh_, "collision_verbose", collision_verbose_); + error += !rosparam_shortcuts::get(parent_name, nh_, "statistics_verbose", statistics_verbose_); + error += !rosparam_shortcuts::get(parent_name, nh_, "collision_verbose_speed", collision_verbose_speed_); + error += !rosparam_shortcuts::get(parent_name, nh_, "show_filtered_grasps", show_filtered_grasps_); + error += !rosparam_shortcuts::get(parent_name, nh_, "show_filtered_arm_solutions", show_filtered_arm_solutions_); + error += !rosparam_shortcuts::get(parent_name, nh_, "show_cutting_planes", show_cutting_planes_); + error += !rosparam_shortcuts::get(parent_name, nh_, "show_filtered_arm_solutions_speed", show_filtered_arm_solutions_speed_); + error += !rosparam_shortcuts::get(parent_name, nh_, "show_filtered_arm_solutions_pregrasp_speed", show_filtered_arm_solutions_pregrasp_speed_); + error += !rosparam_shortcuts::get(parent_name, nh_, "show_grasp_filter_collision_if_failed", show_grasp_filter_collision_if_failed_); + // clang-format on + + rosparam_shortcuts::shutdownIfError(parent_name, error); + + // Add planning scene publisher for debugging + planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>("/published_planning_scene", 100, false); +} + +bool GraspFilter::filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const robot_model::JointModelGroup* arm_jmg, + const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, + const std::string& target_object_id) +{ + planning_scene::PlanningScenePtr planning_scene; + { + planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor); + planning_scene = planning_scene::PlanningScene::clone(scene); + } + return filterGrasps(grasp_candidates, planning_scene, arm_jmg, seed_state, filter_pregrasp, target_object_id); +} + +bool GraspFilter::filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates, + const planning_scene::PlanningScenePtr& planning_scene, + const robot_model::JointModelGroup* arm_jmg, + const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, + const std::string& target_object_id) +{ + bool verbose = false; + + // Error check + if (grasp_candidates.empty()) + { + ROS_ERROR_NAMED(name_, "Unable to filter grasps because vector is empty"); + return false; + } + if (!filter_pregrasp) + ROS_WARN_STREAM_NAMED(name_, "Not filtering pre-grasp - GraspCandidate may have bad data"); + + // Visualize the cutting planes if desired + visualizeCuttingPlanes(); + + // Get the solver timeout from kinematics.yaml + solver_timeout_ = arm_jmg->getDefaultIKTimeout(); + ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Grasp filter IK timeout " << solver_timeout_); + + // Choose how many degrees of freedom + num_variables_ = arm_jmg->getVariableCount(); + ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Solver for " << num_variables_ << " degrees of freedom"); + + // Get the end effector joint model group + if (arm_jmg->getAttachedEndEffectorNames().size() == 0) + { + ROS_ERROR_STREAM_NAMED(name_, "No end effectors attached to this arm"); + return false; + } + else if (arm_jmg->getAttachedEndEffectorNames().size() > 1) + { + ROS_ERROR_STREAM_NAMED(name_, "More than one end effectors attached to this arm"); + return false; + } + + // Try to filter grasps not in verbose mode + std::size_t remaining_grasps = filterGraspsHelper(grasp_candidates, planning_scene, arm_jmg, seed_state, + filter_pregrasp, verbose, target_object_id); + + // Print stats + printFilterStatistics(grasp_candidates); + + if (remaining_grasps == 0) + { + ROS_INFO_STREAM_NAMED(name_, "Grasp filters removed all grasps"); + if (show_grasp_filter_collision_if_failed_) + { + ROS_INFO_STREAM_NAMED(name_, "Re-running in verbose mode since it failed"); + verbose = true; + remaining_grasps = filterGraspsHelper(grasp_candidates, planning_scene, arm_jmg, seed_state, filter_pregrasp, + verbose, target_object_id); + } + else + ROS_INFO_STREAM_NAMED(name_, "NOT re-running in verbose mode"); + } + + // Visualize valid grasps as arrows with cartesian path as well + if (show_filtered_grasps_) + { + ROS_INFO_STREAM_NAMED(name_, "Showing filtered grasps"); + visualizeGrasps(grasp_candidates, arm_jmg); + } + + // Visualize valid grasp as arm positions + if (show_filtered_arm_solutions_) + { + ROS_INFO_STREAM_NAMED(name_, "Showing filtered arm solutions"); + visualizeCandidateGrasps(grasp_candidates); + } + + if (grasp_candidates.empty()) + { + ROS_WARN_STREAM_NAMED(name_, "No grasps remaining after filtering"); + return false; + } + + return true; +} + +bool GraspFilter::filterGraspByPlane(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& filter_pose, + GraspParallelPlane plane, int direction) const +{ + Eigen::Isometry3d grasp_pose; + Eigen::Vector3d grasp_position; + + // get grasp translation in filter pose CS + grasp_pose = visual_tools_->convertPose(grasp_candidate->grasp_.grasp_pose.pose); + grasp_position = filter_pose.inverse() * grasp_pose.translation(); + + // filter grasps by cutting plane + double epsilon = 0.00000001; + switch (plane) + { + case XY: + if ((direction == -1 && grasp_position(2) < 0 + epsilon) || (direction == 1 && grasp_position(2) > 0 - epsilon)) + grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE; + break; + case XZ: + if ((direction == -1 && grasp_position(1) < 0 + epsilon) || (direction == 1 && grasp_position(1) > 0 - epsilon)) + grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE; + break; + case YZ: + if ((direction == -1 && grasp_position(0) < 0 + epsilon) || (direction == 1 && grasp_position(0) > 0 - epsilon)) + grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE; + break; + default: + ROS_WARN_STREAM_NAMED("filter_by_plane", "plane not specified correctly"); + break; + } + + return grasp_candidate->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE; +} + +bool GraspFilter::filterGraspByOrientation(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& desired_pose, + double max_angular_offset) const +{ + Eigen::Isometry3d tcp_grasp_pose; + Eigen::Isometry3d eef_mount_grasp_pose; + Eigen::Vector3d desired_z_axis; + Eigen::Vector3d grasp_z_axis; + double angle; + + // convert grasp pose back to standard grasping orientation + eef_mount_grasp_pose = visual_tools_->convertPose(grasp_candidate->grasp_.grasp_pose.pose); + tcp_grasp_pose = eef_mount_grasp_pose * grasp_candidate->grasp_data_->tcp_to_eef_mount_.inverse(); + + // compute the angle between the z-axes of the desired and grasp poses + grasp_z_axis = tcp_grasp_pose.rotation() * Eigen::Vector3d(0, 0, 1); + desired_z_axis = desired_pose.rotation() * Eigen::Vector3d(0, 0, 1); + double cos_angle = grasp_z_axis.normalized().dot(desired_z_axis.normalized()); + angle = acos(std::max(-1.0, std::min(1.0, cos_angle))); + + if (angle > max_angular_offset) + { + ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "grasp filtered by orientation"); + grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_ORIENTATION; + return true; + } + else + return false; +} + +bool GraspFilter::filterGraspByGraspIK(const GraspCandidatePtr& grasp_candidate, std::vector<double>& grasp_ik_solution, + const IkThreadStructPtr& ik_thread_struct) const +{ + // Get pose + ik_thread_struct->ik_pose_ = grasp_candidate->grasp_.grasp_pose; + + // Create constraint_fn + moveit::core::GroupStateValidityCallbackFn constraint_fn = + boost::bind(&isGraspStateValid, ik_thread_struct->planning_scene_.get(), + collision_verbose_ || ik_thread_struct->visual_debug_, collision_verbose_speed_, visual_tools_, _1, + _2, _3); + + // Set gripper position (eg. how open the eef is) to the custom open position + grasp_candidate->getGraspStateOpenEEOnly(ik_thread_struct->robot_state_); + + // Solve IK Problem for grasp posture + grasp_ik_solution.resize(0); + if (!findIKSolution(grasp_ik_solution, ik_thread_struct, grasp_candidate, constraint_fn)) + { + ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Unable to find the-grasp IK solution"); + grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_IK; + } + return grasp_candidate->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_IK; +} + +bool GraspFilter::filterGraspByPreGraspIK(const GraspCandidatePtr& grasp_candidate, + std::vector<double>& pregrasp_ik_solution, + const IkThreadStructPtr& ik_thread_struct) const +{ + if (!ik_thread_struct->filter_pregrasp_) + { + ROS_DEBUG_STREAM_NAMED(name_, "Not filtering pregrasp"); + return true; + } + + // Set IK target pose to the pre-grasp pose + const std::string& ee_parent_link_name = grasp_candidate->grasp_data_->ee_jmg_->getEndEffectorParentGroup().second; + ik_thread_struct->ik_pose_ = GraspGenerator::getPreGraspPose(grasp_candidate, ee_parent_link_name); + + moveit::core::GroupStateValidityCallbackFn constraint_fn = + boost::bind(&isGraspStateValid, ik_thread_struct->planning_scene_.get(), + collision_verbose_ || ik_thread_struct->visual_debug_, collision_verbose_speed_, visual_tools_, _1, + _2, _3); + + // Solve IK Problem for pregrasp + pregrasp_ik_solution.resize(0); + if (!findIKSolution(pregrasp_ik_solution, ik_thread_struct, grasp_candidate, constraint_fn)) + { + ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Unable to find PRE-grasp IK solution"); + grasp_candidate->grasp_filtered_code_ = GraspFilterCode::PREGRASP_FILTERED_BY_IK; + } + return grasp_candidate->grasp_filtered_code_ == GraspFilterCode::PREGRASP_FILTERED_BY_IK; +} + +std::size_t +GraspFilter::filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const robot_model::JointModelGroup* arm_jmg, + const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, bool visualize, + const std::string& target_object_id) +{ + planning_scene::PlanningScenePtr planning_scene; + { + planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor); + planning_scene = planning_scene::PlanningScene::clone(scene); + } + return filterGraspsHelper(grasp_candidates, planning_scene, arm_jmg, seed_state, filter_pregrasp, visualize, + target_object_id); +} + +std::size_t GraspFilter::filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates, + const planning_scene::PlanningScenePtr& planning_scene, + const robot_model::JointModelGroup* arm_jmg, + const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, + bool visualize, const std::string& target_object_id) +{ + // Clone the planning scene for const correctness + planning_scene::PlanningScenePtr planning_scene_clone = planning_scene::PlanningScene::clone(planning_scene); + + // Setup collision checking + *robot_state_ = planning_scene_clone->getCurrentState(); + + // Check that we have grasp candidates + if (!grasp_candidates.size()) + { + ROS_WARN_NAMED(name_, "filterGraspsHelper passed empty grasp candidates"); + return 0; + } + + // Choose Number of cores + std::size_t num_threads = omp_get_max_threads(); + if (num_threads > grasp_candidates.size()) + { + num_threads = grasp_candidates.size(); + } + + // Debug + if (visualize || collision_verbose_) + { + num_threads = 1; + ROS_WARN_STREAM_NAMED(name_, "Using only " << num_threads << " threads because verbose is true"); + } + ROS_INFO_STREAM_NAMED(name_, "Filtering " << grasp_candidates.size() << " candidate grasps with " << num_threads + << " threads"); + + // Load kinematic solvers if not already loaded + if (kin_solvers_[arm_jmg->getName()].size() != num_threads) + { + kin_solvers_[arm_jmg->getName()].clear(); + + // Create an ik solver for every thread + for (std::size_t i = 0; i < num_threads; ++i) + { + // ROS_DEBUG_STREAM_NAMED(name_,"Creating ik solver " << i); + kin_solvers_[arm_jmg->getName()].push_back(arm_jmg->getSolverInstance()); + + // Test to make sure we have a valid kinematics solver + if (!kin_solvers_[arm_jmg->getName()][i]) + { + ROS_ERROR_STREAM_NAMED(name_, "No kinematic solver found"); + return 0; + } + } + } + + // Robot states + // Create a robot state for every thread + if (robot_states_.size() != num_threads) + { + robot_states_.clear(); + for (std::size_t i = 0; i < num_threads; ++i) + { + // Copy the previous robot state + robot_states_.push_back(std::make_shared<robot_state::RobotState>(*robot_state_)); + } + } + else // update the states + { + for (std::size_t i = 0; i < num_threads; ++i) + { + // Copy the previous robot state + *(robot_states_[i]) = *robot_state_; + } + } + + // Transform poses + // bring the pose to the frame of the IK solver + const std::string& ik_frame = kin_solvers_[arm_jmg->getName()][0]->getBaseFrame(); + Eigen::Isometry3d link_transform; + ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", + "Frame transform from ik_frame: " << ik_frame << " and robot model frame: " + << robot_state_->getRobotModel()->getModelFrame()); + if (!moveit::core::Transforms::sameFrame(ik_frame, robot_state_->getRobotModel()->getModelFrame())) + { + const robot_model::LinkModel* lm = + robot_state_->getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame); + + if (!lm) + { + ROS_ERROR_STREAM_NAMED(name_, "Unable to find frame for link transform"); + return 0; + } + + link_transform = robot_state_->getGlobalLinkTransform(lm).inverse(); + } + + // Ensure the ACM entries are set to ignore collisions between the eef and the target object + if (!target_object_id.empty()) + { + if (!planning_scene_clone->knowsFrameTransform(target_object_id)) + { + ROS_ERROR_STREAM_NAMED(name_, "target_object_id: " << target_object_id << " unknown to the planning scene"); + return 0; + } + std::vector<std::string> ee_links = grasp_candidates.front()->grasp_data_->ee_jmg_->getLinkModelNames(); + if (!ee_links.empty()) + { + setACMFingerEntry(target_object_id, true, ee_links, planning_scene_clone); + } + } + + // Create the seed state vector + std::vector<double> ik_seed_state; + seed_state->copyJointGroupPositions(arm_jmg, ik_seed_state); + + // Thread data + // Allocate only once to increase performance + std::vector<IkThreadStructPtr> ik_thread_structs; + ik_thread_structs.resize(num_threads); + for (std::size_t thread_id = 0; thread_id < num_threads; ++thread_id) + { + ik_thread_structs[thread_id] = + std::make_shared<IkThreadStruct>(grasp_candidates, planning_scene_clone, link_transform, + 0, // this is filled in by OpenMP + kin_solvers_[arm_jmg->getName()][thread_id], robot_states_[thread_id], + solver_timeout_, filter_pregrasp, visualize, thread_id, target_object_id); + ik_thread_structs[thread_id]->ik_seed_state_ = ik_seed_state; + } + + // Benchmark time + ros::Time start_time; + start_time = ros::Time::now(); + + // Loop through poses and find those that are kinematically feasible + + omp_set_num_threads(num_threads); +#pragma omp parallel for schedule(dynamic) + for (std::size_t grasp_id = 0; grasp_id < grasp_candidates.size(); ++grasp_id) + { + std::size_t thread_id = omp_get_thread_num(); + ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Thread " << thread_id << " processing grasp " << grasp_id); + + // If in verbose mode allow for quick exit + if (ik_thread_structs[thread_id]->visual_debug_ && !ros::ok()) + continue; // breaking a for loop is not allows with OpenMP + + // Assign grasp to process + ik_thread_structs[thread_id]->grasp_id = grasp_id; + + // Process the grasp if it hasn't already been filtered out + if (grasp_candidates[grasp_id]->isValid()) + processCandidateGrasp(ik_thread_structs[thread_id]); + } + + if (statistics_verbose_) + { + // End Benchmark time + double duration = (ros::Time::now() - start_time).toSec(); + ROS_INFO_STREAM_NAMED(name_ + ".print_filter_statistics", "==================================================="); + ROS_INFO_STREAM_NAMED(name_ + ".print_filter_statistics", "FILTER DURATION"); + ROS_INFO_STREAM_NAMED(name_ + ".print_filter_statistics", "Grasp Filter Duration :\t" << duration); + ROS_INFO_STREAM_NAMED(name_ + ".print_filter_statistics", "---------------------------------------------------"); + } + + std::size_t not_filtered = 0; + for (std::size_t i = 0; i < grasp_candidates.size(); ++i) + if (grasp_candidates[i]->isValid()) + ++not_filtered; + + return not_filtered; +} + +void GraspFilter::printFilterStatistics(const std::vector<GraspCandidatePtr>& grasp_candidates) const +{ + if (!statistics_verbose_) + return; + static const std::string logger_name = name_ + ".filter_statistics"; + // Count number of grasps remaining + std::size_t not_filtered = 0; + std::size_t grasp_filtered_by_ik = 0; + std::size_t grasp_filtered_by_cutting_plane = 0; + std::size_t grasp_filtered_by_orientation = 0; + std::size_t pregrasp_filtered_by_ik = 0; + + for (std::size_t i = 0; i < grasp_candidates.size(); ++i) + { + if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_IK) + ++grasp_filtered_by_ik; + else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE) + ++grasp_filtered_by_cutting_plane; + else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_ORIENTATION) + ++grasp_filtered_by_orientation; + else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::PREGRASP_FILTERED_BY_IK) + ++pregrasp_filtered_by_ik; + else if (grasp_candidates[i]->isValid()) + ++not_filtered; + } + + ROS_INFO_STREAM_NAMED(logger_name, "======================================================="); + ROS_INFO_STREAM_NAMED(logger_name, "GRASP FILTER RESULTS "); + ROS_INFO_STREAM_NAMED(logger_name, "Total candidate grasps " << grasp_candidates.size()); + ROS_INFO_STREAM_NAMED(logger_name, "Total valid grasps " << not_filtered); + ROS_INFO_STREAM_NAMED(logger_name, "-------------------------------------------------------"); + ROS_INFO_STREAM_NAMED(logger_name, "grasp_filtered_by_cutting_plane " << grasp_filtered_by_cutting_plane); + ROS_INFO_STREAM_NAMED(logger_name, "grasp_filtered_by_orientation " << grasp_filtered_by_orientation); + ROS_INFO_STREAM_NAMED(logger_name, "grasp_filtered_by_ik " << grasp_filtered_by_ik); + ROS_INFO_STREAM_NAMED(logger_name, "pregrasp_filtered_by_ik " << pregrasp_filtered_by_ik); +} + +bool GraspFilter::processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct) +{ + ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Checking grasp #" << ik_thread_struct->grasp_id); + + // Helper pointer + GraspCandidatePtr& grasp_candidate = ik_thread_struct->grasp_candidates_[ik_thread_struct->grasp_id]; + + // Get pose + ik_thread_struct->ik_pose_ = grasp_candidate->grasp_.grasp_pose; + + // Filter by cutting planes + for (auto& cutting_plane : cutting_planes_) + { + if (filterGraspByPlane(grasp_candidate, cutting_plane->pose_, cutting_plane->plane_, cutting_plane->direction_)) + { + return false; + } + } + + // Filter by desired orientation + for (auto& desired_grasp_orientation : desired_grasp_orientations_) + { + if (filterGraspByOrientation(grasp_candidate, desired_grasp_orientation->pose_, + desired_grasp_orientation->max_angle_offset_)) + { + return false; + } + } + + std::vector<double> grasp_ik_solution; + if (filterGraspByGraspIK(grasp_candidate, grasp_ik_solution, ik_thread_struct)) + { + return false; + } + + // Assign the grasp_ik solution. + // TODO: This should be moved out of filtering + grasp_candidate->grasp_ik_solution_ = grasp_ik_solution; + + // Copy solution to seed state so that next solution is faster + ik_thread_struct->ik_seed_state_ = grasp_ik_solution; + + std::vector<double> pregrasp_ik_solution; + if (filterGraspByPreGraspIK(grasp_candidate, pregrasp_ik_solution, ik_thread_struct)) + { + return false; + } + // Assign the pregrasp_ik solution. + // TODO: This should be moved out of filtering + grasp_candidate->pregrasp_ik_solution_ = pregrasp_ik_solution; + + return true; +} + +bool GraspFilter::findIKSolution(std::vector<double>& ik_solution, const IkThreadStructPtr& ik_thread_struct, + const GraspCandidatePtr& grasp_candidate, + const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const +{ + robot_state::RobotState state = ik_thread_struct->planning_scene_->getCurrentState(); + if (ik_thread_struct->ik_seed_state_.size() == grasp_candidate->grasp_data_->arm_jmg_->getActiveJointModels().size()) + { + state.setJointGroupPositions(grasp_candidate->grasp_data_->arm_jmg_, ik_thread_struct->ik_seed_state_); + state.update(); + } + + // Copy in attached bodies which get removed by setJointGroupPositions for some reason + std::vector<const robot_state::AttachedBody*> attached_bodies; + ik_thread_struct->planning_scene_->getCurrentState().getAttachedBodies(attached_bodies); + + for (const robot_state::AttachedBody* ab : attached_bodies) + state.attachBody(ab->getName(), ab->getPose(), ab->getShapes(), ab->getShapePoses(), ab->getTouchLinks(), + ab->getAttachedLinkName(), ab->getDetachPosture(), ab->getSubframes()); + + bool ik_success = state.setFromIK(grasp_candidate->grasp_data_->arm_jmg_, ik_thread_struct->ik_pose_.pose, + ik_thread_struct->timeout_, constraint_fn); + + // Results + if (ik_success) + { + state.copyJointGroupPositions(grasp_candidate->grasp_data_->arm_jmg_, ik_solution); + return true; + } + else + { + // The grasp was valid but the pre-grasp was not + ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "IK solution not found"); + return false; + } +} + +void GraspFilter::addCuttingPlane(const Eigen::Isometry3d& pose, GraspParallelPlane plane, int direction) +{ + cutting_planes_.push_back(std::make_shared<CuttingPlane>(pose, plane, direction)); +} + +void GraspFilter::addDesiredGraspOrientation(const Eigen::Isometry3d& pose, double max_angle_offset) +{ + desired_grasp_orientations_.push_back(std::make_shared<DesiredGraspOrientation>(pose, max_angle_offset)); +} + +bool GraspFilter::removeInvalidAndFilter(std::vector<GraspCandidatePtr>& grasp_candidates) const +{ + std::size_t original_num_grasps = grasp_candidates.size(); + + // Remove all invalid grasps + for (std::vector<GraspCandidatePtr>::iterator it = grasp_candidates.begin(); it != grasp_candidates.end();) + { + // Check if valid grasp + if (!(*it)->isValid()) + { + it = grasp_candidates.erase(it); // not valid + } + else + { + ++it; // was valid, keep + } + } + ROS_INFO_STREAM_NAMED(name_, "Removed " << original_num_grasps - grasp_candidates.size() + << " invalid grasp candidates, " << grasp_candidates.size() << " remaining"); + + // Error Check + if (grasp_candidates.empty()) + { + ROS_WARN_STREAM_NAMED(name_, "No remaining grasp candidates"); + return false; + } + + // Order remaining valid grasps by best score + std::sort(grasp_candidates.begin(), grasp_candidates.end(), compareGraspScores); + + ROS_INFO_STREAM_NAMED(name_, "Sorted valid grasps, highest quality is " + << grasp_candidates.front()->grasp_.grasp_quality << " and lowest quality is " + << grasp_candidates.back()->grasp_.grasp_quality); + + return true; +} + +bool GraspFilter::visualizeGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates, + const moveit::core::JointModelGroup* arm_jmg) +{ + // Publish in batch + visual_tools_->enableBatchPublishing(true); + + /* + NOTE: duplicated in README.md + MAGENTA - grasp filtered by cutting plane + YELLOW - grasp filtered by orientation + RED - grasp filtered by ik + PINK - grasp filtered by collision + BLUE - pregrasp filtered by ik + CYAN - pregrasp filtered by collision + GREY - misc filtered + GREEN - valid + */ + ROS_INFO_STREAM_NAMED(name_, "Showing " << grasp_candidates.size() << " solutions at a speed of " + << show_filtered_arm_solutions_speed_ << "sec per solution"); + ROS_INFO_STREAM_NAMED(name_, "---------------------------------------------"); + ROS_INFO_STREAM_NAMED(name_, " MAGENTA - grasp filtered by cutting plane"); + ROS_INFO_STREAM_NAMED(name_, " YELLOW - grasp filtered by orientation"); + ROS_INFO_STREAM_NAMED(name_, " RED - grasp filtered by ik"); + ROS_INFO_STREAM_NAMED(name_, " PINK - grasp filtered by collision"); + ROS_INFO_STREAM_NAMED(name_, " BLUE - pregrasp filtered by ik"); + ROS_INFO_STREAM_NAMED(name_, " CYAN - pregrasp filtered by collision"); + ROS_INFO_STREAM_NAMED(name_, " GREY - misc filtered"); + ROS_INFO_STREAM_NAMED(name_, " GREEN - valid"); + ROS_INFO_STREAM_NAMED(name_, "---------------------------------------------"); + for (std::size_t i = 0; i < grasp_candidates.size(); ++i) + { + double size = 0.03; // 0.01 * grasp_candidates[i]->grasp_.grasp_quality; + + if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_IK) + { + visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::RED, + rviz_visual_tools::SMALL, size); + } + else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::PREGRASP_FILTERED_BY_IK) + { + visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::BLUE, + rviz_visual_tools::SMALL, size); + } + else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE) + { + visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::MAGENTA, + rviz_visual_tools::SMALL, size); + } + else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_ORIENTATION) + { + visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::YELLOW, + rviz_visual_tools::SMALL, size); + } + else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::NOT_FILTERED) + { + visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::GREEN, + rviz_visual_tools::SMALL, size); + } + else + visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::GREY, + rviz_visual_tools::SMALL, size); + + // Publish in batch + visual_tools_->trigger(); + ros::Duration(.01).sleep(); + } + + return true; +} + +bool GraspFilter::visualizeIKSolutions(const std::vector<GraspCandidatePtr>& grasp_candidates, + const moveit::core::JointModelGroup* arm_jmg, double animation_speed) +{ + // Convert the grasp_candidates into a format moveit_visual_tools can use + std::vector<trajectory_msgs::JointTrajectoryPoint> ik_solutions; + for (std::size_t i = 0; i < grasp_candidates.size(); ++i) + { + // Check if valid grasp + if (!grasp_candidates[i]->isValid()) + continue; + + trajectory_msgs::JointTrajectoryPoint new_point; + new_point.positions = grasp_candidates[i]->grasp_ik_solution_; + ik_solutions.push_back(new_point); + } + + if (ik_solutions.empty()) + { + ROS_ERROR_STREAM_NAMED(name_, "Unable to visualize IK solutions because there are no valid ones"); + return false; + } + + return visual_tools_->publishIKSolutions(ik_solutions, arm_jmg, animation_speed); +} + +bool GraspFilter::visualizeCandidateGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates) +{ + for (std::size_t i = 0; i < grasp_candidates.size(); ++i) + { + // Check if valid grasp + if (!grasp_candidates[i]->isValid()) + continue; + + // Apply the pregrasp state + if (grasp_candidates[i]->getPreGraspState(robot_state_)) + { + // Show in Rviz + visual_tools_->publishRobotState(robot_state_, rviz_visual_tools::ORANGE); + visual_tools_->trigger(); + ros::Duration(show_filtered_arm_solutions_pregrasp_speed_).sleep(); + } + + // Apply the grasp state + if (grasp_candidates[i]->getGraspStateClosed(robot_state_)) + { + // Show in Rviz + visual_tools_->publishRobotState(robot_state_, rviz_visual_tools::WHITE); + visual_tools_->trigger(); + ros::Duration(show_filtered_arm_solutions_speed_).sleep(); + } + } + + return true; +} + +bool GraspFilter::visualizeCuttingPlanes() +{ + if (show_cutting_planes_) + { + for (auto& cutting_plane : cutting_planes_) + { + switch (cutting_plane->plane_) + { + case XY: + visual_tools_->publishXYPlane(cutting_plane->pose_); + break; + case XZ: + visual_tools_->publishXZPlane(cutting_plane->pose_); + break; + case YZ: + visual_tools_->publishYZPlane(cutting_plane->pose_); + break; + default: + ROS_ERROR_STREAM_NAMED(name_, "Unknown cutting plane type"); + } + } + visual_tools_->trigger(); + } + return true; +} + +void GraspFilter::clearCuttingPlanes() +{ + cutting_planes_.clear(); +} + +void GraspFilter::clearDesiredGraspOrientations() +{ + desired_grasp_orientations_.clear(); +} + +bool GraspFilter::addCuttingPlanesForBin(const Eigen::Isometry3d& world_to_bin, const Eigen::Isometry3d& bin_to_product, + const double& bin_width, const double& bin_height) +{ + // Add grasp filters + clearCuttingPlanes(); + clearDesiredGraspOrientations(); + + // Bottom of bin + addCuttingPlane(world_to_bin, XY, -1); + + // Right wall of bin + addCuttingPlane(world_to_bin, XZ, -1); + + // Top of bin + Eigen::Isometry3d world_to_bin_top = world_to_bin; + world_to_bin_top.translation() += Eigen::Vector3d(0, bin_width, bin_height); + addCuttingPlane(world_to_bin_top, XY, 1); + + // Left wall of bin + addCuttingPlane(world_to_bin_top, XZ, 1); + + // // Back half of product + Eigen::Isometry3d world_to_bin_back = world_to_bin; + world_to_bin_back.translation() += + Eigen::Vector3d(bin_to_product.translation().x(), bin_width / 2.0, bin_height / 2.0); + addCuttingPlane(world_to_bin_back, YZ, 1); + + return true; +} + +void GraspFilter::setACMFingerEntry(const std::string& object_name, bool allowed, + const std::vector<std::string>& ee_link_names, + const planning_scene::PlanningScenePtr& scene) +{ + static const std::string logger_name = name_ + ".set_acm_finger_entry"; + ROS_DEBUG_STREAM_NAMED(logger_name, "" << object_name.c_str() << ", " << (allowed ? "true" : "false")); + + // Lock planning scene + for (std::size_t i = 0; i < ee_link_names.size(); ++i) + { + ROS_DEBUG_NAMED(logger_name, "collisions between %s and %s : %s", object_name.c_str(), ee_link_names[i].c_str(), + allowed ? "allowed" : "not allowed"); + scene->getAllowedCollisionMatrixNonConst().setEntry(object_name, ee_link_names[i], allowed); + } + + // Debug current matrix + if (false) + { + moveit_msgs::AllowedCollisionMatrix msg; + scene->getAllowedCollisionMatrix().getMessage(msg); + ROS_DEBUG_STREAM_NAMED(logger_name, "Current collision matrix: " << msg); + } +} + +void GraspFilter::publishPlanningScene(const planning_scene::PlanningScenePtr& ps) const +{ + moveit_msgs::PlanningScene msg; + ps->getPlanningSceneMsg(msg); + planning_scene_publisher_.publish(msg); +} + +} // namespace moveit_grasps diff --git a/src/mtc/src/grasp_generator.cpp b/src/mtc/src/grasp_generator.cpp new file mode 100644 index 0000000000000000000000000000000000000000..95b469ba40a6db936dd90b72708e79fe7b13dabc --- /dev/null +++ b/src/mtc/src/grasp_generator.cpp @@ -0,0 +1,199 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy + Desc: Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches +*/ + +#include "../include/grasp_generator.h" +#include "../include/grasp_filter.h" + +#include <rosparam_shortcuts/rosparam_shortcuts.h> + +namespace +{ +void debugFailedOpenGripper(double percent_open, double min_finger_open_on_approach, double object_width, + double grasp_padding_on_approach) +{ + ROS_ERROR_STREAM_NAMED("grasp_generator", "Unable to set grasp width to " + << percent_open << " % open. Stats:" + << "\n min_finger_open_on_approach: \t " << min_finger_open_on_approach + << "\n object_width: \t " << object_width + << "\n grasp_padding_on_approach_: \t " << grasp_padding_on_approach); +} + +} // namespace + +namespace moveit_grasps + +{ +// Constructor +GraspGenerator::GraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, bool verbose) + : ideal_grasp_pose_(Eigen::Isometry3d::Identity()) + , visual_tools_(visual_tools) + , verbose_(verbose) + , nh_("~/moveit_grasps/generator") + , grasp_score_weights_(std::make_shared<GraspScoreWeights>()) +{ + // Load visulization settings + const std::string parent_name = "grasps"; // for namespacing logging messages + std::size_t error = 0; + + error += !rosparam_shortcuts::get(parent_name, nh_, "verbose", verbose_); + error += !rosparam_shortcuts::get(parent_name, nh_, "show_prefiltered_grasps", show_prefiltered_grasps_); + error += !rosparam_shortcuts::get(parent_name, nh_, "show_prefiltered_grasps_speed", show_prefiltered_grasps_speed_); + + // Load scoring weights + rosparam_shortcuts::shutdownIfError(parent_name, error); +} + +void GraspGenerator::setIdealTCPGraspPoseRPY(const std::vector<double>& ideal_grasp_orientation_rpy) +{ + ROS_ASSERT_MSG(ideal_grasp_orientation_rpy.size() == 3, "setIdealTCPGraspPoseRPY must be set with a vector of length " + "3"); + + // copy the ideal_grasp_pose.translation() so that we only change the orientation. + Eigen::Vector3d ideal_grasp_pose_translation(ideal_grasp_pose_.translation()); + + // Set ideal grasp pose (currently only uses orientation of pose) + ideal_grasp_pose_ = Eigen::Isometry3d::Identity() * + Eigen::AngleAxisd(ideal_grasp_orientation_rpy[0], Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(ideal_grasp_orientation_rpy[1], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(ideal_grasp_orientation_rpy[2], Eigen::Vector3d::UnitZ()); + + ideal_grasp_pose_.translation() = ideal_grasp_pose_translation; +} + +Eigen::Vector3d GraspGenerator::getPreGraspDirection(const moveit_msgs::Grasp& grasp, const std::string& ee_parent_link) +{ + // Grasp Pose Variables + Eigen::Isometry3d grasp_pose_eef_mount_eigen; + tf::poseMsgToEigen(grasp.grasp_pose.pose, grasp_pose_eef_mount_eigen); + + // The direction of the pre-grasp in the frame of the parent link + Eigen::Vector3d pre_grasp_approach_direction = + Eigen::Vector3d(grasp.pre_grasp_approach.direction.vector.x, grasp.pre_grasp_approach.direction.vector.y, + grasp.pre_grasp_approach.direction.vector.z); + + // Decide if we need to change the approach_direction to the local frame of the end effector orientation + if (grasp.pre_grasp_approach.direction.header.frame_id == ee_parent_link) + { + // Apply/compute the approach_direction vector in the local frame of the grasp_pose orientation + return grasp_pose_eef_mount_eigen.rotation() * pre_grasp_approach_direction; + } + return pre_grasp_approach_direction; +} + +geometry_msgs::PoseStamped GraspGenerator::getPreGraspPose(const GraspCandidatePtr& grasp_candidate, + const std::string& ee_parent_link) +{ + // Grasp Pose Variables + Eigen::Isometry3d grasp_pose_eef_mount_eigen; + tf::poseMsgToEigen(grasp_candidate->grasp_.grasp_pose.pose, grasp_pose_eef_mount_eigen); + + // Get pre-grasp pose first + Eigen::Isometry3d pre_grasp_pose_eef_mount_eigen = + grasp_pose_eef_mount_eigen; // Copy original grasp pose to pre-grasp pose + + // Approach direction + Eigen::Vector3d pre_grasp_approach_direction_local = getPreGraspDirection(grasp_candidate->grasp_, ee_parent_link); + + // Update the grasp matrix usign the new locally-framed approach_direction + pre_grasp_pose_eef_mount_eigen.translation() -= + pre_grasp_approach_direction_local * grasp_candidate->grasp_.pre_grasp_approach.desired_distance; + + // Convert eigen pre-grasp position back to regular message + geometry_msgs::PoseStamped pre_grasp_pose_eef_mount_msg; + tf::poseEigenToMsg(pre_grasp_pose_eef_mount_eigen, pre_grasp_pose_eef_mount_msg.pose); + + // Copy original header to new grasp + pre_grasp_pose_eef_mount_msg.header = grasp_candidate->grasp_.grasp_pose.header; + + return pre_grasp_pose_eef_mount_msg; +} + +void GraspGenerator::getGraspWaypoints(const GraspCandidatePtr& grasp_candidate, + EigenSTL::vector_Isometry3d& grasp_waypoints) +{ + Eigen::Isometry3d grasp_pose; + tf::poseMsgToEigen(grasp_candidate->grasp_.grasp_pose.pose, grasp_pose); + + const geometry_msgs::PoseStamped pregrasp_pose_msg = + GraspGenerator::getPreGraspPose(grasp_candidate, grasp_candidate->grasp_data_->parent_link_->getName()); + + // Create waypoints + Eigen::Isometry3d pregrasp_pose; + tf::poseMsgToEigen(pregrasp_pose_msg.pose, pregrasp_pose); + + Eigen::Isometry3d lifted_grasp_pose = grasp_pose; + lifted_grasp_pose.translation().z() += grasp_candidate->grasp_data_->lift_distance_desired_; + + // Solve for post grasp retreat + Eigen::Isometry3d retreat_pose = lifted_grasp_pose; + Eigen::Vector3d postgrasp_vector(grasp_candidate->grasp_.post_grasp_retreat.direction.vector.x, + grasp_candidate->grasp_.post_grasp_retreat.direction.vector.y, + grasp_candidate->grasp_.post_grasp_retreat.direction.vector.z); + postgrasp_vector.normalize(); + + retreat_pose.translation() += + retreat_pose.rotation() * postgrasp_vector * grasp_candidate->grasp_.post_grasp_retreat.desired_distance; + + grasp_waypoints.clear(); + grasp_waypoints.resize(4); + grasp_waypoints[0] = pregrasp_pose; + grasp_waypoints[1] = grasp_pose; + grasp_waypoints[2] = lifted_grasp_pose; + grasp_waypoints[3] = retreat_pose; +} + +void GraspGenerator::publishGraspArrow(const geometry_msgs::Pose& grasp, const GraspDataPtr& grasp_data, + const rviz_visual_tools::colors& color, double approach_length) +{ + visual_tools_->publishArrow(grasp, color, rviz_visual_tools::MEDIUM); +} + +bool GraspGenerator::visualizeAnimatedGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates, + const moveit::core::JointModelGroup* ee_jmg, double animation_speed) +{ + // Convert the grasp_candidates into a format moveit_visual_tools can use + std::vector<moveit_msgs::Grasp> grasps; + for (std::size_t i = 0; i < grasp_candidates.size(); ++i) + { + grasps.push_back(grasp_candidates[i]->grasp_); + } + + return visual_tools_->publishAnimatedGrasps(grasps, ee_jmg, show_prefiltered_grasps_speed_); +} + +} // namespace moveit_grasps diff --git a/src/mtc/src/grasp_pipeline.cpp b/src/mtc/src/grasp_pipeline.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a9298095a78636464955600c86d8e162b2483e48 --- /dev/null +++ b/src/mtc/src/grasp_pipeline.cpp @@ -0,0 +1,695 @@ +/********************************************************************* + * Software License Agreement (BSD 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************/ + +// ROS +#include <ros/ros.h> + +// MoveIt +#include <moveit/robot_state/robot_state.h> +#include <moveit/planning_scene_monitor/planning_scene_monitor.h> +#include <moveit/kinematic_constraints/utils.h> +#include <moveit/planning_interface/planning_interface.h> +#include <moveit/planning_pipeline/planning_pipeline.h> +#include <moveit/robot_state/conversions.h> + +// Visualization +#include <moveit_visual_tools/moveit_visual_tools.h> + +// Grasp +#include "../include/two_finger_grasp_generator.h" +#include "../include/two_finger_grasp_data.h" +#include "../include/two_finger_grasp_filter.h" +#include "../include/grasp_planner.h" + + +#include <fstream> +#include "yaml-cpp/yaml.h" +#include "tf2_eigen/tf2_eigen.h" + + +// Parameter loading +#include <rosparam_shortcuts/rosparam_shortcuts.h> + + +YAML::Node node; + +namespace moveit_grasps +{ +static const std::string LOGNAME = "grasp_pipeline_demo"; + +namespace +{ +bool isStateValid(const planning_scene::PlanningScene* planning_scene, + const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, robot_state::RobotState* robot_state, + const robot_model::JointModelGroup* group, const double* ik_solution) +{ + robot_state->setJointGroupPositions(group, ik_solution); + robot_state->update(); + return !planning_scene->isStateColliding(*robot_state, group->getName()); +} + +void waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt) +{ + visual_tools->prompt(prompt); +} + +} // namespace + +class GraspPipeline +{ +public: + // Constructor + GraspPipeline() : nh_("~") + { + // Get arm info from param server + const std::string parent_name = "grasp_filter_demo"; // for namespacing logging messages + rosparam_shortcuts::get(parent_name, nh_, "planning_group_name", planning_group_name_); + rosparam_shortcuts::get(parent_name, nh_, "ee_group_name", ee_group_name_); + + ROS_INFO_STREAM_NAMED("test", "End Effector: " << ee_group_name_); + ROS_INFO_STREAM_NAMED("test", "Planning Group: " << planning_group_name_); + + loadScene(); + setupGraspPipeline(); + } + + void loadScene() + { + // --------------------------------------------------------------------------------------------- + // Load planning scene to share + planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"); + if (!planning_scene_monitor_->getPlanningScene()) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning scene not configured"); + return; + } + planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, + "grasping_planning_scene"); + planning_scene_monitor_->getPlanningScene()->setName("grasping_planning_scene"); + + robot_model_loader::RobotModelLoaderPtr robot_model_loader; + robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description"); + + // Load the robot model + robot_model_ = robot_model_loader->getModel(); + arm_jmg_ = robot_model_->getJointModelGroup(planning_group_name_); + + // --------------------------------------------------------------------------------------------- + // Load the Robot Viz Tools for publishing to Rviz + visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>( + robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_); + visual_tools_->loadMarkerPub(); + visual_tools_->loadRobotStatePub("/display_robot_state"); + visual_tools_->loadTrajectoryPub("/display_planned_path"); + visual_tools_->loadSharedRobotState(); + visual_tools_->enableBatchPublishing(); + visual_tools_->deleteAllMarkers(); + visual_tools_->removeAllCollisionObjects(); + visual_tools_->hideRobot(); + visual_tools_->trigger(); + + // Publish the global frame + visual_tools_->publishAxis(Eigen::Isometry3d::Identity()); + visual_tools_->trigger(); + } + + void setupGraspPipeline() + { + // --------------------------------------------------------------------------------------------- + // Load grasp data specific to our robot + grasp_data_ = + std::make_shared<moveit_grasps::TwoFingerGraspData>(nh_, ee_group_name_, visual_tools_->getRobotModel()); + if (!grasp_data_->loadGraspData(nh_, ee_group_name_)) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Failed to load Grasp Data parameters."); + exit(-1); + } + + // --------------------------------------------------------------------------------------------- + // Load grasp generator + grasp_generator_ = std::make_shared<moveit_grasps::TwoFingerGraspGenerator>(visual_tools_); + + // Set the ideal grasp orientation for scoring + std::vector<double> ideal_grasp_rpy = { 3.14, 0.0, 0.0 }; + grasp_generator_->setIdealTCPGraspPoseRPY(ideal_grasp_rpy); + + // Set custom grasp score weights + auto grasp_score_weights = std::make_shared<moveit_grasps::TwoFingerGraspScoreWeights>(); + grasp_score_weights->orientation_x_score_weight_ = 2.0; + grasp_score_weights->orientation_y_score_weight_ = 2.0; + grasp_score_weights->orientation_z_score_weight_ = 2.0; + grasp_score_weights->translation_x_score_weight_ = 1.0; + grasp_score_weights->translation_y_score_weight_ = 1.0; + grasp_score_weights->translation_z_score_weight_ = 1.0; + // Finger gripper specific weights. + grasp_score_weights->depth_score_weight_ = 2.0; + grasp_score_weights->width_score_weight_ = 2.0; + // Assign the grasp score weights in the grasp_generator + grasp_generator_->setGraspScoreWeights(grasp_score_weights); + + // --------------------------------------------------------------------------------------------- + // Load grasp filter + grasp_filter_ = + std::make_shared<moveit_grasps::TwoFingerGraspFilter>(visual_tools_->getSharedRobotState(), visual_tools_); + + // --------------------------------------------------------------------------------------------- + // Load grasp planner for approach, lift and retreat planning + grasp_planner_ = std::make_shared<moveit_grasps::GraspPlanner>(visual_tools_); + + // MoveIt Grasps allows for a manual breakpoint debugging tool to be optionally passed in + grasp_planner_->setWaitForNextStepCallback(boost::bind(&waitForNextStep, visual_tools_, _1)); + + // ----------------------------------------------------- + // Load the motion planning pipeline + planning_pipeline_ = + std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, nh_, "planning_plugin", "request_adapter"); + } + + bool demoRandomGrasp() + { + // ----------------------------------- + // Generate random object to grasp + geometry_msgs::Pose object_pose; + double object_x_depth; + double object_y_width; + double object_z_height; + std::string object_name; + if (!generateRandomCuboid(object_name, object_pose, object_x_depth, object_y_width, object_z_height)) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to add random cuboid ot planning scene"); + return false; + } + + // ----------------------------------- + // Generate grasp candidates + std::vector<moveit_grasps::GraspCandidatePtr> grasp_candidates; + + // Configure the desired types of grasps + moveit_grasps::TwoFingerGraspCandidateConfig grasp_generator_config = + moveit_grasps::TwoFingerGraspCandidateConfig(); + grasp_generator_config.disableAll(); + grasp_generator_config.enable_face_grasps_ = true; + grasp_generator_config.generate_y_axis_grasps_ = true; + grasp_generator_config.generate_x_axis_grasps_ = true; + grasp_generator_config.generate_z_axis_grasps_ = true; + + grasp_generator_->setGraspCandidateConfig(grasp_generator_config); + if (!grasp_generator_->generateGrasps(visual_tools_->convertPose(object_pose), object_x_depth, object_y_width, + object_z_height, grasp_data_, grasp_candidates)) + { + ROS_ERROR_NAMED(LOGNAME, "Grasp generator failed to generate any valid grasps"); + return false; + } + + // -------------------------------------------- + // Generating a seed state for filtering grasps + robot_state::RobotStatePtr seed_state = + std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState()); + Eigen::Isometry3d eef_mount_grasp_pose = + visual_tools_->convertPose(object_pose) * grasp_data_->tcp_to_eef_mount_.inverse(); + if (!getIKSolution(arm_jmg_, eef_mount_grasp_pose, *seed_state, grasp_data_->parent_link_->getName())) + { + ROS_WARN_STREAM_NAMED(LOGNAME, "The ideal seed state is not reachable. Using start state as seed."); + } + + // -------------------------------------------- + // Filtering grasps + // Note: This step also solves for the grasp and pre-grasp states and stores them in grasp candidates) + bool filter_pregrasps = true; + if (!grasp_filter_->filterGrasps(grasp_candidates, planning_scene_monitor_, arm_jmg_, seed_state, filter_pregrasps, + object_name)) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Filter grasps failed"); + return false; + } + if (!grasp_filter_->removeInvalidAndFilter(grasp_candidates)) + { + ROS_WARN_NAMED(LOGNAME, "Grasp filtering removed all grasps"); + return false; + } + ROS_INFO_STREAM_NAMED(LOGNAME, "" << grasp_candidates.size() << " remain after filtering"); + + // Plan free-space approach, cartesian approach, lift and retreat trajectories + moveit_grasps::GraspCandidatePtr selected_grasp_candidate; + moveit_msgs::MotionPlanResponse pre_approach_plan; + if (!planFullGrasp(grasp_candidates, selected_grasp_candidate, pre_approach_plan, object_name)) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Failed to plan grasp motions"); + return false; + } + + visualizePick(selected_grasp_candidate, pre_approach_plan); + + return true; + } + + void visualizePick(const moveit_grasps::GraspCandidatePtr& valid_grasp_candidate, + const moveit_msgs::MotionPlanResponse& pre_approach_plan) + { + EigenSTL::vector_Isometry3d waypoints; + moveit_grasps::GraspGenerator::getGraspWaypoints(valid_grasp_candidate, waypoints); + + // Visualize waypoints + visual_tools_->publishAxisLabeled(waypoints[0], "pregrasp"); + visual_tools_->publishAxisLabeled(waypoints[1], "grasp"); + visual_tools_->publishAxisLabeled(waypoints[2], "lifted"); + visual_tools_->publishAxisLabeled(waypoints[3], "retreat"); + visual_tools_->trigger(); + + // Get the pre and post grasp states + visual_tools_->prompt("pre_grasp"); + robot_state::RobotStatePtr pre_grasp_state = + std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState()); + valid_grasp_candidate->getPreGraspState(pre_grasp_state); + visual_tools_->publishRobotState(pre_grasp_state, rviz_visual_tools::ORANGE); + robot_state::RobotStatePtr grasp_state = + std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState()); + if (valid_grasp_candidate->getGraspStateClosed(grasp_state)) + { + visual_tools_->prompt("grasp"); + visual_tools_->publishRobotState(grasp_state, rviz_visual_tools::YELLOW); + } + if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 1 && + valid_grasp_candidate->segmented_cartesian_traj_[1].size()) + { + visual_tools_->prompt("lift"); + visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[1].back(), + rviz_visual_tools::BLUE); + } + if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 2 && + valid_grasp_candidate->segmented_cartesian_traj_[2].size()) + { + visual_tools_->prompt("retreat"); + visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[2].back(), + rviz_visual_tools::PURPLE); + } + + visual_tools_->prompt("show free space approach"); + visual_tools_->hideRobot(); + visual_tools_->trigger(); + + bool wait_for_animation = true; + visual_tools_->publishTrajectoryPath(pre_approach_plan.trajectory, pre_grasp_state, wait_for_animation); + ros::Duration(0.25).sleep(); + if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::APPROACH) + visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH], + valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); + ros::Duration(0.25).sleep(); + + if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::LIFT) + visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::LIFT], + valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); + ros::Duration(0.25).sleep(); + + if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::RETREAT) + visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::RETREAT], + valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); + ros::Duration(0.25).sleep(); + } + + bool planFullGrasp(std::vector<moveit_grasps::GraspCandidatePtr>& grasp_candidates, + moveit_grasps::GraspCandidatePtr& valid_grasp_candidate, + moveit_msgs::MotionPlanResponse& pre_approach_plan, const std::string& object_name) + { + moveit::core::RobotStatePtr current_state; + { + boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls( + new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_)); + current_state = std::make_shared<robot_state::RobotState>((*ls)->getCurrentState()); + } + + bool success = false; + for (; !grasp_candidates.empty(); grasp_candidates.erase(grasp_candidates.begin())) + { + valid_grasp_candidate = grasp_candidates.front(); + valid_grasp_candidate->getPreGraspState(current_state); + if (!grasp_planner_->planApproachLiftRetreat(valid_grasp_candidate, current_state, planning_scene_monitor_, false, + object_name)) + { + ROS_INFO_NAMED(LOGNAME, "failed to plan approach lift retreat"); + continue; + } + + robot_state::RobotStatePtr pre_grasp_state = + valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH].front(); + if (!planPreApproach(*pre_grasp_state, pre_approach_plan)) + { + ROS_WARN_NAMED(LOGNAME, "failed to plan to pregrasp_state"); + continue; + } + + success = true; + break; + } + return success; + } + + bool planPreApproach(const robot_state::RobotState& goal_state, moveit_msgs::MotionPlanResponse& pre_approach_plan) + { + planning_interface::MotionPlanRequest req; + planning_interface::MotionPlanResponse res; + + double tolerance_above = 0.01; + double tolerance_below = 0.01; + // req.planner_id = "RRTConnectkConfigDefault"; + req.group_name = arm_jmg_->getName(); + req.num_planning_attempts = 5; + req.allowed_planning_time = 1.5; + moveit_msgs::Constraints goal = + kinematic_constraints::constructGoalConstraints(goal_state, arm_jmg_, tolerance_below, tolerance_above); + + req.goal_constraints.push_back(goal); + boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls( + new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_)); + + // --------------------------------- + // Change the robot current state + // NOTE: We have to do this since Panda start configuration is in self collision. + robot_state::RobotState rs = (*ls)->getCurrentState(); + std::vector<double> starting_joint_values = { 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; + std::vector<std::string> joint_names = { "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", + "panda_joint5", "panda_joint6", "panda_joint7" }; + // arm_jmg_->getActiveJointModelNames(); + for (std::size_t i = 0; i < joint_names.size(); ++i) + { + rs.setJointPositions(joint_names[i], &starting_joint_values[i]); + } + rs.update(); + robot_state::robotStateToRobotStateMsg(rs, req.start_state); + // --------------------------- + + planning_pipeline_->generatePlan(*ls, req, res); + if (res.error_code_.val != res.error_code_.SUCCESS) + { + ROS_INFO_NAMED(LOGNAME, "Failed to plan approach successfully"); + return false; + } + + res.getMessage(pre_approach_plan); + return true; + } + + bool getIKSolution(const moveit::core::JointModelGroup* arm_jmg, const Eigen::Isometry3d& target_pose, + robot_state::RobotState& solution, const std::string& link_name) + { + boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls( + new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_)); + + moveit::core::GroupStateValidityCallbackFn constraint_fn = boost::bind( + &isStateValid, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get(), visual_tools_, _1, _2, _3); + + // seed IK call with current state + solution = (*ls)->getCurrentState(); + + + // Solve IK problem for arm + // disable explicit restarts to guarantee close solution if one exists + const double timeout = 0.1; + + + + + + if (solution.setFromIK(arm_jmg, target_pose, link_name, timeout, constraint_fn)) { + geometry_msgs::Pose target = tf2::toMsg(target_pose); + node["data"].push_back(std::string( + std::to_string(target.position.x) + " " + + std::to_string(target.position.y) + " " + + std::to_string(target.position.z) + " " + + std::to_string(target.orientation.x) + " " + + std::to_string(target.orientation.y) + " " + + std::to_string(target.orientation.z) + " " + + std::to_string(target.orientation.w) + )); + return true; + } + return false; + } + + bool generateRandomCuboid(std::string& object_name, geometry_msgs::Pose& object_pose, double& x_depth, + double& y_width, double& z_height) + { + + // Generate random cuboid + double xmin = 0.5; + double xmax = 0.7; + double ymin = -0.25; + double ymax = 0.25; + double zmin = 0.2; + double zmax = 0.7; + rviz_visual_tools::RandomPoseBounds pose_bounds(xmin, xmax, ymin, ymax, zmin, zmax); + + double cuboid_size_min = 0.01; + double cuboid_size_max = 0.03; + rviz_visual_tools::RandomCuboidBounds cuboid_bounds(cuboid_size_min, cuboid_size_max); + + object_name = "pick_target"; + visual_tools_->generateRandomCuboid(object_pose, x_depth, y_width, z_height, pose_bounds, cuboid_bounds); + visual_tools_->publishCollisionCuboid(object_pose, x_depth, y_width, z_height, object_name, rviz_visual_tools::RED); + visual_tools_->publishAxis(object_pose, rviz_visual_tools::MEDIUM); + visual_tools_->trigger(); + + bool success = true; + double timeout = 5; // seconds + ros::Rate rate(100); + while (success && !planning_scene_monitor_->getPlanningScene()->knowsFrameTransform(object_name)) + { + rate.sleep(); + success = rate.cycleTime().toSec() < timeout; + } + return success; + } + + bool reachGrasp (float& x, float& y, float& z , std::string& object_name, double& x_depth, double& y_depth, double& z_depth) { + // Generate random cuboid + geometry_msgs::Pose object_pose; + double xmin = 0.5; + double xmax = 0.7; + double ymin = -0.25; + double ymax = 0.25; + double zmin = 0.2; + double zmax = 0.7; + rviz_visual_tools::RandomPoseBounds pose_bounds(xmin, xmax, ymin, ymax, zmin, zmax); + + double cuboid_size_min = 0.01; + double cuboid_size_max = 0.03; + rviz_visual_tools::RandomCuboidBounds cuboid_bounds(cuboid_size_min, cuboid_size_max); + + visual_tools_->generateRandomCuboid(object_pose, x_depth, y_depth, z_depth, pose_bounds, cuboid_bounds); + + object_pose.position.x = x; + object_pose.position.y = y; + object_pose.position.z = z; + + visual_tools_->publishCollisionCuboid(object_pose, x_depth, y_depth, z_depth, object_name, rviz_visual_tools::RED); + visual_tools_->publishAxis(object_pose, rviz_visual_tools::MEDIUM); + visual_tools_->trigger(); + + bool success = true; + double timeout = 5; + ros::Rate rate(100); + + while (success && !planning_scene_monitor_->getPlanningScene()->knowsFrameTransform(object_name)){ + rate.sleep(); + success = rate.cycleTime().toSec() < timeout; + } + + if (!success){ + ROS_ERROR_NAMED(LOGNAME, "Something went wring in Cube construction ._."); + return false; + } + + // ----------------------------------- + // Generate grasp candidates + std::vector<moveit_grasps::GraspCandidatePtr> grasp_candidates; + + // Configure the desired types of grasps + moveit_grasps::TwoFingerGraspCandidateConfig grasp_generator_config = + moveit_grasps::TwoFingerGraspCandidateConfig(); + grasp_generator_config.disableAll(); + grasp_generator_config.enable_face_grasps_ = true; + grasp_generator_config.generate_y_axis_grasps_ = true; + grasp_generator_config.generate_x_axis_grasps_ = true; + grasp_generator_config.generate_z_axis_grasps_ = true; + + grasp_generator_->setGraspCandidateConfig(grasp_generator_config); + if (!grasp_generator_->generateGrasps(visual_tools_->convertPose(object_pose), x_depth, y_depth, + z_depth, grasp_data_, grasp_candidates)){ + ROS_ERROR_NAMED(LOGNAME, "Something went wrong in grasp generation"); + return false; + } + + // -------------------------------------------- + // Generating a seed state for filtering grasps + robot_state::RobotStatePtr seed_state = + std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState()); + Eigen::Isometry3d eef_mount_grasp_pose = + visual_tools_->convertPose(object_pose) * grasp_data_->tcp_to_eef_mount_.inverse(); + if (!getIKSolution(arm_jmg_, eef_mount_grasp_pose, *seed_state, grasp_data_->parent_link_->getName())) + { + ROS_WARN_STREAM_NAMED(LOGNAME, "The ideal seed state is not reachable. Using start state as seed."); + } + + // -------------------------------------------- + // Filtering grasps + // Note: This step also solves for the grasp and pre-grasp states and stores them in grasp candidates) + bool filter_pregrasps = true; + if (!grasp_filter_->filterGrasps(grasp_candidates, planning_scene_monitor_, arm_jmg_, seed_state, filter_pregrasps, + object_name)){ + ROS_ERROR_STREAM_NAMED(LOGNAME, "Filter grasps failed"); + return false; + } + if (!grasp_filter_->removeInvalidAndFilter(grasp_candidates)){ + ROS_WARN_NAMED(LOGNAME, "Grasp filtering removed all grasps"); + return false; + } + ROS_INFO_STREAM_NAMED(LOGNAME, "" << grasp_candidates.size() << " remain after filtering"); + + // Plan free-space approach, cartesian approach, lift and retreat trajectories + /* + moveit_grasps::GraspCandidatePtr selected_grasp_candidate; + moveit_msgs::MotionPlanResponse pre_approach_plan; + if (!planFullGrasp(grasp_candidates, selected_grasp_candidate, pre_approach_plan, object_name)){ + ROS_ERROR_STREAM_NAMED(LOGNAME, "Failed to plan grasp motions"); + return false; + } + */ + + + return true; + } + +private: + // A shared node handle + ros::NodeHandle nh_; + + // Tool for visualizing things in Rviz + moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; + + // MoveIt! Grasps + moveit_grasps::TwoFingerGraspGeneratorPtr grasp_generator_; + + // Robot-specific data for generating grasps + moveit_grasps::TwoFingerGraspDataPtr grasp_data_; + + // For planning approach and retreats + moveit_grasps::GraspPlannerPtr grasp_planner_; + + // For selecting good grasps + moveit_grasps::TwoFingerGraspFilterPtr grasp_filter_; + + // Shared planning scene (load once for everything) + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // Arm + const robot_model::JointModelGroup* arm_jmg_; + + // Robot + robot_model::RobotModelPtr robot_model_; + + // All the motion planning components + planning_pipeline::PlanningPipelinePtr planning_pipeline_; + + // Choose which arm to use + std::string ee_group_name_; + std::string planning_group_name_; + +}; // end of class + +} // namespace moveit_grasps + +int main(int argc, char* argv[]) +{ + int num_tests = 1; + + ros::init(argc, argv, "grasp_filter_demo"); + + // Allow the action server to recieve and send ros messages + ros::AsyncSpinner spinner(2); + spinner.start(); + + // Seed random + srand(ros::Time::now().toSec()); + + // Benchmark time + ros::Time start_time; + start_time = ros::Time::now(); + + // Head of yaml + + + // Guess I need this + double x_depth = 0.2; + double y_depth = 0.4; + double z_depth = 0.4; + float dimension_x = 0.9; + float dimension_y = 0.9; + float dimension_z = 0; //1.2; + + float resolution = 0.2; + + moveit_grasps::GraspPipeline tester; + std::string object_name = "pick_target"; + + for (float x = 0 - dimension_x; x <= 0 + dimension_x; x+= resolution){ + for(float y = 0 - dimension_y; y <= 0 + dimension_y; y+= resolution){ + for(float z = 0 - dimension_z; z <= 0 + dimension_z; z+= resolution){ + //tester.reachGrasp(x,y,z, object_name, x_depth, y_depth, z_depth); + } + tester.reachGrasp(x,y,dimension_z, object_name, x_depth, y_depth, z_depth); + } + } + + + + + // Benchmark time + double duration = (ros::Time::now() - start_time).toSec(); + + node["description"]["system"] = "Franka Emika 'Panda'"; + node["description"]["resolution"] = resolution; + node["description"]["dimension"] = std::to_string(dimension_x) + " " + std::to_string(dimension_y) + " " + std::to_string(dimension_z) + " (m)"; + node["description"]["timestamp"] = std::to_string(duration) + " sec"; + + std::ofstream file(ros::package::getPath("mtc") + "/maps/dummy.yaml"); + file << node; + + + ROS_INFO_STREAM_NAMED("grasp_filter_demo", "Total time: " << duration << "\t" << num_tests); + std::cout << "Total time: " << duration << "\t" << num_tests << std::endl; + + ros::Duration(1.0).sleep(); // let rviz markers finish publishing + + return 0; +} diff --git a/src/mtc/src/grasp_planner.cpp b/src/mtc/src/grasp_planner.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4cb443cf218ca23386d93795f7726cf1c1b3c96f --- /dev/null +++ b/src/mtc/src/grasp_planner.cpp @@ -0,0 +1,435 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman <dave@picknik.ai> + Desc: Find the approach, lift, and retreat path for a candidate grasp (if a valid one exists) +*/ + +// moveit_grasps +#include "../include/grasp_planner.h" +#include "../include/state_validity_callback.h" +#include <moveit/robot_state/cartesian_interpolator.h> + +// Parameter loading +#include <rosparam_shortcuts/rosparam_shortcuts.h> + +namespace moveit_grasps +{ +constexpr char ENABLED_PARENT_NAME[] = "grasp_planner"; // for namespacing logging messages +constexpr char ENABLED_SETTINGS_NAMESPACE[] = "moveit_grasps/planner"; + +GraspPlanner::GraspPlanner(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools) + : nh_("~"), visual_tools_(visual_tools) +{ + loadEnabledSettings(); +} + +bool GraspPlanner::planAllApproachLiftRetreat( + std::vector<GraspCandidatePtr>& grasp_candidates, const robot_state::RobotStatePtr& robot_state, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const std::string& grasp_object_id) +{ + boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls; + ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor)); + return planAllApproachLiftRetreat(grasp_candidates, robot_state, + static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls), grasp_object_id); +} + +bool GraspPlanner::planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates, + const robot_state::RobotStatePtr& robot_state, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const std::string& grasp_object_id) +{ + ROS_INFO_STREAM_NAMED("grasp_planner", "Planning all remaining grasps with approach lift retreat cartesian path"); + + // For each remaining grasp, calculate entire approach, lift, and retreat path. + // Remove those that have no valid path + bool verbose_cartesian_filtering = isEnabled("verbose_cartesian_filtering"); + std::size_t grasp_candidates_before_cartesian_path = grasp_candidates.size(); + + std::size_t count = 0; + for (std::vector<GraspCandidatePtr>::iterator grasp_it = grasp_candidates.begin(); grasp_it != grasp_candidates.end();) + { + if (!ros::ok()) + return false; + + if (isEnabled("verbose_cartesian_filtering")) + { + ROS_INFO_STREAM_NAMED("grasp_planner", ""); + ROS_INFO_STREAM_NAMED("grasp_planner", "Attempting to plan cartesian grasp path #" + << count++ << ". " << grasp_candidates.size() << " remaining."); + } + + if (!planApproachLiftRetreat(*grasp_it, robot_state, planning_scene, verbose_cartesian_filtering, grasp_object_id)) + { + ROS_INFO_STREAM_NAMED("grasp_planner", "Grasp candidate was unable to find valid cartesian waypoint path"); + + grasp_it = grasp_candidates.erase(grasp_it); // not valid + } + else + { + ++grasp_it; // move to next grasp + } + + if (isEnabled("show_cartesian_waypoints")) + { + visual_tools_->deleteAllMarkers(); + visual_tools_->trigger(); + } + } + + // Results + if (isEnabled("statistics_verbose")) + { + std::cout << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; + std::cout << "Total grasp candidates: " << grasp_candidates_before_cartesian_path << std::endl; + std::cout << "Failed due to invalid cartesian path: " + << grasp_candidates_before_cartesian_path - grasp_candidates.size() << std::endl; + std::cout << "Remaining grasp candidates: " << grasp_candidates.size() << std::endl; + std::cout << "-------------------------------------------------------" << std::endl; + std::cout << std::endl; + } + + // If no grasp candidates had valid paths, then we return false + if (grasp_candidates.size() == 0) + { + ROS_DEBUG_STREAM_NAMED("grasp_planner", "No valid grasp plan possible"); + return false; + } + return true; +} + +bool GraspPlanner::planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, + const robot_state::RobotStatePtr& robot_state, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + bool verbose_cartesian_filtering, const std::string& grasp_object_id) +{ + boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls; + ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor)); + return planApproachLiftRetreat(grasp_candidate, robot_state, + static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls), + verbose_cartesian_filtering, grasp_object_id); +} + +bool GraspPlanner::planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, + const robot_state::RobotStatePtr& robot_state, + const planning_scene::PlanningSceneConstPtr& planning_scene, + bool verbose_cartesian_filtering, const std::string& grasp_object_id) +{ + EigenSTL::vector_Isometry3d waypoints; + GraspGenerator::getGraspWaypoints(grasp_candidate, waypoints); + + // Visualize waypoints + bool show_cartesian_waypoints = isEnabled("show_cartesian_waypoints"); + if (show_cartesian_waypoints) + { + visual_tools_->publishAxisLabeled(waypoints[0], "pregrasp"); + visual_tools_->publishAxisLabeled(waypoints[1], "grasp"); + visual_tools_->publishAxisLabeled(waypoints[2], "lifted"); + visual_tools_->publishAxisLabeled(waypoints[3], "retreat"); + visual_tools_->trigger(); + + if (grasp_candidate->getPreGraspState(visual_tools_->getSharedRobotState())) + { + waitForNextStep("see pre grasp state"); + visual_tools_->publishRobotState(visual_tools_->getSharedRobotState(), rviz_visual_tools::RED); + visual_tools_->trigger(); + } + if (grasp_candidate->getGraspStateClosed(visual_tools_->getSharedRobotState())) + { + waitForNextStep("see grasp state"); + visual_tools_->publishRobotState(visual_tools_->getSharedRobotState(), rviz_visual_tools::BLUE); + visual_tools_->trigger(); + } + waitForNextStep("continue cartesian planning"); + } + + // Remove the pre-grasp point since we plan from that start state + waypoints.erase(waypoints.begin()); + + // Starting state + moveit::core::RobotStatePtr start_state(new moveit::core::RobotState(*robot_state)); + if (!grasp_candidate->getPreGraspState(start_state)) + { + ROS_WARN_STREAM_NAMED("grasp_planner.waypoints", "Unable to set pregrasp"); + return false; + } + + if (!computeCartesianWaypointPath(grasp_candidate, planning_scene, start_state, waypoints, grasp_object_id)) + { + ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Unable to plan approach lift retreat path"); + + return false; + } + + // Feedback + ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Found valid and complete waypoint manipulation path for grasp " + "candidate"); + + // Show visuals + if (show_cartesian_waypoints) + { + ROS_INFO_STREAM_NAMED("grasp_planner.waypoints", "Visualize end effector position of cartesian path for " + << grasp_candidate->segmented_cartesian_traj_.size() + << " segments"); + visual_tools_->publishTrajectoryPoints(grasp_candidate->segmented_cartesian_traj_[APPROACH], + grasp_candidate->grasp_data_->parent_link_, rviz_visual_tools::YELLOW); + visual_tools_->publishTrajectoryPoints(grasp_candidate->segmented_cartesian_traj_[LIFT], + grasp_candidate->grasp_data_->parent_link_, rviz_visual_tools::ORANGE); + visual_tools_->publishTrajectoryPoints(grasp_candidate->segmented_cartesian_traj_[RETREAT], + grasp_candidate->grasp_data_->parent_link_, rviz_visual_tools::RED); + visual_tools_->trigger(); + + bool wait_for_animation = true; + visual_tools_->publishTrajectoryPath(grasp_candidate->segmented_cartesian_traj_[APPROACH], + grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); + visual_tools_->publishTrajectoryPath(grasp_candidate->segmented_cartesian_traj_[LIFT], + grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); + visual_tools_->publishTrajectoryPath(grasp_candidate->segmented_cartesian_traj_[RETREAT], + grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); + } + + if (verbose_cartesian_filtering) + waitForNextStep("try next candidate grasp"); + + return true; +} + +bool GraspPlanner::computeCartesianWaypointPath( + GraspCandidatePtr& grasp_candidate, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const moveit::core::RobotStatePtr& start_state, const EigenSTL::vector_Isometry3d& waypoints, + const std::string& grasp_object_id) +{ + boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls; + ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor)); + + return computeCartesianWaypointPath(grasp_candidate, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls), + start_state, waypoints, grasp_object_id); +} + +bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const moveit::core::RobotStatePtr& start_state, + const EigenSTL::vector_Isometry3d& waypoints, + const std::string& grasp_object_id) +{ + // End effector parent link (arm tip for ik solving) + const moveit::core::LinkModel* ik_tip_link = grasp_candidate->grasp_data_->parent_link_; + + // Resolution of trajectory + // The maximum distance in Cartesian space between consecutive points on the resulting path + const double max_step = 0.01; + + // Jump threshold for preventing consequtive joint values from 'jumping' by a large amount in joint space + const double jump_threshold = 4; // config_->jump_threshold_; // aka jump factor + + // Collision setting + const bool collision_checking_verbose = isEnabled("collision_checking_verbose"); + const bool only_check_self_collision = false; + + // Reference frame setting + const bool global_reference_frame = true; + + // Check for kinematic solver + if (!grasp_candidate->grasp_data_->arm_jmg_->canSetStateFromIK(ik_tip_link->getName())) + { + ROS_ERROR_STREAM_NAMED("grasp_planner.waypoints", "No IK Solver loaded - make sure moveit_config/kinamatics.yaml " + "is loaded in this namespace"); + return false; + } + + std::size_t attempts = 0; + static const std::size_t MAX_IK_ATTEMPTS = 5; + bool valid_path_found = false; + while (attempts < MAX_IK_ATTEMPTS) + { + if (attempts > 0) + { + ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Attempting IK solution, attempt # " << attempts + 1); + } + attempts++; + + moveit::core::RobotStatePtr start_state_copy(new moveit::core::RobotState(*start_state)); + if (!grasp_candidate->getPreGraspState(start_state_copy)) + { + ROS_ERROR_STREAM_NAMED("grasp_planner.waypoints", "Unable to set pregrasp"); + return false; + } + + // Create a clone where we modify the ACM to disable collision checking between the end effector and object + planning_scene::PlanningScenePtr scene = planning_scene::PlanningScene::clone(planning_scene); + // If the grasp_object_id is set then we disable collision checking in the planning scene clone + if (!grasp_object_id.empty() && scene->knowsFrameTransform(grasp_object_id)) + { + std::vector<std::string> ee_link_names = grasp_candidate->grasp_data_->ee_jmg_->getLinkModelNames(); + for (const auto& ee_link : ee_link_names) + scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object_id, ee_link, true); + } + // Collision check + moveit::core::GroupStateValidityCallbackFn constraint_fn = + boost::bind(&isGraspStateValid, scene.get(), collision_checking_verbose, only_check_self_collision, + visual_tools_, _1, _2, _3); + + // Compute Cartesian Path + grasp_candidate->segmented_cartesian_traj_.clear(); + grasp_candidate->segmented_cartesian_traj_.resize(3); + double valid_approach_percentage = robot_state::CartesianInterpolator::computeCartesianPath( + start_state_copy.get(), grasp_candidate->grasp_data_->arm_jmg_, + grasp_candidate->segmented_cartesian_traj_[APPROACH], ik_tip_link, waypoints[APPROACH], global_reference_frame, + robot_state::MaxEEFStep(max_step), robot_state::JumpThreshold(jump_threshold), constraint_fn, + kinematics::KinematicsQueryOptions()); + + if (!grasp_candidate->getGraspStateClosedEEOnly(start_state_copy)) + { + ROS_ERROR_STREAM_NAMED("grasp_planner", "Unable to set pregrasp"); + return false; + } + + // Attach CO to end effector + if (!grasp_object_id.empty() && scene->knowsFrameTransform(grasp_object_id)) + { + // Create an AttachedCollisionObject + moveit_msgs::AttachedCollisionObject aco; + + // Create a reference to the collision object for convenience + moveit_msgs::CollisionObject& suction_voxel_co = aco.object; + + suction_voxel_co.id = grasp_object_id; + suction_voxel_co.header.frame_id = ik_tip_link->getName(); + + // Set the aco attached link name + aco.link_name = ik_tip_link->getName(); + + // move the robot in the ps to the end of the approach path. + scene->setCurrentState(*start_state_copy); + + // Mark object to be added + suction_voxel_co.operation = moveit_msgs::CollisionObject::ADD; + if (!scene->processAttachedCollisionObjectMsg(aco)) + { + ROS_WARN_STREAM_NAMED("grasp_planner", "Failed to attach: " << aco.object.id); + } + else + { + auto start_state_with_body = std::make_shared<robot_state::RobotState>(scene->getCurrentState()); + std::vector<const robot_state::AttachedBody*> attached_bodies; + start_state_with_body->getAttachedBodies(attached_bodies); + for (const auto& ab : attached_bodies) + start_state_copy->attachBody(ab->getName(), ab->getPose(), ab->getShapes(), ab->getShapePoses(), + ab->getTouchLinks(), ab->getAttachedLinkName(), ab->getDetachPosture(), + ab->getSubframes()); + constraint_fn = boost::bind(&isGraspStateValid, scene.get(), collision_checking_verbose, + only_check_self_collision, visual_tools_, _1, _2, _3); + } + } + + double valid_lift_retreat_percentage = robot_state::CartesianInterpolator::computeCartesianPath( + start_state_copy.get(), grasp_candidate->grasp_data_->arm_jmg_, + grasp_candidate->segmented_cartesian_traj_[LIFT], ik_tip_link, waypoints[LIFT], global_reference_frame, + robot_state::MaxEEFStep(max_step), robot_state::JumpThreshold(jump_threshold), constraint_fn, + kinematics::KinematicsQueryOptions()); + + valid_lift_retreat_percentage *= robot_state::CartesianInterpolator::computeCartesianPath( + start_state_copy.get(), grasp_candidate->grasp_data_->arm_jmg_, + grasp_candidate->segmented_cartesian_traj_[RETREAT], ik_tip_link, waypoints[RETREAT], global_reference_frame, + robot_state::MaxEEFStep(max_step), robot_state::JumpThreshold(jump_threshold), constraint_fn, + kinematics::KinematicsQueryOptions()); + + ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "valid_approach_percentage: " << valid_approach_percentage + << " \tvalid_lift_retreat_" + "percentage: " + << valid_lift_retreat_percentage); + + // The retreat has to work for the most part but doesn't need to be perfect + double min_allowed_valid_lift_retreat_percentage = 0.90; + if (valid_approach_percentage == 1 && valid_lift_retreat_percentage >= min_allowed_valid_lift_retreat_percentage) + { + ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Found valid cartesian path"); + valid_path_found = true; + break; + } + } // end while AND scoped pointer of locked planning scenep + + if (!valid_path_found) + { + ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", + "UNABLE to find valid waypoint cartesian path after " << MAX_IK_ATTEMPTS << " attempts"); + return false; + } + + return true; +} + +void GraspPlanner::waitForNextStep(const std::string& message) +{ + if (wait_for_next_step_callback_) + wait_for_next_step_callback_(message); +} + +void GraspPlanner::setWaitForNextStepCallback(WaitForNextStepCallback callback) +{ + wait_for_next_step_callback_ = callback; +} + +bool GraspPlanner::loadEnabledSettings() +{ + // Check if the map has been loaded yet + if (!enabled_settings_loaded_) + { + enabled_settings_loaded_ = true; + return rosparam_shortcuts::get(ENABLED_PARENT_NAME, nh_, ENABLED_SETTINGS_NAMESPACE, enabled_setting_); + } + return true; +} + +bool GraspPlanner::isEnabled(const std::string& setting_name) +{ + // Check if the map has been loaded yet. it is preferred if this is called manually + if (!enabled_settings_loaded_) + ROS_ERROR_STREAM_NAMED("rosparam_shortcuts", "Enabled settings are not yet loaded e.g. call loadEnabledSettings()"); + + std::map<std::string, bool>::iterator it = enabled_setting_.find(setting_name); + if (it != enabled_setting_.end()) + { + // Element found; + return it->second; + } + ROS_ERROR_STREAM_NAMED("rosparam_shortcuts", "isEnabled() key '" << nh_.getNamespace() << "/" + << ENABLED_SETTINGS_NAMESPACE << "/" << setting_name + << "' does not exist on the parameter server"); + + return false; +} + +} // namespace moveit_grasps diff --git a/src/mtc/src/grasp_scorer.cpp b/src/mtc/src/grasp_scorer.cpp new file mode 100644 index 0000000000000000000000000000000000000000..934e4832e61a7d44a12006b281cc4aaece2d255c --- /dev/null +++ b/src/mtc/src/grasp_scorer.cpp @@ -0,0 +1,165 @@ +/********************************************************************* + * Software License Agreement ("Modified BSD License") + * + * Copyright (c) 2014, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/** + * Authors : Andy McEvoy ( mcevoy.andy@gmail.com ) + * Desc : Functions for scoring grasps. See *.h file for documentation + */ + +#include "../include/grasp_scorer.h" + +namespace moveit_grasps +{ +double GraspScoreWeights::computeScore(const Eigen::Vector3d& orientation_scores, + const Eigen::Vector3d& translation_scores, bool verbose) const +{ + double total_score = + orientation_scores[0] * orientation_x_score_weight_ + orientation_scores[1] * orientation_y_score_weight_ + + orientation_scores[2] * orientation_z_score_weight_ + translation_scores[0] * translation_x_score_weight_ + + translation_scores[1] * translation_y_score_weight_ + translation_scores[2] * translation_z_score_weight_; + + total_score /= getWeightTotal(); + + if (verbose) + { + static const std::string logger_name = "grasp_scorer.compute_score"; + ROS_DEBUG_STREAM_NAMED(logger_name, "Grasp score: "); + // clang-format off + ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.x = " << orientation_scores[0] << "\tweight = "<< orientation_x_score_weight_); + ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.y = " << orientation_scores[1] << "\tweight = "<< orientation_y_score_weight_); + ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.z = " << orientation_scores[2] << "\tweight = "<< orientation_z_score_weight_); + ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.x = " << translation_scores[0] << "\tweight = "<< translation_x_score_weight_); + ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.y = " << translation_scores[1] << "\tweight = "<< translation_y_score_weight_); + ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.z = " << translation_scores[2] << "\tweight = "<< translation_z_score_weight_); + // Total + ROS_DEBUG_STREAM_NAMED(logger_name, "\ttotal_score = " << total_score); + // clang-format on + } + return total_score; +} + +double GraspScoreWeights::getWeightTotal() const +{ + return orientation_x_score_weight_ + orientation_y_score_weight_ + orientation_z_score_weight_ + + translation_x_score_weight_ + translation_y_score_weight_ + translation_z_score_weight_; +} + +Eigen::Vector3d GraspScorer::scoreRotationsFromDesired(const Eigen::Isometry3d& grasp_pose_tcp, + const Eigen::Isometry3d& ideal_pose) +{ + Eigen::Vector3d grasp_pose_axis; + Eigen::Vector3d ideal_pose_axis; + Eigen::Vector3d scores; + double cos_angle; + double angle; + + // get angle between x-axes + grasp_pose_axis = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitX(); + ideal_pose_axis = ideal_pose.rotation() * Eigen::Vector3d::UnitX(); + cos_angle = grasp_pose_axis.dot(ideal_pose_axis); + angle = acos(std::max(-1.0, std::min(1.0, cos_angle))); + ROS_DEBUG_STREAM_NAMED("grasp_scorer.angle", "x angle = " << angle * 180.0 / M_PI); + scores[0] = (M_PI - angle) / M_PI; + + // get angle between y-axes + grasp_pose_axis = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitY(); + ideal_pose_axis = ideal_pose.rotation() * Eigen::Vector3d::UnitY(); + cos_angle = grasp_pose_axis.dot(ideal_pose_axis); + angle = acos(std::max(-1.0, std::min(1.0, cos_angle))); + ROS_DEBUG_STREAM_NAMED("grasp_scorer.angle", "y angle = " << angle * 180.0 / M_PI); + scores[1] = (M_PI - angle) / M_PI; + + // get angle between z-axes + grasp_pose_axis = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ(); + ideal_pose_axis = ideal_pose.rotation() * Eigen::Vector3d::UnitZ(); + cos_angle = grasp_pose_axis.dot(ideal_pose_axis); + angle = acos(std::max(-1.0, std::min(1.0, cos_angle))); + ROS_DEBUG_STREAM_NAMED("grasp_scorer.angle", "z angle = " << angle * 180.0 / M_PI); + scores[2] = (M_PI - angle) / M_PI; + + return scores; +} + +Eigen::Vector3d GraspScorer::scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp, + const Eigen::Isometry3d& ideal_pose) +{ + // We assume that the ideal is in the middle + Eigen::Vector3d scores = -Eigen::Vector3d(grasp_pose_tcp.translation() - ideal_pose.translation()).array().abs(); + + ROS_DEBUG_STREAM_NAMED("grasp_scorer.scoreGraspTranslation", + "value, ideal, score:\n" + << "x: " << grasp_pose_tcp.translation()[0] << "\t" << ideal_pose.translation()[0] << "\t" + << scores[0] << "\n" + << "y: " << grasp_pose_tcp.translation()[1] << "\t" << ideal_pose.translation()[1] << "\t" + << scores[1] << "\n" + << "x: " << grasp_pose_tcp.translation()[2] << "\t" << ideal_pose.translation()[2] << "\t" + << scores[2] << "\n"); + + return scores; +} + +Eigen::Vector3d GraspScorer::scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp, + const Eigen::Vector3d& min_translations, + const Eigen::Vector3d& max_translations) +{ + Eigen::Vector3d scores; + + for (std::size_t i = 0; i < 3; ++i) + { + // We assume that the ideal is in the middle + double ideal = (max_translations[i] + min_translations[i]) / 2; + double translation = grasp_pose_tcp.translation()[i] - ideal; + double range = max_translations[i] - min_translations[i]; + double score; + if (range == 0) + score = 0; + else + score = translation / range; + + scores[i] = pow(score, 2); + } + + ROS_DEBUG_STREAM_NAMED("grasp_scorer.translation", + "\nvalue, min, max, score:\n" + << grasp_pose_tcp.translation()[0] << ", " << min_translations[0] << ", " + << max_translations[0] << ", " << scores[0] << "\n" + << grasp_pose_tcp.translation()[1] << ", " << min_translations[1] << ", " + << max_translations[1] << ", " << scores[1] << "\n" + << grasp_pose_tcp.translation()[2] << ", " << min_translations[2] << ", " + << max_translations[2] << ", " << scores[2] << "\n"); + + return scores; +} + +} // namespace moveit_grasps diff --git a/src/mtc/src/impl/base_by_rotation.cpp b/src/mtc/src/impl/base_by_rotation.cpp new file mode 100644 index 0000000000000000000000000000000000000000..495a480d77a9b124c2721bba0adba1bfbbbc6868 --- /dev/null +++ b/src/mtc/src/impl/base_by_rotation.cpp @@ -0,0 +1,5 @@ +#include "impl/base_by_rotation.h" + +void Base_by_rotation::base_algorithm(){ + ROS_INFO("hello"); +} \ No newline at end of file diff --git a/src/mtc/src/impl/map_loader.cpp b/src/mtc/src/impl/map_loader.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c1b5a3f560374ba32f0bcb76922a32d5c73ea59c --- /dev/null +++ b/src/mtc/src/impl/map_loader.cpp @@ -0,0 +1,57 @@ +#include <ros/ros.h> +#include "impl/abstract_map_loader.h" +#include "impl/map_loader.h" + + + +Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){ + std::regex reg("\\s+"); + std::vector<geometry_msgs::Pose> inv_map; + + ROS_INFO("setup inverse map calulation"); + for (int i = 0; i < map.size();i++){ + std::string str = map[i]; + std::sregex_token_iterator to_it(str.begin(), str.end(),reg,-1); + std::vector<std::string> temp{to_it, {}}; + tf2::Vector3 t(std::stof(temp[0]), std::stof(temp[1]), std::stof(temp[2])); + tf2::Quaternion so(std::stof(temp[3]), std::stof(temp[4]), std::stof(temp[5]), std::stof(temp[6])); + + tf2::Transform trans(so, t); + trans = trans.inverse(); + geometry_msgs::Pose pose; + pose.position.x = trans.getOrigin().getX(); + pose.position.y = trans.getOrigin().getY(); + pose.position.z = trans.getOrigin().getZ(); + pose.orientation.x = trans.getRotation().getX(); + pose.orientation.y = trans.getRotation().getY(); + pose.orientation.z = trans.getRotation().getZ(); + pose.orientation.w = trans.getRotation().getW(); + inv_map.push_back(pose); + } + this->set_inv_map(inv_map); + + std::vector<std::vector<geometry_msgs::Point>> task_; + for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = task.begin(); it != task.end(); ++it){ + std::vector<geometry_msgs::Point> task_per_robot; + for (int i = 0; i < it->second.size(); i++){ + double x = it->second[i][0]; + double y = it->second[i][1]; + double z = it->second[i][2]; + geometry_msgs::Point point; + point.x = x; + point.y = y; + point.z = z; + + task_per_robot.push_back(point); + } + task_.push_back(task_per_robot); + } + + this->set_task(task_); + + ROS_INFO("Map is loaded with value %li", inv_map.size()); +} + +void Map_loader::base_calculation(){ + this->get_strategy()->base_algorithm(); +} \ No newline at end of file diff --git a/src/mtc/src/mtc2taskspace.cpp b/src/mtc/src/mtc2taskspace.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b8c58f8091983555f7df02a7c89842e7ace60a34 --- /dev/null +++ b/src/mtc/src/mtc2taskspace.cpp @@ -0,0 +1,81 @@ +#include "../include/mtc2taskspace.h" + +int main(int argc, char **argv){ + ros::init(argc, argv, "mtc2taskspace"); + ros::NodeHandle n; + + std::string path; + XmlRpc::XmlRpcValue resources; + n.getParam("/objects", resources); + + std::vector<tf2::Vector3> robot_1; + std::vector<tf2::Vector3> robot_2; + std::vector<tf2::Vector3> robot_middle; + std::vector<tf2::Vector3>* robot_pointer = nullptr; + + tf2::Vector3 pos(0,0,0); + // resources is a TypeArray + for (int i = 0; i < resources.size(); i++){ + // resources[i] is a struct, which could be a location, ot the table itself, tricky + // every struct has 5 values, but not sorted, dont know why + XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = resources[i].begin(); + + for(; it != resources[i].end(); ++it){ + if(it->first == "id"){ + ROS_INFO("hey"); + std::string str = it->second; + if(str[0] == 'A') robot_pointer = &robot_1; + if(str[0] == 'B') robot_pointer = &robot_1; + if(str[0] == 'C') robot_pointer = &robot_1; + if(str[0] == 'D') robot_pointer = &robot_1; + if(str[0] == 'E') robot_pointer = &robot_middle; + if(str[0] == 'F') robot_pointer = &robot_middle; + if(str[0] == 'G') robot_pointer = &robot_2; + if(str[0] == 'H') robot_pointer = &robot_2; + if(str[0] == 'I') robot_pointer = &robot_2; + if(str[0] == 'J') robot_pointer = &robot_2; + } + } + + if (robot_pointer == nullptr) continue; + it = resources[i].begin(); + + for(; it != resources[i].end(); ++it){ + if(it->first == "pos"){ + XmlRpc::XmlRpcValue::ValueStruct::const_iterator it_1 = it->second.begin(); + for(; it_1 != it->second.end(); ++it_1){ + float t = (it_1->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(it_1->second) : (double) it_1->second; + if(it_1->first == "x") pos.setX(t); + if(it_1->first == "y") pos.setY(t); + if(it_1->first == "z") pos.setZ(t); + } + robot_pointer->push_back(pos); + pos.setZero(); + } + } + + robot_pointer == nullptr; + } + + // lets make some result data + std::ofstream o(ros::package::getPath("mtc") + "/descriptions/dummy.yaml"); + YAML::Node node; + for(tf2::Vector3& vec : robot_1){ + std::vector<float> arr = {(float)vec.getX(), (float) vec.getY(), (float)vec.getZ()}; + node["task"]["groups"]["robot_x"].push_back(arr); + } + + for(tf2::Vector3& vec : robot_2){ + std::vector<float> arr = {(float)vec.getX(), (float) vec.getY(), (float)vec.getZ()}; + node["task"]["groups"]["robot_y"].push_back(arr); + } + + for(tf2::Vector3& vec : robot_middle){ + std::vector<float> arr = {(float)vec.getX(), (float) vec.getY(), (float)vec.getZ()}; + node["task"]["groups"]["robot_x"].push_back(arr); + node["task"]["groups"]["robot_y"].push_back(arr); + } + + o << node; + return 0; +} \ No newline at end of file diff --git a/src/mtc/src/two_finger_grasp_data.cpp b/src/mtc/src/two_finger_grasp_data.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f2db2eb0d9fc5891f89069a42a6f5798917f034c --- /dev/null +++ b/src/mtc/src/two_finger_grasp_data.cpp @@ -0,0 +1,215 @@ +/* + * Software License Agreement (Modified BSD License) + * + * Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L. + * 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 Univ of CO, Boulder, PAL Robotics, S.L. + * nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Authors: Dave Coleman, Bence Magyar + Description: Data class used by the grasp generator. +*/ + +#include "../include/two_finger_grasp_data.h" + +// Eigen +#include <Eigen/Core> +#include <Eigen/Geometry> +#include <eigen_conversions/eigen_msg.h> + +// C++ +#include <cmath> + +// Parameter loading +#include <rosparam_shortcuts/rosparam_shortcuts.h> + +// Pose conversion +#include <rviz_visual_tools/rviz_visual_tools.h> + +namespace moveit_grasps +{ +const std::string LOGNAME = "grasp_data.two_finger_gripper"; + +TwoFingerGraspData::TwoFingerGraspData(const ros::NodeHandle& nh, const std::string& end_effector, + const moveit::core::RobotModelConstPtr& robot_model) + : GraspData(nh, end_effector, robot_model) +{ +} + +bool TwoFingerGraspData::loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector) +{ + if (!GraspData::loadGraspData(nh, end_effector)) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "GraspData::loadGraspData failed"); + return false; + } + // Load all other parameters + const std::string parent_name = "grasp_data"; // for namespacing logging messages + std::size_t error = 0; + + // Search within the sub-namespace of this end effector name + ros::NodeHandle child_nh(nh, end_effector); + + error += !rosparam_shortcuts::get(parent_name, child_nh, "gripper_finger_width", gripper_finger_width_); + error += !rosparam_shortcuts::get(parent_name, child_nh, "max_grasp_width", max_grasp_width_); + error += !rosparam_shortcuts::get(parent_name, child_nh, "max_finger_width", max_finger_width_); + error += !rosparam_shortcuts::get(parent_name, child_nh, "min_finger_width", min_finger_width_); + rosparam_shortcuts::shutdownIfError(parent_name, error); + + return true; +} + +bool TwoFingerGraspData::setGraspWidth(double fraction_open, double min_finger_width, + trajectory_msgs::JointTrajectory& grasp_posture) +{ + if (fraction_open < 0 || fraction_open > 1) + { + ROS_ERROR_STREAM_NAMED("grasp_data", "Invalid fraction passed in " << fraction_open); + return false; + } + + // Ensure min_finger_width is not less than actual min finger width + double min_finger_width_adjusted = std::max(min_finger_width, min_finger_width_); + + // Max width = max_finger_width_ + // Min width = min_finger_width_adjusted + double distance_btw_fingers = + min_finger_width_adjusted + (max_finger_width_ - min_finger_width_adjusted) * fraction_open; + return fingerWidthToGraspPosture(distance_btw_fingers, grasp_posture); +} + +bool TwoFingerGraspData::fingerWidthToGraspPosture(double distance_btw_fingers, + trajectory_msgs::JointTrajectory& grasp_posture) +{ + // TODO(mlautman): Change this function to take in a method for translating joint values to grasp width + // Currently this function simply interpolates between max open and max closed + ROS_DEBUG_STREAM_NAMED("grasp_data", + "Setting grasp posture to have distance_between_fingers of " << distance_btw_fingers); + + // Error check + if (distance_btw_fingers > max_finger_width_ + std::numeric_limits<double>::epsilon() || + distance_btw_fingers < min_finger_width_ - std::numeric_limits<double>::epsilon()) + { + ROS_DEBUG_STREAM_NAMED("grasp_data", "Requested " << distance_btw_fingers << " is beyond limits of " + << min_finger_width_ << "," << max_finger_width_); + return false; + } + + // NOTE: We assume a linear relationship between the actuated joint values and the distance between fingers. + // This is probably incorrect but until we expose an interface for passing in a function to translate from + // joint values to grasp width, it's the best we got... + // TODO(mlautman): Make it so that a user can pass in a function here. + const std::vector<std::string>& joint_names = pre_grasp_posture_.joint_names; + const std::vector<double>& grasp_pose = grasp_posture_.points[0].positions; + const std::vector<double>& pre_grasp_pose = pre_grasp_posture_.points[0].positions; + if (joint_names.size() != grasp_pose.size() || grasp_pose.size() != pre_grasp_pose.size()) + { + ROS_ERROR_NAMED("grasp_data", + "Mismatched vector sizes joint_names.size()=%zu, grasp_pose.size()=%zu, and " + "pre_grasp_pose.size()=%zu", + joint_names.size(), grasp_pose.size(), pre_grasp_pose.size()); + return false; + } + + std::vector<double> slope(joint_names.size()); + std::vector<double> intercept(joint_names.size()); + std::vector<double> joint_positions(joint_names.size()); + for (std::size_t joint_index = 0; joint_index < joint_names.size(); joint_index++) + { + slope[joint_index] = + (max_finger_width_ - min_finger_width_) / (pre_grasp_pose[joint_index] - grasp_pose[joint_index]); + intercept[joint_index] = max_finger_width_ - slope[joint_index] * pre_grasp_pose[joint_index]; + + // Sanity check + ROS_ASSERT_MSG(intercept[joint_index] == min_finger_width_ - slope[joint_index] * grasp_pose[joint_index], + "we got different y intercept!! %.3f and %.3f", intercept[joint_index], + min_finger_width_ - slope[joint_index] * grasp_pose[joint_index]); + + joint_positions[joint_index] = (distance_btw_fingers - intercept[joint_index]) / slope[joint_index]; + + ROS_DEBUG_NAMED("grasp_data", "Converted joint %s to position %.3f", joint_names[joint_index].c_str(), + joint_positions[joint_index]); + } + + return jointPositionsToGraspPosture(joint_positions, grasp_posture); +} + +bool TwoFingerGraspData::jointPositionsToGraspPosture(const std::vector<double>& joint_positions, + trajectory_msgs::JointTrajectory& grasp_posture) +{ + for (std::size_t joint_index = 0; joint_index < pre_grasp_posture_.joint_names.size(); joint_index++) + { + const moveit::core::JointModel* joint = robot_model_->getJointModel(pre_grasp_posture_.joint_names[joint_index]); + ROS_ASSERT_MSG(joint->getVariableBounds().size() > 0, "joint->getVariableBounds() is empty for %s", + pre_grasp_posture_.joint_names[joint_index].c_str()); + const moveit::core::VariableBounds& bound = joint->getVariableBounds()[0]; + + if (joint_positions[joint_index] > bound.max_position_ || joint_positions[joint_index] < bound.min_position_) + { + ROS_ERROR_STREAM_NAMED("grasp_data", "Requested joint " << pre_grasp_posture_.joint_names[joint_index].c_str() + << "at index" << joint_index << " with value " + << joint_positions[joint_index] << " is beyond limits of " + << bound.min_position_ << ", " << bound.max_position_); + return false; + } + } + + // Get default grasp posture + grasp_posture = grasp_posture_; + + // Error check + if (joint_positions.size() != grasp_posture.points.front().positions.size()) + { + ROS_ERROR_STREAM_NAMED("grasp_data", + "Not enough finger joints passed in: " << joint_positions.size() << " positions but expect " + << grasp_posture.points.front().positions.size()); + return false; + } + + // Set joint positions + grasp_posture.points.front().positions = joint_positions; + + return true; +} + +void TwoFingerGraspData::print() +{ + GraspData::print(); + + std::cout << "Finger Gripper Parameters: " << std::endl; + std::cout << "\tpre_grasp_posture_: \n" << pre_grasp_posture_ << std::endl; + std::cout << "\tgrasp_posture_: \n" << grasp_posture_ << std::endl; + std::cout << "\tgripper_finger_width_: " << gripper_finger_width_ << std::endl; + std::cout << "\tmax_grasp_width_: " << max_grasp_width_ << std::endl; + std::cout << "\tmax_finger_width_: " << max_finger_width_ << std::endl; + std::cout << "\tmin_finger_width_: " << min_finger_width_ << std::endl; +} + +} // namespace moveit_grasps diff --git a/src/mtc/src/two_finger_grasp_filter.cpp b/src/mtc/src/two_finger_grasp_filter.cpp new file mode 100644 index 0000000000000000000000000000000000000000..11d2376983c8368e135441d014e061f919c1f681 --- /dev/null +++ b/src/mtc/src/two_finger_grasp_filter.cpp @@ -0,0 +1,95 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman <dave@picknik.ai> + Desc: Filters grasps based on kinematic feasibility and collision +*/ + +// Parent Class +#include "../include/state_validity_callback.h" + +// moveit_grasps +#include "../include/two_finger_grasp_filter.h" + +// moveit +#include <moveit/transforms/transforms.h> +#include <moveit/collision_detection/collision_tools.h> + +namespace moveit_grasps +{ +// Constructor +TwoFingerGraspFilter::TwoFingerGraspFilter(const robot_state::RobotStatePtr& robot_state, + const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools) + : GraspFilter(robot_state, visual_tools), name_("two_finger_grasp_filter") +{ +} + +bool TwoFingerGraspFilter::processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct) +{ + bool filer_results = GraspFilter::processCandidateGrasp(ik_thread_struct); + if (!filer_results) + return false; + + // Helper pointer + GraspCandidatePtr& grasp_candidate = ik_thread_struct->grasp_candidates_[ik_thread_struct->grasp_id]; + + moveit::core::GroupStateValidityCallbackFn constraint_fn = + boost::bind(&isGraspStateValid, ik_thread_struct->planning_scene_.get(), + collision_verbose_ || ik_thread_struct->visual_debug_, collision_verbose_speed_, visual_tools_, _1, + _2, _3); + + // Check if IK solution for grasp pose is valid for fingers closed as well + return checkFingersClosedIK(grasp_candidate->grasp_ik_solution_, ik_thread_struct, grasp_candidate, constraint_fn); +} + +bool TwoFingerGraspFilter::checkFingersClosedIK(std::vector<double>& ik_solution, + const IkThreadStructPtr& ik_thread_struct, + GraspCandidatePtr& grasp_candidate, + const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const +{ + // Set gripper position (how open the fingers are) to CLOSED + grasp_candidate->getGraspStateClosedEEOnly(ik_thread_struct->robot_state_); + + // Check constraint function + if (!constraint_fn(ik_thread_struct->robot_state_.get(), grasp_candidate->grasp_data_->arm_jmg_, &ik_solution[0])) + { + ROS_WARN_STREAM_NAMED(name_ + ".superdebug", "Grasp filtered because in collision with fingers CLOSED"); + grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_IK_CLOSED; + return false; + } + + return true; +} + +} // namespace moveit_grasps diff --git a/src/mtc/src/two_finger_grasp_generator.cpp b/src/mtc/src/two_finger_grasp_generator.cpp new file mode 100644 index 0000000000000000000000000000000000000000..969967742f554dab2d52d00c465ed10a9688a85d --- /dev/null +++ b/src/mtc/src/two_finger_grasp_generator.cpp @@ -0,0 +1,887 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy + Desc: Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches +*/ + +#include "../include/two_finger_grasp_generator.h" +#include "../include/grasp_filter.h" + +#include <rosparam_shortcuts/rosparam_shortcuts.h> + +namespace +{ +void debugFailedOpenGripper(double percent_open, double min_finger_open_on_approach, double object_width, + double grasp_padding_on_approach) +{ + ROS_ERROR_STREAM_NAMED("grasp_generator", "Unable to set grasp width to " + << percent_open << " % open. Stats:" + << "\n min_finger_open_on_approach: \t " << min_finger_open_on_approach + << "\n object_width: \t " << object_width + << "\n grasp_padding_on_approach_: \t " << grasp_padding_on_approach); +} + +} // namespace + +namespace moveit_grasps +{ +TwoFingerGraspCandidateConfig::TwoFingerGraspCandidateConfig() + : enable_corner_grasps_(true) + , enable_face_grasps_(true) + , enable_variable_angle_grasps_(true) + , enable_edge_grasps_(true) + , generate_x_axis_grasps_(true) + , generate_y_axis_grasps_(true) + , generate_z_axis_grasps_(true) +{ +} +void TwoFingerGraspCandidateConfig::enableAllGraspTypes() +{ + enable_corner_grasps_ = true; + enable_face_grasps_ = true; + enable_variable_angle_grasps_ = true; + enable_edge_grasps_ = true; +} +void TwoFingerGraspCandidateConfig::enableAllGraspAxes() +{ + generate_x_axis_grasps_ = true; + generate_y_axis_grasps_ = true; + generate_z_axis_grasps_ = true; +} +void TwoFingerGraspCandidateConfig::enableAll() +{ + enableAllGraspTypes(); + enableAllGraspAxes(); +} +void TwoFingerGraspCandidateConfig::disableAllGraspTypes() +{ + enable_corner_grasps_ = false; + enable_face_grasps_ = false; + enable_variable_angle_grasps_ = false; + enable_edge_grasps_ = false; +} +void TwoFingerGraspCandidateConfig::disableAllGraspAxes() +{ + generate_x_axis_grasps_ = false; + generate_y_axis_grasps_ = false; + generate_z_axis_grasps_ = false; +} +void TwoFingerGraspCandidateConfig::disableAll() +{ + disableAllGraspTypes(); + disableAllGraspAxes(); +} + +// Constructor +TwoFingerGraspGenerator::TwoFingerGraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, + bool verbose) + : GraspGenerator(visual_tools, verbose) +{ + auto two_finger_grasp_score_weights = std::make_shared<TwoFingerGraspScoreWeights>(); + grasp_score_weights_ = std::dynamic_pointer_cast<GraspScoreWeights>(two_finger_grasp_score_weights); + + grasp_candidate_config_ = TwoFingerGraspCandidateConfig(); +} + +void TwoFingerGraspGenerator::setGraspCandidateConfig(const TwoFingerGraspCandidateConfig& grasp_candidate_config) +{ + grasp_candidate_config_ = grasp_candidate_config; +} + +bool TwoFingerGraspGenerator::generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, + double height, const moveit_grasps::GraspDataPtr& grasp_data, + std::vector<GraspCandidatePtr>& grasp_candidates) +{ + auto two_finger_grasp_data = std::dynamic_pointer_cast<TwoFingerGraspData>(grasp_data); + if (!two_finger_grasp_data) + { + ROS_ERROR_STREAM_NAMED("grasp_generator", + "grasp_data is not castable to TwoFingerGraspData. Make sure you are using " + "the child class"); + return false; + } + return generateGrasps(cuboid_pose, depth, width, height, two_finger_grasp_data, grasp_candidates); +} + +bool TwoFingerGraspGenerator::generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, + double height, const TwoFingerGraspDataPtr& grasp_data, + std::vector<GraspCandidatePtr>& grasp_candidates) +{ + bool result = + generateFingerGrasps(cuboid_pose, depth, width, height, grasp_data, grasp_candidates, grasp_candidate_config_); + + if (result) + std::sort(grasp_candidates.begin(), grasp_candidates.end(), GraspFilter::compareGraspScores); + + return result; +} + +bool TwoFingerGraspGenerator::addGrasp(const Eigen::Isometry3d& grasp_pose_eef_mount, + const TwoFingerGraspDataPtr& grasp_data, const Eigen::Isometry3d& object_pose, + const Eigen::Vector3d& object_size, double object_width, + std::vector<GraspCandidatePtr>& grasp_candidates) +{ + // Transform the grasp pose eef mount to the tcp grasp pose + Eigen::Isometry3d grasp_pose_tcp = grasp_pose_eef_mount * grasp_data->tcp_to_eef_mount_.inverse(); + + // The new grasp + moveit_msgs::Grasp new_grasp; + + // Approach and retreat - aligned with eef to grasp transform + // set pregrasp + moveit_msgs::GripperTranslation pre_grasp_approach; + new_grasp.pre_grasp_approach.direction.header.stamp = ros::Time::now(); + new_grasp.pre_grasp_approach.desired_distance = grasp_data->grasp_max_depth_ + grasp_data->approach_distance_desired_; + new_grasp.pre_grasp_approach.min_distance = 0; // NOT IMPLEMENTED + new_grasp.pre_grasp_approach.direction.header.frame_id = grasp_data->parent_link_->getName(); + + Eigen::Vector3d grasp_approach_vector = -1 * grasp_data->tcp_to_eef_mount_.translation(); + grasp_approach_vector = grasp_approach_vector / grasp_approach_vector.norm(); + + new_grasp.pre_grasp_approach.direction.vector.x = grasp_approach_vector.x(); + new_grasp.pre_grasp_approach.direction.vector.y = grasp_approach_vector.y(); + new_grasp.pre_grasp_approach.direction.vector.z = grasp_approach_vector.z(); + + // set postgrasp + moveit_msgs::GripperTranslation post_grasp_retreat; + new_grasp.post_grasp_retreat.direction.header.stamp = ros::Time::now(); + new_grasp.post_grasp_retreat.desired_distance = grasp_data->grasp_max_depth_ + grasp_data->retreat_distance_desired_; + new_grasp.post_grasp_retreat.min_distance = 0; // NOT IMPLEMENTED + new_grasp.post_grasp_retreat.direction.header.frame_id = grasp_data->parent_link_->getName(); + new_grasp.post_grasp_retreat.direction.vector.x = -1 * grasp_approach_vector.x(); + new_grasp.post_grasp_retreat.direction.vector.y = -1 * grasp_approach_vector.y(); + new_grasp.post_grasp_retreat.direction.vector.z = -1 * grasp_approach_vector.z(); + + // set grasp pose + geometry_msgs::PoseStamped grasp_pose_msg; + grasp_pose_msg.header.stamp = ros::Time::now(); + grasp_pose_msg.header.frame_id = grasp_data->base_link_; + + // name the grasp + static std::size_t grasp_id = 0; + new_grasp.id = "Grasp" + boost::lexical_cast<std::string>(grasp_id); + grasp_id++; + + tf::poseEigenToMsg(grasp_pose_eef_mount, grasp_pose_msg.pose); + new_grasp.grasp_pose = grasp_pose_msg; + + // set grasp postures e.g. hand closed + new_grasp.grasp_posture = grasp_data->grasp_posture_; + + // set minimum opening of fingers for pre grasp approach + double min_finger_open_on_approach = object_width + 2 * grasp_data->grasp_padding_on_approach_; + double percent_open; + + // Create grasp with widest fingers possible ---------------------------------------------- + percent_open = 1.0; + if (!grasp_data->setGraspWidth(percent_open, min_finger_open_on_approach, new_grasp.pre_grasp_posture)) + { + debugFailedOpenGripper(percent_open, min_finger_open_on_approach, object_width, + grasp_data->grasp_padding_on_approach_); + return false; + } + + new_grasp.grasp_quality = scoreFingerGrasp(grasp_pose_tcp, grasp_data, object_pose, percent_open); + + // Show visualization for widest grasp + + grasp_candidates.push_back(std::make_shared<GraspCandidate>(new_grasp, grasp_data, object_pose)); + + // Create grasp with middle width fingers ------------------------------------------------- + percent_open = 0.5; + if (!grasp_data->setGraspWidth(percent_open, min_finger_open_on_approach, new_grasp.pre_grasp_posture)) + { + debugFailedOpenGripper(percent_open, min_finger_open_on_approach, object_width, + grasp_data->grasp_padding_on_approach_); + return false; + } + new_grasp.grasp_quality = scoreFingerGrasp(grasp_pose_tcp, grasp_data, object_pose, percent_open); + grasp_candidates.push_back(std::make_shared<GraspCandidate>(new_grasp, grasp_data, object_pose)); + + // Create grasp with fingers at minimum width --------------------------------------------- + percent_open = 0.0; + if (!grasp_data->setGraspWidth(percent_open, min_finger_open_on_approach, new_grasp.pre_grasp_posture)) + { + debugFailedOpenGripper(percent_open, min_finger_open_on_approach, object_width, + grasp_data->grasp_padding_on_approach_); + return false; + } + new_grasp.grasp_quality = scoreFingerGrasp(grasp_pose_tcp, grasp_data, object_pose, percent_open); + grasp_candidates.push_back(std::make_shared<GraspCandidate>(new_grasp, grasp_data, object_pose)); + + return true; +} + +bool TwoFingerGraspGenerator::generateCuboidAxisGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, + double height, grasp_axis_t axis, + const TwoFingerGraspDataPtr& grasp_data, + const TwoFingerGraspCandidateConfig& grasp_candidate_config, + std::vector<GraspCandidatePtr>& grasp_candidates) +{ + double finger_depth = grasp_data->grasp_max_depth_ - grasp_data->grasp_min_depth_; + double length_along_a, length_along_b, length_along_c; + double delta_a, delta_b, delta_f; + double alpha_x, alpha_y, alpha_z; + Eigen::Vector3d object_size(depth, width, height); + + double object_width; + EigenSTL::vector_Isometry3d grasp_poses_tcp; + + Eigen::Isometry3d grasp_pose_tcp = cuboid_pose; + Eigen::Vector3d a_dir, b_dir, c_dir; + + if (axis == X_AXIS) + { + length_along_a = width; + length_along_b = height; + length_along_c = depth; + a_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitY(); + b_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ(); + c_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitX(); + alpha_x = -M_PI / 2.0; + alpha_y = 0; + alpha_z = -M_PI / 2.0; + object_width = depth; + } + else if (axis == Y_AXIS) + { + length_along_a = depth; + length_along_b = height; + length_along_c = width; + a_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitX(); + b_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ(); + c_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitY(); + alpha_x = 0; + alpha_y = M_PI / 2.0; + alpha_z = M_PI; + object_width = width; + } + else // Z_AXIS + { + length_along_a = depth; + length_along_b = width; + length_along_c = height; + a_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitX(); + b_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitY(); + c_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ(); + alpha_x = M_PI / 2.0; + alpha_y = M_PI / 2.0; + alpha_z = 0; + object_width = height; + } + + double rotation_angles[3]; + rotation_angles[0] = alpha_x; + rotation_angles[1] = alpha_y; + rotation_angles[2] = alpha_z; + + a_dir = a_dir.normalized(); + b_dir = b_dir.normalized(); + c_dir = c_dir.normalized(); + + // Add grasps at corners, grasps are centroid aligned + double offset = 0.001; // back the palm off of the object slightly + Eigen::Vector3d corner_translation_a; + Eigen::Vector3d corner_translation_b; + double angle_res = grasp_data->angle_resolution_ * M_PI / 180.0; + std::size_t num_radial_grasps = ceil((M_PI / 2.0) / angle_res); + Eigen::Vector3d translation; + + if (grasp_candidate_config.enable_corner_grasps_) + { + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding corner grasps..."); + corner_translation_a = 0.5 * (length_along_a + offset) * a_dir; + corner_translation_b = 0.5 * (length_along_b + offset) * b_dir; + + if (num_radial_grasps <= 0) + num_radial_grasps = 1; + + // move to corner 0.5 * ( -a, -b) + translation = -corner_translation_a - corner_translation_b; + addCornerGraspsHelper(cuboid_pose, rotation_angles, translation, 0.0, num_radial_grasps, grasp_poses_tcp); + + // move to corner 0.5 * ( -a, +b) + translation = -corner_translation_a + corner_translation_b; + addCornerGraspsHelper(cuboid_pose, rotation_angles, translation, -M_PI / 2.0, num_radial_grasps, grasp_poses_tcp); + + // move to corner 0.5 * ( +a, +b) + translation = corner_translation_a + corner_translation_b; + addCornerGraspsHelper(cuboid_pose, rotation_angles, translation, M_PI, num_radial_grasps, grasp_poses_tcp); + + // move to corner 0.5 * ( +a, -b) + translation = corner_translation_a - corner_translation_b; + addCornerGraspsHelper(cuboid_pose, rotation_angles, translation, M_PI / 2.0, num_radial_grasps, grasp_poses_tcp); + } + std::size_t num_corner_grasps = grasp_poses_tcp.size(); + + // Create grasps along faces of cuboid, grasps are axis aligned + std::size_t num_grasps_along_a; + std::size_t num_grasps_along_b; + double rotation; + Eigen::Vector3d a_translation; + Eigen::Vector3d b_translation; + Eigen::Vector3d delta; + + // get exact deltas for sides from desired delta + num_grasps_along_a = floor((length_along_a - grasp_data->gripper_finger_width_) / grasp_data->grasp_resolution_) + 1; + num_grasps_along_b = floor((length_along_b - grasp_data->gripper_finger_width_) / grasp_data->grasp_resolution_) + 1; + + // if the gripper fingers are wider than the object we're trying to grasp, try with gripper aligned with + // top/center/bottom of object + // note that current implementation limits objects that are the same size as the gripper_finger_width to 1 grasp + if (num_grasps_along_a <= 0) + { + delta_a = length_along_a - grasp_data->gripper_finger_width_ / 2.0; + num_grasps_along_a = 3; + } + if (num_grasps_along_b <= 0) + { + delta_b = length_along_b - grasp_data->gripper_finger_width_ / 2.0; + num_grasps_along_b = 3; + } + + if (num_grasps_along_a == 1) + delta_a = 0; + else + delta_a = (length_along_a - grasp_data->gripper_finger_width_) / static_cast<double>(num_grasps_along_a - 1); + + if (num_grasps_along_b == 1) + delta_b = 0; + else + delta_b = (length_along_b - grasp_data->gripper_finger_width_) / static_cast<double>(num_grasps_along_b - 1); + + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "delta_a : delta_b = " << delta_a << " : " << delta_b); + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "num_grasps_along_a : num_grasps_along_b = " + << num_grasps_along_a << " : " << num_grasps_along_b); + + // TODO(mlautman): There is a bug with face grasps allowing the grasp generator to generate grasps where the gripper + // fingers + // are in collision with the object being grasped + if (grasp_candidate_config.enable_face_grasps_) + { + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding face grasps..."); + + a_translation = -(0.5 * (length_along_a + offset) * a_dir) - + 0.5 * (length_along_b - grasp_data->gripper_finger_width_) * b_dir - delta_b * b_dir; + b_translation = -0.5 * (length_along_a - grasp_data->gripper_finger_width_) * a_dir - delta_a * a_dir - + (0.5 * (length_along_b + offset) * b_dir); + + // grasps along -a_dir face + delta = delta_b * b_dir; + rotation = 0.0; + addFaceGraspsHelper(cuboid_pose, rotation_angles, a_translation, delta, rotation, num_grasps_along_b, + grasp_poses_tcp); + + // grasps along +b_dir face + rotation = -M_PI / 2.0; + delta = -delta_a * a_dir; + addFaceGraspsHelper(cuboid_pose, rotation_angles, -b_translation, delta, rotation, num_grasps_along_b, + grasp_poses_tcp); + + // grasps along +a_dir face + rotation = M_PI; + delta = -delta_b * b_dir; + addFaceGraspsHelper(cuboid_pose, rotation_angles, -a_translation, delta, rotation, num_grasps_along_b, + grasp_poses_tcp); + + // grasps along -b_dir face + rotation = M_PI / 2.0; + delta = delta_a * a_dir; + addFaceGraspsHelper(cuboid_pose, rotation_angles, b_translation, delta, rotation, num_grasps_along_b, + grasp_poses_tcp); + } + + // add grasps at variable angles + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding variable angle grasps..."); + Eigen::Isometry3d base_pose; + std::size_t num_grasps = grasp_poses_tcp.size(); + if (grasp_candidate_config.enable_variable_angle_grasps_) + { + // corner grasps at zero depth don't need variable angles + for (std::size_t i = num_corner_grasps; i < num_grasps; ++i) + { + base_pose = grasp_poses_tcp[i]; + + grasp_pose_tcp = base_pose * Eigen::AngleAxisd(angle_res, Eigen::Vector3d::UnitY()); + std::size_t max_iterations = M_PI / angle_res + 1; + std::size_t iterations = 0; + while (graspIntersectionHelper(cuboid_pose, depth, width, height, grasp_pose_tcp, grasp_data)) + { + grasp_poses_tcp.push_back(grasp_pose_tcp); + grasp_pose_tcp *= Eigen::AngleAxisd(angle_res, Eigen::Vector3d::UnitY()); + iterations++; + if (iterations > max_iterations) + { + ROS_WARN_STREAM_NAMED("cuboid_axis_grasps", "exceeded max iterations while creating variable angle grasps"); + break; + } + } + + iterations = 0; + grasp_pose_tcp = base_pose * Eigen::AngleAxisd(-angle_res, Eigen::Vector3d::UnitY()); + while (graspIntersectionHelper(cuboid_pose, depth, width, height, grasp_pose_tcp, grasp_data)) + { + grasp_poses_tcp.push_back(grasp_pose_tcp); + // visual_tools_->publishZArrow(grasp_pose_tcp, rviz_visual_tools::CYAN, rviz_visual_tools::XSMALL, 0.02); + grasp_pose_tcp *= Eigen::AngleAxisd(-angle_res, Eigen::Vector3d::UnitY()); + // ros::Duration(0.2).sleep(); + iterations++; + if (iterations > max_iterations) + { + ROS_WARN_STREAM_NAMED("cuboid_axis_grasps", "exceeded max iterations while creating variable angle grasps"); + break; + } + } + } + } + + if (grasp_candidate_config.enable_edge_grasps_) + { + // Add grasps along edges + // move grasp pose to edge of cuboid + double a_sign = 1.0; + double b_sign = 1.0; + double a_rot_sign = 1.0; + double b_rot_sign = 1.0; + + if (axis == Y_AXIS) + { + a_sign = -1.0; + b_rot_sign = -1.0; + } + + if (axis == Z_AXIS) + { + a_sign = -1.0; + b_sign = -1.0; + a_rot_sign = -1.0; + b_rot_sign = -1.0; + } + + a_translation = -0.5 * (length_along_a + offset) * a_dir - + 0.5 * (length_along_b - grasp_data->gripper_finger_width_) * b_dir - delta_b * b_dir - + 0.5 * (length_along_c + offset) * c_dir * a_sign; + b_translation = -0.5 * (length_along_a - grasp_data->gripper_finger_width_) * a_dir - delta_a * a_dir - + (0.5 * (length_along_b + offset) * b_dir) - 0.5 * (length_along_c + offset) * c_dir * b_sign; + + // grasps along -a_dir face + delta = delta_b * b_dir; + rotation = 0.0; + addEdgeGraspsHelper(cuboid_pose, rotation_angles, a_translation, delta, rotation, num_grasps_along_b, + grasp_poses_tcp, -M_PI / 4.0 * a_rot_sign); + + // grasps along +b_dir face + rotation = -M_PI / 2.0; + delta = -delta_a * a_dir; + addEdgeGraspsHelper(cuboid_pose, rotation_angles, -b_translation, delta, rotation, num_grasps_along_b, + grasp_poses_tcp, M_PI / 4.0 * b_rot_sign); + + // grasps along +a_dir face + rotation = M_PI; + delta = -delta_b * b_dir; + addEdgeGraspsHelper(cuboid_pose, rotation_angles, -a_translation, delta, rotation, num_grasps_along_b, + grasp_poses_tcp, M_PI / 4.0 * a_rot_sign); + + // grasps along -b_dir face + rotation = M_PI / 2.0; + delta = delta_a * a_dir; + addEdgeGraspsHelper(cuboid_pose, rotation_angles, b_translation, delta, rotation, num_grasps_along_b, + grasp_poses_tcp, -M_PI / 4.0 * b_rot_sign); + } + // Add grasps at variable depths + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding depth grasps..."); + std::size_t num_depth_grasps = ceil(finger_depth / grasp_data->grasp_depth_resolution_); + if (num_depth_grasps <= 0) + num_depth_grasps = 1; + delta_f = finger_depth / static_cast<double>(num_depth_grasps); + + num_grasps = grasp_poses_tcp.size(); + Eigen::Vector3d grasp_dir; + Eigen::Isometry3d depth_pose; + + for (std::size_t i = 0; i < num_grasps; ++i) + { + grasp_dir = grasp_poses_tcp[i].rotation() * Eigen::Vector3d::UnitZ(); + depth_pose = grasp_poses_tcp[i]; + for (std::size_t j = 0; j < num_depth_grasps; ++j) + { + depth_pose.translation() += delta_f * grasp_dir; + grasp_poses_tcp.push_back(depth_pose); + } + } + + // add grasps in both directions + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding bi-directional grasps..."); + num_grasps = grasp_poses_tcp.size(); + for (std::size_t i = 0; i < num_grasps; ++i) + { + grasp_pose_tcp = grasp_poses_tcp[i]; + grasp_pose_tcp *= Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()); + grasp_poses_tcp.push_back(grasp_pose_tcp); + } + + // compute min/max distances to object + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "computing min/max grasp distance..."); + num_grasps = grasp_poses_tcp.size(); + min_grasp_distance_ = std::numeric_limits<double>::max(); + max_grasp_distance_ = std::numeric_limits<double>::min(); + min_translations_ = Eigen::Vector3d(std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), + std::numeric_limits<double>::max()); + max_translations_ = Eigen::Vector3d(std::numeric_limits<double>::min(), std::numeric_limits<double>::min(), + std::numeric_limits<double>::min()); + double grasp_distance; + + for (std::size_t i = 0; i < num_grasps; ++i) + { + grasp_pose_tcp = grasp_poses_tcp[i]; + grasp_distance = (grasp_pose_tcp.translation() - cuboid_pose.translation()).norm(); + if (grasp_distance > max_grasp_distance_) + max_grasp_distance_ = grasp_distance; + + if (grasp_distance < min_grasp_distance_) + min_grasp_distance_ = grasp_distance; + + for (std::size_t j = 0; j < 3; ++j) + { + if (grasp_pose_tcp.translation()[j] < min_translations_[j]) + min_translations_[j] = grasp_pose_tcp.translation()[j]; + + if (grasp_pose_tcp.translation()[j] > max_translations_[j]) + max_translations_[j] = grasp_pose_tcp.translation()[j]; + } + } + + ROS_DEBUG_STREAM_NAMED("grasp_generator.add", + "min/max distance = " << min_grasp_distance_ << ", " << max_grasp_distance_); + + // add all poses as possible grasps + std::size_t num_grasps_added = 0; + + for (std::size_t i = 0; i < grasp_poses_tcp.size(); ++i) + { + Eigen::Isometry3d grasp_pose_eef_mount = grasp_poses_tcp[i] * grasp_data->tcp_to_eef_mount_; + if (!addGrasp(grasp_pose_eef_mount, grasp_data, cuboid_pose, object_size, object_width, grasp_candidates)) + { + ROS_DEBUG_STREAM_NAMED("grasp_generator.add", "Unable to add grasp - function returned false"); + } + else + num_grasps_added++; + } + ROS_INFO_STREAM_NAMED("grasp_generator.add", "\033[1;36madded " << num_grasps_added << " of " + << grasp_poses_tcp.size() + << " grasp poses created\033[0m"); + return true; +} + +std::size_t TwoFingerGraspGenerator::addCornerGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3], + const Eigen::Vector3d& translation, double corner_rotation, + std::size_t num_radial_grasps, + EigenSTL::vector_Isometry3d& grasp_poses_tcp) +{ + std::size_t num_grasps_added = 0; + double delta_angle = (M_PI / 2.0) / static_cast<double>(num_radial_grasps + 1); + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "delta_angle = " << delta_angle); + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_radial_grasps = " << num_radial_grasps); + + // rotate & translate pose to be aligned with edge of cuboid + Eigen::Isometry3d grasp_pose_tcp = pose; + grasp_pose_tcp *= Eigen::AngleAxisd(rotation_angles[0], Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(rotation_angles[1], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rotation_angles[2], Eigen::Vector3d::UnitZ()); + grasp_pose_tcp *= Eigen::AngleAxisd(corner_rotation, Eigen::Vector3d::UnitY()); + grasp_pose_tcp.translation() += translation; + + for (std::size_t i = 0; i < num_radial_grasps; ++i) + { + // Eigen::Vector3d grasp_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ(); + // Eigen::Isometry3d radial_pose = grasp_pose_tcp; + grasp_pose_tcp *= Eigen::AngleAxisd(delta_angle, Eigen::Vector3d::UnitY()); + grasp_poses_tcp.push_back(grasp_pose_tcp); + num_grasps_added++; + } + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps_added : grasp_poses_tcp.size() = " + << num_grasps_added << " : " << grasp_poses_tcp.size()); + return num_grasps_added; +} + +std::size_t TwoFingerGraspGenerator::addFaceGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3], + const Eigen::Vector3d& translation, + const Eigen::Vector3d& delta, double alignment_rotation, + std::size_t num_grasps, + EigenSTL::vector_Isometry3d& grasp_poses_tcp) +{ + std::size_t num_grasps_added = 0; + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "delta = \n" << delta); + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps = " << num_grasps); + + Eigen::Isometry3d grasp_pose_tcp = pose; + grasp_pose_tcp *= Eigen::AngleAxisd(rotation_angles[0], Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(rotation_angles[1], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rotation_angles[2], Eigen::Vector3d::UnitZ()); + grasp_pose_tcp *= Eigen::AngleAxisd(alignment_rotation, Eigen::Vector3d::UnitY()); + grasp_pose_tcp.translation() += translation; + + for (std::size_t i = 0; i < num_grasps; ++i) + { + grasp_pose_tcp.translation() += delta; + grasp_poses_tcp.push_back(grasp_pose_tcp); + num_grasps_added++; + } + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps_added : grasp_poses_tcp.size() = " + << num_grasps_added << " : " << grasp_poses_tcp.size()); + return true; +} + +std::size_t TwoFingerGraspGenerator::addEdgeGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3], + const Eigen::Vector3d& translation, + const Eigen::Vector3d& delta, double alignment_rotation, + std::size_t num_grasps, + EigenSTL::vector_Isometry3d& grasp_poses_tcp, + double corner_rotation) +{ + std::size_t num_grasps_added = 0; + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "delta = \n" << delta); + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps = " << num_grasps); + + Eigen::Isometry3d grasp_pose_tcp = pose; + grasp_pose_tcp *= Eigen::AngleAxisd(rotation_angles[0], Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(rotation_angles[1], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rotation_angles[2], Eigen::Vector3d::UnitZ()); + grasp_pose_tcp *= Eigen::AngleAxisd(alignment_rotation, Eigen::Vector3d::UnitY()); + + // rotate towards cuboid + grasp_pose_tcp *= Eigen::AngleAxisd(corner_rotation, Eigen::Vector3d::UnitX()); + grasp_pose_tcp.translation() += translation; + + for (std::size_t i = 0; i < num_grasps; ++i) + { + grasp_pose_tcp.translation() += delta; + grasp_poses_tcp.push_back(grasp_pose_tcp); + num_grasps_added++; + } + ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps_added : grasp_poses_tcp.size() = " + << num_grasps_added << " : " << grasp_poses_tcp.size()); + return true; +} + +bool TwoFingerGraspGenerator::graspIntersectionHelper(const Eigen::Isometry3d& cuboid_pose, double depth, double width, + double height, const Eigen::Isometry3d& grasp_pose_tcp, + const TwoFingerGraspDataPtr& grasp_data) +{ + // TODO(davetcoleman): add parameter to enable vizualization commented lines after further testing + + // get line segment from grasp point to fingertip + Eigen::Vector3d point_a = grasp_pose_tcp.translation(); + Eigen::Vector3d point_b = + point_a + grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ() * grasp_data->grasp_max_depth_; + + // translate points into cuboid coordinate system + point_a = cuboid_pose.inverse() * point_a; // T_cuboid-world * p_world = p_cuboid + point_b = cuboid_pose.inverse() * point_b; + + double t, u, v; + Eigen::Vector3d intersection; + // check if line segment intersects XY faces of cuboid (z = +/- height/2) + t = (height / 2.0 - point_a[2]) / (point_b[2] - point_a[2]); // parameterization of line segment in 3d + if (intersectionHelper(t, point_a[0], point_a[1], point_b[0], point_b[1], depth, width, u, v)) + { + return true; + } + + t = (-height / 2.0 - point_a[2]) / (point_b[2] - point_a[2]); + if (intersectionHelper(t, point_a[0], point_a[1], point_b[0], point_b[1], depth, width, u, v)) + { + return true; + } + + // check if line segment intersects XZ faces of cuboid (y = +/- width/2) + t = (width / 2.0 - point_a[1]) / (point_b[1] - point_a[1]); + if (intersectionHelper(t, point_a[0], point_a[2], point_b[0], point_b[2], depth, height, u, v)) + { + return true; + } + + t = (-width / 2.0 - point_a[1]) / (point_b[1] - point_a[1]); + if (intersectionHelper(t, point_a[0], point_a[2], point_b[0], point_b[2], depth, height, u, v)) + { + return true; + } + + // check if line segment intersects YZ faces of cuboid (x = +/- depth/2) + t = (depth / 2.0 - point_a[0]) / (point_b[0] - point_a[0]); + if (intersectionHelper(t, point_a[1], point_a[2], point_b[1], point_b[2], width, height, u, v)) + { + return true; + } + + t = (-depth / 2.0 - point_a[0]) / (point_b[0] - point_a[0]); + if (intersectionHelper(t, point_a[1], point_a[2], point_b[1], point_b[2], width, height, u, v)) + { + return true; + } + + // no intersection found + return false; +} + +bool TwoFingerGraspGenerator::intersectionHelper(double t, double u1, double v1, double u2, double v2, double a, + double b, double& u, double& v) +{ + // plane must cross through our line segment + if (t >= 0 && t <= 1) + { + u = u1 + t * (u2 - u1); + v = v1 + t * (v2 - v1); + + if (u >= -a / 2 && u <= a / 2 && v >= -b / 2 && v <= b / 2) + return true; + } + + return false; +} + +double TwoFingerGraspGenerator::scoreFingerGrasp(const Eigen::Isometry3d& grasp_pose_tcp, + const TwoFingerGraspDataPtr& grasp_data, + const Eigen::Isometry3d& object_pose, double percent_open) +{ + static const std::string logger_name = "grasp_generator.scoreGrasp"; + ROS_DEBUG_STREAM_NAMED(logger_name, "starting to score grasp..."); + + // get portion of score based on the gripper's opening width on approach + double width_score = TwoFingerGraspScorer::scoreGraspWidth(grasp_data, percent_open); + + // get portion of score based on the pinchers being down + Eigen::Vector3d orientation_scores = + TwoFingerGraspScorer::scoreRotationsFromDesired(grasp_pose_tcp, ideal_grasp_pose_); + + // get portion of score based on the distance of the grasp pose to the object pose + + // NOTE: when this function is called we've lost the references to the acutal size of the object. + // max_distance should be the length of the fingers minus some minimum amount that the fingers need to grip an object + // since we don't know the distance from the centoid of the object to the edge of the object, this is set as an + // arbitrary number given our target object set + double distance_score = TwoFingerGraspScorer::scoreDistanceToPalm(grasp_pose_tcp, grasp_data, object_pose, + min_grasp_distance_, max_grasp_distance_); + + // should really change this to be like orienation_scores so we can score any translation + Eigen::Vector3d translation_scores = + TwoFingerGraspScorer::scoreGraspTranslation(grasp_pose_tcp, min_translations_, max_translations_); + + // want minimum translation + translation_scores *= -1.0; + translation_scores += Eigen::Vector3d(1.0, 1.0, 1.0); + + double total_score = 0; + auto two_finger_grasp_score_weights = std::dynamic_pointer_cast<TwoFingerGraspScoreWeights>(grasp_score_weights_); + if (two_finger_grasp_score_weights) + { + total_score = two_finger_grasp_score_weights->computeScore(orientation_scores, translation_scores, distance_score, + width_score, getVerbose()); + } + else + { + ROS_WARN_NAMED(logger_name, "Failed to cast grasp_score_weights_ as TwoFingerGraspScoreWeights. continuing without " + "finger specific scores"); + total_score = grasp_score_weights_->computeScore(orientation_scores, translation_scores, getVerbose()); + } + + return total_score; +} + +bool TwoFingerGraspGenerator::generateFingerGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, + double height, const TwoFingerGraspDataPtr& grasp_data, + std::vector<GraspCandidatePtr>& grasp_candidates, + const TwoFingerGraspCandidateConfig& grasp_candidate_config) +{ + // Generate grasps over axes that aren't too wide to grip + // Most default type of grasp is X axis + TwoFingerGraspCandidateConfig grasp_candidate_config_copy(grasp_candidate_config); + if (grasp_candidate_config_copy.generate_x_axis_grasps_) + { + ROS_DEBUG_STREAM_NAMED("grasp_generator", "Generating grasps around x-axis of cuboid"); + if (depth > grasp_data->max_grasp_width_) // depth = size along x-axis + { + grasp_candidate_config_copy.disableAllGraspTypes(); + grasp_candidate_config_copy.enable_edge_grasps_ = grasp_candidate_config.enable_edge_grasps_; + grasp_candidate_config_copy.enable_corner_grasps_ = grasp_candidate_config.enable_corner_grasps_; + } + generateCuboidAxisGrasps(cuboid_pose, depth, width, height, X_AXIS, grasp_data, grasp_candidate_config_copy, + grasp_candidates); + } + + grasp_candidate_config_copy = grasp_candidate_config; + if (grasp_candidate_config_copy.generate_y_axis_grasps_) + { + ROS_DEBUG_STREAM_NAMED("grasp_generator", "Generating grasps around y-axis of cuboid"); + if (width > grasp_data->max_grasp_width_) // width = size along y-axis + { + grasp_candidate_config_copy.disableAllGraspTypes(); + grasp_candidate_config_copy.enable_edge_grasps_ = grasp_candidate_config.enable_edge_grasps_; + grasp_candidate_config_copy.enable_corner_grasps_ = grasp_candidate_config.enable_corner_grasps_; + } + generateCuboidAxisGrasps(cuboid_pose, depth, width, height, Y_AXIS, grasp_data, grasp_candidate_config_copy, + grasp_candidates); + } + + grasp_candidate_config_copy = grasp_candidate_config; + if (grasp_candidate_config_copy.generate_z_axis_grasps_) + { + ROS_DEBUG_STREAM_NAMED("grasp_generator", "Generating grasps around z-axis of cuboid"); + if (height > grasp_data->max_grasp_width_) // height = size along z-axis + { + grasp_candidate_config_copy.disableAllGraspTypes(); + grasp_candidate_config_copy.enable_edge_grasps_ = grasp_candidate_config.enable_edge_grasps_; + grasp_candidate_config_copy.enable_corner_grasps_ = grasp_candidate_config.enable_corner_grasps_; + } + generateCuboidAxisGrasps(cuboid_pose, depth, width, height, Z_AXIS, grasp_data, grasp_candidate_config_copy, + grasp_candidates); + } + + if (!grasp_candidates.size()) + ROS_WARN_STREAM_NAMED("grasp_generator", "Generated 0 grasps"); + else + ROS_INFO_STREAM_NAMED("grasp_generator", "Generated " << grasp_candidates.size() << " grasps"); + + // Visualize animated grasps that have been generated + if (show_prefiltered_grasps_) + { + ROS_DEBUG_STREAM_NAMED("grasp_generator", "Animating all generated (candidate) grasps before filtering"); + visualizeAnimatedGrasps(grasp_candidates, grasp_data->ee_jmg_, show_prefiltered_grasps_speed_); + } + + return true; +} + +} // namespace moveit_grasps diff --git a/src/mtc/src/two_finger_grasp_scorer.cpp b/src/mtc/src/two_finger_grasp_scorer.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c542176fc239759108533e31b4a939b6e3a5269e --- /dev/null +++ b/src/mtc/src/two_finger_grasp_scorer.cpp @@ -0,0 +1,102 @@ +/********************************************************************* + * Software License Agreement ("Modified BSD License") + * + * Copyright (c) 2014, University of Colorado, Boulder + * 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 Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/** + * Authors : Andy McEvoy ( mcevoy.andy@gmail.com ) + * Desc : Functions for scoring grasps. See *.h file for documentation + */ + +#include "../include/two_finger_grasp_scorer.h" + +namespace moveit_grasps +{ +double TwoFingerGraspScoreWeights::computeScore(const Eigen::Vector3d& orientation_scores, + const Eigen::Vector3d& translation_scores, double depth_score, + double width_score, bool verbose) const +{ + double total_score = GraspScoreWeights::computeScore(orientation_scores, translation_scores, false) * + GraspScoreWeights::getWeightTotal(); + total_score += depth_score * depth_score_weight_ + width_score * width_score_weight_; + + total_score /= getWeightTotal(); + + if (verbose) + { + static const std::string logger_name = "grasp_scorer.compute_score"; + // clang-format off + ROS_DEBUG_STREAM_NAMED(logger_name, "Two Finger Grasp score: "); + ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.x = " << orientation_scores[0] << "\tweight = "<< orientation_x_score_weight_); + ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.y = " << orientation_scores[1] << "\tweight = "<< orientation_y_score_weight_); + ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.z = " << orientation_scores[2] << "\tweight = "<< orientation_z_score_weight_); + ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.x = " << translation_scores[0] << "\tweight = "<< translation_x_score_weight_); + ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.y = " << translation_scores[1] << "\tweight = "<< translation_y_score_weight_); + ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.z = " << translation_scores[2] << "\tweight = "<< translation_z_score_weight_); + ROS_DEBUG_STREAM_NAMED(logger_name, "\tdepth_score = " << depth_score << "\tweight = "<< depth_score_weight_); + ROS_DEBUG_STREAM_NAMED(logger_name, "\twidth_score = " << width_score << "\tweight = "<< width_score_weight_); + // Total + ROS_DEBUG_STREAM_NAMED(logger_name, "\ttotal_score = " << total_score); + // clang-format on + } + return total_score; +} + +double TwoFingerGraspScoreWeights::getWeightTotal() const +{ + return GraspScoreWeights::getWeightTotal() + depth_score_weight_ + width_score_weight_; +} + +double TwoFingerGraspScorer::scoreGraspWidth(const TwoFingerGraspDataPtr& grasp_data, double percent_open) +{ + ROS_DEBUG_STREAM_NAMED("grasp_scorer.graspWidth", "raw score = " << percent_open); + return pow(percent_open, 2); +} + +double TwoFingerGraspScorer::scoreDistanceToPalm(const Eigen::Isometry3d& grasp_pose_tcp, + const TwoFingerGraspDataPtr& grasp_data, + const Eigen::Isometry3d& object_pose, double min_grasp_distance, + double max_grasp_distance) +{ + double distance = (grasp_pose_tcp.translation() - object_pose.translation()).norm(); + ROS_DEBUG_STREAM_NAMED("grasp_scorer.distance", + "distance = " << distance << ", " << min_grasp_distance << ":" << max_grasp_distance); + + double score = 1.0 - (distance - min_grasp_distance) / (max_grasp_distance - min_grasp_distance); + + ROS_DEBUG_STREAM_NAMED("grasp_scorer.distance", "raw score = " << score); + if (score < 0) + ROS_WARN_STREAM_NAMED("grasp_scorer.distance", "score < 0!"); + return pow(score, 4); +} + +} // namespace moveit_grasps diff --git a/src/panda_moveit_config/CMakeLists.txt b/src/panda_moveit_config/CMakeLists.txt index b4e880c67048e6eadc6a48a994880593a9903906..43e77c5c2a0f8726547938192f29f5f10379eef9 100644 --- a/src/panda_moveit_config/CMakeLists.txt +++ b/src/panda_moveit_config/CMakeLists.txt @@ -3,6 +3,7 @@ project(panda_moveit_config) find_package(catkin REQUIRED) + catkin_package() install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}