├── resources ├── demo.png └── filter.png ├── action └── GenerateGrasps.action ├── msg └── GraspGeneratorOptions.msg ├── launch ├── grasp_generator_server.launch ├── grasp_test_rviz.launch ├── grasp_filter_test.launch ├── grasp_test.launch └── simple_grasps.rviz ├── config ├── clam_grasp_data.yaml ├── reem_grasp_data.yaml ├── baxter_grasp_data.yaml ├── nao_grasp_data.yaml ├── pepper_grasp_data.yaml └── romeo_grasp_data.yaml ├── package.xml ├── .travis.yml ├── CMakeLists.txt ├── CHANGELOG.rst ├── scripts └── server_test.py ├── include └── moveit_simple_grasps │ ├── custom_environment2.h │ ├── grasp_data.h │ ├── grasp_filter.h │ ├── simple_grasps.h │ └── moveit_blocks.h ├── src ├── simple_grasps_server.cpp ├── simple_grasps_test.cpp ├── grasp_filter_test.cpp ├── grasp_filter.cpp ├── grasp_data.cpp └── simple_grasps.cpp └── README.md /resources/demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/moveit_simple_grasps/HEAD/resources/demo.png -------------------------------------------------------------------------------- /resources/filter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/moveit_simple_grasps/HEAD/resources/filter.png -------------------------------------------------------------------------------- /action/GenerateGrasps.action: -------------------------------------------------------------------------------- 1 | #goal 2 | geometry_msgs/Pose pose 3 | float64 width 4 | moveit_simple_grasps/GraspGeneratorOptions[] options 5 | --- 6 | #result 7 | moveit_msgs/Grasp[] grasps 8 | --- 9 | #feedback 10 | moveit_msgs/Grasp[] grasps 11 | -------------------------------------------------------------------------------- /msg/GraspGeneratorOptions.msg: -------------------------------------------------------------------------------- 1 | uint8 grasp_axis 2 | uint8 GRASP_AXIS_X = 0 3 | uint8 GRASP_AXIS_Y = 1 4 | uint8 GRASP_AXIS_Z = 2 5 | 6 | uint8 grasp_direction 7 | uint8 GRASP_DIRECTION_UP = 0 8 | uint8 GRASP_DIRECTION_DOWN = 1 9 | 10 | uint8 grasp_rotation 11 | uint8 GRASP_ROTATION_HALF = 0 12 | uint8 GRASP_ROTATION_FULL = 1 13 | -------------------------------------------------------------------------------- /launch/grasp_generator_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /launch/grasp_test_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /config/clam_grasp_data.yaml: -------------------------------------------------------------------------------- 1 | base_link: 'base_link' 2 | 3 | # ===================================================== 4 | gripper_group: # ee group name 5 | end_effector_name: 'gripper_group' # ee group name 6 | 7 | #actuated joints in end effector 8 | joints : ['gripper_finger_joint'] 9 | 10 | #open position 11 | pregrasp_posture : [-0.925023722222] 12 | pregrasp_time_from_start : 4.0 13 | 14 | #close position 15 | grasp_posture : [1.0] 16 | grasp_time_from_start : 4.0 17 | 18 | #desired pose from end effector to grasp - [x,y,z] 19 | grasp_pose_to_eef : [-0.14,0,0] 20 | 21 | #desired pose from end effector to grasp - [r, p, y] 22 | grasp_pose_to_eef_rotation : [0, 0, 0] # 1.5707 = PI/2 23 | 24 | end_effector_parent_link : 'gripper_roll_link' -------------------------------------------------------------------------------- /launch/grasp_filter_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /config/reem_grasp_data.yaml: -------------------------------------------------------------------------------- 1 | base_link: 'base_link' 2 | 3 | # ===================================================== 4 | right_hand: 5 | end_effector_name: 'right_hand' 6 | 7 | #default grasp params 8 | joints : ['hand_right_index_joint', 'hand_right_middle_joint', 'hand_right_thumb_joint'] 9 | pregrasp_posture : [0.0, 0.0, 2.0] 10 | pregrasp_time_from_start : 4.0 11 | grasp_posture : [4.5, 4.5, 2.0] 12 | grasp_time_from_start : 4.0 13 | 14 | #desired pose from end effector to grasp 15 | grasp_pose_to_eef : [0.054, 0.0, 0.018] 16 | 17 | #desired pose from end effector to grasp - [r, p, y] 18 | grasp_pose_to_eef_rotation : [0, 1.5708, 0] # 1.5707 = PI/2 19 | 20 | end_effector_parent_link: 'right_hand_grasping_frame' 21 | # ===================================================== 22 | left_hand: 23 | end_effector_name: 'left_hand' 24 | 25 | #default grasp params 26 | joints : ['hand_left_index_joint', 'hand_left_middle_joint', 'hand_left_thumb_joint'] 27 | pregrasp_posture : [0.0, 0.0, 2.0] 28 | pregrasp_time_from_start : 4.0 29 | grasp_posture : [4.5, 4.5, 2.0] 30 | grasp_time_from_start : 4.0 31 | 32 | #desired pose from end effector to grasp 33 | grasp_pose_to_eef : [0.054, 0.0, 0.018] 34 | 35 | #desired pose from end effector to grasp - [r, p, y] 36 | grasp_pose_to_eef_rotation : [0, 1.5708, 0] # 1.5707 = PI/2 37 | 38 | end_effector_parent_link: 'left_hand_grasping_frame' 39 | 40 | -------------------------------------------------------------------------------- /launch/grasp_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /config/baxter_grasp_data.yaml: -------------------------------------------------------------------------------- 1 | base_link: 'base' 2 | 3 | # ===================================================== 4 | left_hand: 5 | end_effector_name: 'left_hand' #ee group name 6 | 7 | #actuated joints in end effector 8 | joints : ['left_gripper_l_finger_joint'] 9 | 10 | #open position 11 | pregrasp_posture : [0.0095] 12 | pregrasp_time_from_start : 4.0 13 | 14 | #close position 15 | grasp_posture : [-0.0125] 16 | grasp_time_from_start : 4.0 17 | 18 | #desired pose from end effector to grasp - [x,y,z] 19 | grasp_pose_to_eef : [-0.15, 0, 0] 20 | 21 | #desired pose from end effector to grasp - [r, p, y] 22 | grasp_pose_to_eef_rotation : [0, 1.5708, 0] # 1.5707 = PI/2 23 | 24 | end_effector_parent_link: 'left_wrist' 25 | # ===================================================== 26 | right_hand: 27 | end_effector_name: 'right_hand' #ee group name 28 | 29 | #actuated joints in end effector 30 | joints : ['right_gripper_l_finger_joint'] 31 | 32 | #open position 33 | pregrasp_posture : [0.0095] 34 | pregrasp_time_from_start : 4.0 35 | 36 | #close position 37 | grasp_posture : [-0.0125] 38 | grasp_time_from_start : 4.0 39 | 40 | #desired pose from end effector to grasp - [x,y,z] 41 | grasp_pose_to_eef : [-0.15, 0, 0] 42 | 43 | #desired pose from end effector to grasp - [r, p, y] 44 | grasp_pose_to_eef_rotation : [0, 1.5708, 0] # 1.5707 = PI/2 45 | 46 | end_effector_parent_link: 'right_wrist' 47 | 48 | -------------------------------------------------------------------------------- /config/nao_grasp_data.yaml: -------------------------------------------------------------------------------- 1 | base_link: '/odom' 2 | 3 | # ===================================================== 4 | left_hand: 5 | end_effector_name: 'left_hand' #ee group name 6 | 7 | #actuated joints in end effector 8 | joints : ['LHand'] 9 | 10 | #open position 11 | pregrasp_posture : [0.0] 12 | pregrasp_time_from_start : 4.0 13 | 14 | #close position 15 | grasp_posture : [1.0] 16 | grasp_time_from_start : 4.0 17 | 18 | #desired pose from end effector to grasp - [x,y,z] 19 | #desired pose from end effector to grasp - [r, p, y] 20 | 21 | #side grasp -pi/4 22 | grasp_pose_to_eef : [-0.065, 0.015, -0.03] 23 | grasp_pose_to_eef_rotation : [-1.025, 0.0, 0.0] 24 | 25 | end_effector_parent_link: 'l_wrist' 26 | # ===================================================== 27 | right_hand: 28 | end_effector_name: 'right_hand' #ee group name 29 | 30 | #actuated joints in end effector 31 | joints : ['RHand'] 32 | 33 | #open position 34 | pregrasp_posture : [0.0] 35 | pregrasp_time_from_start : 4.0 36 | 37 | #close position 38 | grasp_posture : [1.0] 39 | grasp_time_from_start : 4.0 40 | 41 | #desired pose from end effector to grasp - [x,y,z] 42 | #desired pose from end effector to grasp - [r, p, y] 43 | 44 | #side grasp pi/4 45 | grasp_pose_to_eef : [-0.065, -0.015, -0.03] 46 | grasp_pose_to_eef_rotation : [1.025, 0.0, 0.0] 47 | 48 | end_effector_parent_link: 'r_wrist' 49 | 50 | -------------------------------------------------------------------------------- /config/pepper_grasp_data.yaml: -------------------------------------------------------------------------------- 1 | base_link: '/odom' 2 | 3 | # ===================================================== 4 | left_hand: 5 | end_effector_name: 'left_hand' #ee group name 6 | 7 | #actuated joints in end effector 8 | joints : ['LHand'] 9 | 10 | #open position 11 | pregrasp_posture : [0.0] #for sim 12 | pregrasp_time_from_start : 4.0 13 | 14 | #close position 15 | grasp_posture : [1.0] #for sim 16 | grasp_time_from_start : 4.0 17 | 18 | #desired pose from end effector to grasp - [x,y,z] 19 | #desired pose from end effector to grasp - [r, p, y] #1.5707 = PI/2 20 | #side grasp -65deg 21 | grasp_pose_to_eef : [-0.09, 0.036, -0.02] 22 | grasp_pose_to_eef_rotation : [-1.22, 0.0, 0.0] 23 | 24 | end_effector_parent_link: 'l_wrist' 25 | # ===================================================== 26 | right_hand: 27 | end_effector_name: 'right_hand' #ee group name 28 | 29 | #actuated joints in end effector 30 | joints : ['RHand'] 31 | 32 | #open position 33 | pregrasp_posture : [0.0] #for sim 34 | pregrasp_time_from_start : 4.0 35 | 36 | #close position 37 | grasp_posture : [1.0] #for sim 38 | grasp_time_from_start : 4.0 39 | 40 | #desired pose from end effector to grasp - [x,y,z] 41 | #desired pose from end effector to grasp - [r, p, y] 42 | #side grasp 65deg 43 | grasp_pose_to_eef : [-0.09, -0.036, -0.02] 44 | grasp_pose_to_eef_rotation : [1.22, 0.0, 0.0] 45 | 46 | end_effector_parent_link: 'r_wrist' 47 | 48 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | moveit_simple_grasps 3 | 1.3.1 4 | 5 | A basic grasp generator for simple objects such as blocks or cylinders for use with the MoveIt! pick and place pipeline. 6 | Does not consider friction cones or other dynamics. 7 | 8 | Dave Coleman 9 | 10 | BSD 11 | 12 | https://github.com/davetcoleman/moveit_simple_grasps/ 13 | https://github.com/davetcoleman/moveit_simple_grasps/issues 14 | https://github.com/davetcoleman/moveit_simple_grasps 15 | 16 | Dave Coleman 17 | 18 | catkin 19 | 20 | roscpp 21 | eigen_conversions 22 | std_msgs 23 | trajectory_msgs 24 | tf 25 | tf_conversions 26 | moveit_ros_planning 27 | moveit_ros_planning_interface 28 | moveit_core 29 | moveit_visual_tools 30 | message_generation 31 | actionlib 32 | moveit_msgs 33 | geometry_msgs 34 | actionlib_msgs 35 | cmake_modules 36 | 37 | std_msgs 38 | trajectory_msgs 39 | moveit_msgs 40 | geometry_msgs 41 | actionlib_msgs 42 | message_runtime 43 | moveit_visual_tools 44 | 45 | 46 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: 2 | - cpp 3 | - python 4 | python: 5 | - "2.7" 6 | compiler: 7 | - gcc 8 | before_install: # Use this to prepare the system to install prerequisites or dependencies 9 | # Define some config vars 10 | - export ROS_DISTRO=hydro 11 | - export CI_SOURCE_PATH=$(pwd) 12 | - echo "Testing branch $TRAVIS_BRANCH " 13 | - pwd 14 | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' 15 | - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 16 | - sudo apt-get update 17 | - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-roscpp 18 | # MongoDB fix - I don't fully understand this 19 | - sudo apt-get remove -y mongodb mongodb-10gen 20 | - sudo apt-get install -y mongodb-clients mongodb-server -o Dpkg::Options::="--force-confdef" # default actions 21 | # Setup rosdep 22 | - sudo rosdep init 23 | - rosdep update 24 | install: # Use this to install any prerequisites or dependencies necessary to run your build 25 | # Create workspace 26 | - mkdir -p ~/ros/ws_test/src 27 | - cd ~/ros/ws_test/src 28 | - git clone https://github.com/davetcoleman/moveit_visual_tools # temporary until hydro package is released 29 | - git clone https://github.com/davetcoleman/graph_msgs.git # temporary 30 | # Delete the baxter.rosinstall version of the repo and use the one of the branch we are testing 31 | - ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace 32 | - cd ../ 33 | # Test manual install 34 | #- sudo apt-get install ros-hydro-gazebo-ros ros-hydro-ros-controllers 35 | # Install dependencies for source repos 36 | - rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y 37 | before_script: # Use this to prepare your build for testing e.g. copy database configurations, environment variables, etc. 38 | - source /opt/ros/$ROS_DISTRO/setup.bash 39 | script: # All commands must exit with code 0 on success. Anything else is considered failure. 40 | - catkin_make DCMAKE_BUILD_TYPE=Debug -j1 41 | #- catkin_make_isolated --source . -j1 --cmake-args --install 42 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(moveit_simple_grasps) 3 | 4 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11") 5 | 6 | # Load catkin and all dependencies required for this package 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | std_msgs 10 | eigen_conversions 11 | moveit_ros_planning 12 | moveit_ros_planning_interface 13 | moveit_core 14 | moveit_visual_tools 15 | tf 16 | tf_conversions 17 | trajectory_msgs 18 | message_generation 19 | geometry_msgs 20 | actionlib 21 | actionlib_msgs 22 | moveit_msgs 23 | cmake_modules 24 | ) 25 | 26 | find_package(Eigen REQUIRED) 27 | find_package(Boost REQUIRED thread system) 28 | 29 | add_message_files(DIRECTORY msg 30 | FILES 31 | GraspGeneratorOptions.msg 32 | ) 33 | 34 | add_action_files(DIRECTORY action 35 | FILES 36 | GenerateGrasps.action 37 | ) 38 | 39 | generate_messages(DEPENDENCIES 40 | geometry_msgs 41 | actionlib_msgs 42 | std_msgs 43 | moveit_msgs 44 | ) 45 | 46 | # Catkin 47 | catkin_package( 48 | LIBRARIES 49 | ${PROJECT_NAME} 50 | ${PROJECT_NAME}_filter 51 | CATKIN_DEPENDS 52 | actionlib_msgs 53 | geometry_msgs 54 | moveit_msgs 55 | trajectory_msgs 56 | std_msgs 57 | message_runtime 58 | moveit_visual_tools 59 | INCLUDE_DIRS include 60 | ) 61 | 62 | ## Build 63 | include_directories( 64 | include 65 | ${catkin_INCLUDE_DIRS} 66 | ) 67 | 68 | # Grasp Generator Library 69 | add_library(${PROJECT_NAME} 70 | src/simple_grasps.cpp 71 | src/grasp_data.cpp 72 | ) 73 | target_link_libraries(${PROJECT_NAME} 74 | ${catkin_LIBRARIES} ${Boost_LIBRARIES} 75 | ) 76 | 77 | # Grasp Filter Library 78 | add_library(${PROJECT_NAME}_filter 79 | src/grasp_filter.cpp 80 | ) 81 | target_link_libraries(${PROJECT_NAME}_filter 82 | ${catkin_LIBRARIES} ${Boost_LIBRARIES} 83 | ) 84 | 85 | # Action Server executable 86 | add_executable(${PROJECT_NAME}_server src/simple_grasps_server.cpp) 87 | add_dependencies(${PROJECT_NAME}_server ${catkin_EXPORTED_TARGETS}) # don't build until necessary msgs are finish 88 | target_link_libraries(${PROJECT_NAME}_server 89 | ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} 90 | ) 91 | 92 | # Test filter executable 93 | add_executable(${PROJECT_NAME}_filter_test src/grasp_filter_test.cpp) 94 | target_link_libraries(${PROJECT_NAME}_filter_test 95 | ${PROJECT_NAME} ${PROJECT_NAME}_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES} 96 | ) 97 | 98 | # Test grasp executable 99 | add_executable(${PROJECT_NAME}_test src/simple_grasps_test.cpp) 100 | target_link_libraries(${PROJECT_NAME}_test 101 | ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} 102 | ) 103 | 104 | ## Install 105 | 106 | # Install libraries 107 | install(TARGETS 108 | ${PROJECT_NAME} 109 | ${PROJECT_NAME}_filter 110 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 111 | 112 | # Install header files 113 | install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 114 | 115 | # Install shared resources 116 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 117 | install(DIRECTORY resources DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 118 | 119 | # Install executables 120 | install(TARGETS 121 | ${PROJECT_NAME}_server 122 | ${PROJECT_NAME}_test 123 | ${PROJECT_NAME}_filter_test 124 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 125 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 126 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package moveit_simple_grasps 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.3.1 (2015-12-07) 6 | ------------------ 7 | * catkin lint cleanup 8 | * Contributors: Dave Coleman 9 | 10 | 1.3.0 (2015-12-05) 11 | ------------------ 12 | * Z Axis implemented 13 | * Fixed API changes in moveit_visual_tools 14 | * Adding grasp configurations for Romeo, Nao and Pepper 15 | * Update README.md 16 | * Fix install space 17 | * New setRobotStatePreGrasp(), setRobotStateGrasp(), and setRobotState() functions for opening and closing EE 18 | * Fixed test launch 19 | * Added new test functions 20 | * Contributors: Dave Coleman, nlyubova, rheidrich 21 | 22 | 1.2.1 (2014-10-27) 23 | ------------------ 24 | * Refactored for new moveit_visual_tools API 25 | * Fixed package.xml 26 | * Updated README 27 | * Contributors: Dave Coleman 28 | 29 | 1.2.0 (2014-09-19) 30 | ------------------ 31 | 32 | 1.1.0 (2014-07-31) 33 | ------------------ 34 | * Fixed grasp pose rotation 35 | * Created new verbose constructor flag to enable easy debugging 36 | * Allow a grasp pose to be rotated along z axis 37 | * Created new pick place pipeline template 38 | * Moved ClamArm config to this repo 39 | * Updated package description 40 | * Updated README 41 | * Contributors: Dave Coleman 42 | 43 | 1.0.1 (2014-05-30) 44 | ------------------ 45 | * Moved base link out of individual end effector configurations 46 | * Fixed tests for new gripper config format 47 | * Fix for strict cppcheck and g++ warnings/errors 48 | * Remove self assignment 49 | * fix functions with no return statement and other cppcheck errors fix 50 | * Compatibility changes for ROS Indigo (Eigen find pkg) 51 | * Restored the lost grasp data for REEM 52 | * Renamed grasp data config file for REEM and updated launch file accordingly. 53 | * Fix lost contents of file and add left hand. 54 | * Enabled dual arm grasping, filtering 55 | * Fixes for more strict moveit_visual_tools data access 56 | * Deprecated function, made robot grasp config files have more than 1 end effector 57 | * Fixed posture bug and renamed local vars to not have _ postfix 58 | * Added ability to filter pre-grasps as well 59 | * Removed this-> because does not follow MoveIt style guidelines 60 | * Made tests left/right invariant 61 | * Refactored RobotGraspData and loader function 62 | * Renamed RobotGraspData to GraspData 63 | * Made graspDataLoader into a function the GraspData class. 64 | * Move grasp data struct to separate file 65 | * Fixed filter test 66 | * Created left hand config for baxter 67 | * Improved tests 68 | * Changed deprecated function name 69 | * Add options to grasp generation action goal. 70 | * Yaml conversion 71 | * Improved error handling of loading from yaml, removed unnecessary data 72 | * Fixes for new grasp data loader 73 | * Fixes per catkin_lintg 74 | * Convert baxter_data.h to baxter_gripper.yaml 75 | * Add launch script for grasp generator server + testing node 76 | * Replace reem_data.h with reem_hand.yaml 77 | * Add grasp_data_loader to be used by server. 78 | * Changed way visualizations are made 79 | * Moved the visualize grasp functionality to moveit_visual_tools. Deprecated generateAllGrasps 80 | * Fixed grasp filter bug of pose in wrong frame of reference 81 | * Trying to visualize arm reaches 82 | * Fixed grasp filter 83 | * Added new hand_roll feature 84 | * Changed name of moveit_simple_grasps 85 | * Renamed files and classes to not have the word MoveIt 86 | * Added picture 87 | * Added ability to do full grasp rotation, finer grained access, documentation 88 | * Fix travis 89 | * Changed name of moveit_visual_tools 90 | * Initial commit 91 | * Contributors: Bence Magyar, Dave Coleman, Jordi Pages 92 | -------------------------------------------------------------------------------- /config/romeo_grasp_data.yaml: -------------------------------------------------------------------------------- 1 | base_link: '/odom' 2 | 3 | # ===================================================== 4 | left_hand: 5 | end_effector_name: 'left_hand' #ee group name 6 | 7 | #actuated joints in end effector 8 | joints : ['LHand', 'LWristPitch'] 9 | 10 | #open position 11 | #pregrasp_posture : [1.0, 0.0] #for the real robot 12 | pregrasp_posture : [0.0, 0.0] #for sim 13 | pregrasp_time_from_start : 4.0 14 | 15 | #close position 16 | #grasp_posture : [0.0, 0.0] #for the real robot 17 | grasp_posture : [1.0, 0.0] #for sim 18 | grasp_time_from_start : 4.0 19 | 20 | #desired pose from end effector to grasp - [x,y,z] 21 | #desired pose from end effector to grasp - [r, p, y] #1.5707 = PI/2 22 | 23 | #side grasp -pi/2 24 | #grasp_pose_to_eef : [-0.12, 0.045, 0.0] 25 | #grasp_pose_to_eef_rotation : [-1.5707, 0.0, 0.0] 26 | #side grasp -pi/3 27 | #grasp_pose_to_eef : [-0.1, 0.055, 0.0] 28 | #grasp_pose_to_eef_rotation : [-1.0472, 0.0, 0.0] 29 | #side grasp -pi/3 - best 30 | grasp_pose_to_eef : [-0.113, 0.036, 0.036] 31 | grasp_pose_to_eef_rotation : [-1.032, 0.0, 0.0] 32 | #side grasp -pi/4 33 | #grasp_pose_to_eef : [-0.1, 0.055, 0.0] 34 | #grasp_pose_to_eef_rotation : [-1.025, 0.0, 0.0] 35 | #front grasp 36 | #grasp_pose_to_eef : [-0.1, 0.07, -0.05] 37 | #grasp_pose_to_eef_rotation : [-0.7854, 0.0, 0.0] 38 | 39 | #top grasp pi/2 40 | #grasp_pose_to_eef : [-0.07, 0.0, 0.125] 41 | #grasp_pose_to_eef_rotation : [0, 1.5707, 0] 42 | #top grasp 0 deg 43 | #grasp_pose_to_eef : [-0.095, 0.0, 0.095] 44 | #grasp_pose_to_eef_rotation : [0, 0, 0] 45 | #top grasp 30 deg 46 | #grasp_pose_to_eef : [-0.095, 0.0, 0.11] 47 | #grasp_pose_to_eef_rotation : [0, 0.52, 0] 48 | #top grasp 35 deg 49 | #grasp_pose_to_eef : [-0.055, 0.0, 0.135] 50 | #grasp_pose_to_eef_rotation : [0, 0.61, 0] 51 | #top grasp 40 deg - best 52 | #grasp_pose_to_eef : [-0.03, 0.0, 0.145] 53 | #grasp_pose_to_eef_rotation : [0, 0.7, 0] 54 | 55 | end_effector_parent_link: 'LWristYaw_link' 56 | # ===================================================== 57 | right_hand: 58 | end_effector_name: 'right_hand' #ee group name 59 | 60 | #actuated joints in end effector 61 | joints : ['RHand', 'RWristPitch'] 62 | 63 | #open position 64 | #pregrasp_posture : [1.0, 0.0] #for the real robot 65 | pregrasp_posture : [0.0, 0.0] #for sim 66 | pregrasp_time_from_start : 4.0 67 | 68 | #close position 69 | #grasp_posture : [0.0, 0.0] #for the real robot 70 | grasp_posture : [1.0, 0.0] #for sim 71 | grasp_time_from_start : 4.0 72 | 73 | #desired pose from end effector to grasp - [x,y,z] 74 | #desired pose from end effector to grasp - [r, p, y] 75 | 76 | #side grasp pi/2 77 | #grasp_pose_to_eef : [-0.12, -0.045, 0.0] 78 | #grasp_pose_to_eef_rotation : [1.5707, 0.0, 0.0] 79 | #side grasp pi/3 80 | #grasp_pose_to_eef : [-0.1, -0.055, 0.0] 81 | #grasp_pose_to_eef_rotation : [1.0472, 0.0, 0.0] 82 | #side grasp pi/3 - best 83 | #grasp_pose_to_eef : [-0.113, -0.036, 0.036] 84 | #grasp_pose_to_eef_rotation : [1.032, 0.0, 0.0] 85 | #side grasp pi/4 86 | #grasp_pose_to_eef : [-0.1, -0.055, 0.0] 87 | #grasp_pose_to_eef_rotation : [1.025, 0.0, 0.0] 88 | #grasp pi * 2/3 89 | #grasp_pose_to_eef : [-0.1, 0.055, 0.0] 90 | #grasp_pose_to_eef_rotation : [-2.0944, 0.0, 0.0] 91 | 92 | #top grasp pi/2 93 | #grasp_pose_to_eef : [-0.07, 0.0, 0.125] 94 | #grasp_pose_to_eef_rotation : [0, 1.5707, 0] 95 | #top grasp 40 deg - best 96 | grasp_pose_to_eef : [-0.03, 0.0, 0.145] 97 | grasp_pose_to_eef_rotation : [0, 0.7, 0] 98 | 99 | end_effector_parent_link: 'RWristYaw_link' 100 | 101 | -------------------------------------------------------------------------------- /scripts/server_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2014, PAL Robotics SL 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the PAL Robotics nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # author: Bence Magyar 35 | 36 | import rospy 37 | from actionlib import SimpleActionClient 38 | from geometry_msgs.msg import PoseArray, Pose, Point 39 | from std_msgs.msg import Header 40 | from moveit_simple_grasps.msg import GenerateGraspsAction, GenerateGraspsGoal 41 | from moveit_msgs.msg import Grasp 42 | import geometry_msgs 43 | 44 | grasp_publisher = None 45 | grasps_ac = None 46 | 47 | def generate_grasps(pose, width): 48 | #send request to block grasp generator service 49 | grasps_ac.wait_for_server() 50 | rospy.loginfo("Successfully connected.") 51 | goal = GenerateGraspsGoal() 52 | goal.pose = pose.pose 53 | goal.width = width 54 | grasps_ac.send_goal(goal) 55 | rospy.loginfo("Sent goal, waiting:\n" + str(goal)) 56 | t_start = rospy.Time.now() 57 | grasps_ac.wait_for_result() 58 | t_end = rospy.Time.now() 59 | t_total = t_end - t_start 60 | rospy.loginfo("Result received in " + str(t_total.to_sec())) 61 | grasp_list = grasps_ac.get_result().grasps 62 | return grasp_list 63 | 64 | def publish_grasps_as_poses(grasps): 65 | rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose") 66 | graspmsg = Grasp() 67 | grasp_PA = PoseArray() 68 | header = Header() 69 | header.frame_id = "base_link" 70 | header.stamp = rospy.Time.now() 71 | grasp_PA.header = header 72 | for graspmsg in grasps: 73 | p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation) 74 | grasp_PA.poses.append(p) 75 | grasp_publisher.publish(grasp_PA) 76 | rospy.loginfo('Published ' + str(len(grasp_PA.poses)) + ' poses') 77 | rospy.sleep(2) 78 | 79 | 80 | if __name__ == '__main__': 81 | name = 'grasp_object_server' 82 | rospy.init_node(name, anonymous=False) 83 | rospy.loginfo("Connecting to grasp generator AS") 84 | grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) 85 | grasp_publisher = rospy.Publisher("generated_grasps", PoseArray) 86 | object_pose = geometry_msgs.msg.PoseStamped() 87 | object_pose.pose.position.x = 1.0 88 | object_pose.pose.position.y = 1.0 89 | object_pose.pose.position.z = 1.0 90 | object_pose.pose.orientation.w = 1.0 91 | object_pose.pose.orientation.x = 0.0 92 | object_pose.pose.orientation.y = 0.0 93 | object_pose.pose.orientation.z = 0.0 94 | grasp_list = generate_grasps(object_pose, 0.06) 95 | rospy.loginfo('Generated ' + str(len(grasp_list)) + ' grasps.') 96 | publish_grasps_as_poses(grasp_list) 97 | rospy.sleep(10.0) 98 | -------------------------------------------------------------------------------- /include/moveit_simple_grasps/custom_environment2.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Custom environments for running MoveIt! 37 | */ 38 | 39 | #include // simple tool for showing grasps 40 | 41 | #ifndef BAXTER_PICK_PLACE__CUSTOM_ENVIRONMENT_ 42 | #define BAXTER_PICK_PLACE__CUSTOM_ENVIRONMENT_ 43 | 44 | namespace baxter_pick_place 45 | { 46 | 47 | // environment 48 | static const std::string SUPPORT_SURFACE1_NAME = "monitor"; 49 | static const std::string SUPPORT_SURFACE2_NAME = "desk"; 50 | static const std::string SUPPORT_SURFACE3_NAME = "table"; 51 | static const std::string WALL1_NAME = "back_wall"; 52 | static const std::string WALL2_NAME = "right_wall"; 53 | static const std::string WALL3_NAME = "left_wall"; 54 | 55 | // table dimensions 56 | static const double TABLE_HEIGHT = 0.818; 57 | 58 | static const double TABLE_WIDTH = 0.87; 59 | static const double TABLE_DEPTH = 0.44; 60 | static const double TABLE_X = 0.83; 61 | static const double TABLE_Y = 0.15; 62 | 63 | // object dimensions 64 | static const double OBJECT_SIZE = 0.04; 65 | 66 | void createEnvironment(moveit_visual_tools::MoveItVisualToolsPtr visual_tools_) 67 | { 68 | visual_tools_->cleanupCO(SUPPORT_SURFACE1_NAME); 69 | visual_tools_->cleanupCO(SUPPORT_SURFACE2_NAME); 70 | visual_tools_->cleanupCO(WALL1_NAME); 71 | visual_tools_->cleanupCO(WALL2_NAME); 72 | visual_tools_->cleanupCO(WALL3_NAME); 73 | 74 | // -------------------------------------------------------------------------------------------- 75 | // Add objects to scene 76 | 77 | // Walls x, y, angle, width, height, name 78 | visual_tools_->publishCollisionWall(-0.55, 0, 0, 2.2, 1.5, WALL1_NAME); // back wall 79 | visual_tools_->publishCollisionWall(0.05, -1.1, M_PI/2, 2.0, 1.5, WALL2_NAME); // baxter's right 80 | visual_tools_->publishCollisionWall(0.05, 1.1, M_PI/2, 2.0, 1.5, WALL3_NAME); // baxter's left 81 | 82 | // Tables x, y, z, angle, width, height, depth, name 83 | visual_tools_->publishCollisionTable(0.78, -0.8, 0, 0, 0.4, 1.4, 0.47, SUPPORT_SURFACE1_NAME); // computer monitor 84 | visual_tools_->publishCollisionTable(0.78, -0.45, 0, 0, 0.4, 0.7, 0.47, SUPPORT_SURFACE2_NAME); // my desk 85 | visual_tools_->publishCollisionTable(TABLE_X, TABLE_Y, 0, 0, TABLE_WIDTH, TABLE_HEIGHT, TABLE_DEPTH, SUPPORT_SURFACE3_NAME); // andy table 86 | } 87 | 88 | double getTableHeight(double floor_offset) 89 | { 90 | return TABLE_HEIGHT + floor_offset + OBJECT_SIZE / 2; 91 | } 92 | 93 | void getTableWidthRange(double &y_min, double &y_max) 94 | { 95 | y_min = TABLE_Y - TABLE_WIDTH / 2; 96 | y_max = TABLE_Y + TABLE_WIDTH / 2; 97 | } 98 | 99 | void getTableDepthRange(double &x_min, double &x_max) 100 | { 101 | x_min = TABLE_X - TABLE_DEPTH / 2; 102 | x_max = TABLE_X + TABLE_DEPTH / 2; 103 | } 104 | 105 | } // namespace 106 | 107 | #endif 108 | -------------------------------------------------------------------------------- /include/moveit_simple_grasps/grasp_data.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Modified BSD License) 3 | * 4 | * Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Univ of CO, Boulder, PAL Robotics, S.L. 18 | * nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | /* Authors: Bence Magyar, Dave Coleman 37 | Description: Data class used by the grasp generator. 38 | */ 39 | 40 | #ifndef MOVEIT_SIMPLE_GRASPS__GRASP_DATA_H_ 41 | #define MOVEIT_SIMPLE_GRASPS__GRASP_DATA_H_ 42 | 43 | // Ros 44 | #include 45 | 46 | // Msgs 47 | #include 48 | #include 49 | #include 50 | 51 | namespace moveit_simple_grasps 52 | { 53 | 54 | class GraspData 55 | { 56 | public: 57 | geometry_msgs::Pose grasp_pose_to_eef_pose_; // Convert generic grasp pose to this end effector's frame of reference 58 | trajectory_msgs::JointTrajectory pre_grasp_posture_; // when the end effector is in "open" position 59 | trajectory_msgs::JointTrajectory grasp_posture_; // when the end effector is in "close" position 60 | std::string base_link_; // name of global frame with z pointing up 61 | std::string ee_parent_link_; // the last link in the kinematic chain before the end effector, e.g. "/gripper_roll_link" 62 | std::string ee_group_; // the end effector name 63 | double grasp_depth_; // distance from center point of object to end effector 64 | int angle_resolution_; // generate grasps at PI/angle_resolution increments 65 | double approach_retreat_desired_dist_; // how far back from the grasp position the pregrasp phase should be 66 | double approach_retreat_min_dist_; // how far back from the grasp position the pregrasp phase should be at minimum 67 | double object_size_; // for visualization 68 | 69 | public: 70 | 71 | /** 72 | * \brief Constructor 73 | */ 74 | GraspData(); 75 | 76 | /** 77 | * \brief Loads grasp data from a yaml file (load from roslaunch) 78 | * \param node handle - allows for namespacing 79 | * \param end effector name - which side of a two handed robot to load data for. should correspond to SRDF EE names 80 | * \return true on success 81 | */ 82 | bool loadRobotGraspData(const ros::NodeHandle& nh, const std::string& end_effector); 83 | 84 | /** 85 | * \brief Alter a robot state so that the end effector corresponding to this grasp data is in pre-grasp state (OPEN) 86 | * \param joint state of robot 87 | * \return true on success 88 | */ 89 | bool setRobotStatePreGrasp( robot_state::RobotStatePtr &robot_state ); 90 | 91 | /** 92 | * \brief Alter a robot state so that the end effector corresponding to this grasp data is in grasp state (CLOSED) 93 | * \param joint state of robot 94 | * \return true on success 95 | */ 96 | bool setRobotStateGrasp( robot_state::RobotStatePtr &robot_state ); 97 | 98 | /** 99 | * \brief Alter a robot state so that the end effector corresponding to this grasp data is in a grasp posture 100 | * \param joint state of robot 101 | * \param posture - what state to set the end effector 102 | * \return true on success 103 | */ 104 | bool setRobotState( robot_state::RobotStatePtr &robot_state, const trajectory_msgs::JointTrajectory &posture ); 105 | 106 | /** 107 | * \brief Debug data to console 108 | */ 109 | void print(); 110 | }; 111 | 112 | } // namespace 113 | 114 | #endif 115 | -------------------------------------------------------------------------------- /include/moveit_simple_grasps/grasp_filter.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | // Author: Dave Coleman 36 | // Desc: Filters grasps based on kinematic feasibility 37 | 38 | #ifndef MOVEIT_SIMPLE_GRASPS__GRASP_FILTER_ 39 | #define MOVEIT_SIMPLE_GRASPS__GRASP_FILTER_ 40 | 41 | // ROS 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | // Grasping 49 | #include 50 | 51 | // Rviz 52 | #include 53 | 54 | // MoveIt 55 | #include 56 | #include 57 | 58 | // C++ 59 | #include 60 | #include 61 | #define _USE_MATH_DEFINES 62 | 63 | namespace moveit_simple_grasps 64 | { 65 | 66 | // Struct for passing parameters to threads, for cleaner code 67 | struct IkThreadStruct 68 | { 69 | IkThreadStruct( 70 | std::vector &possible_grasps, // the input 71 | std::vector &filtered_grasps, // the result 72 | std::vector &ik_solutions, // the resulting solutions for each filtered grasp 73 | Eigen::Affine3d &link_transform, 74 | int grasps_id_start, 75 | int grasps_id_end, 76 | kinematics::KinematicsBaseConstPtr kin_solver, 77 | bool filter_pregrasp, 78 | std::string ee_parent_link, 79 | double timeout, 80 | boost::mutex *lock, 81 | int thread_id) 82 | : possible_grasps_(possible_grasps), 83 | filtered_grasps_(filtered_grasps), 84 | ik_solutions_(ik_solutions), 85 | link_transform_(link_transform), 86 | grasps_id_start_(grasps_id_start), 87 | grasps_id_end_(grasps_id_end), 88 | kin_solver_(kin_solver), 89 | filter_pregrasp_(filter_pregrasp), 90 | ee_parent_link_(ee_parent_link), 91 | timeout_(timeout), 92 | lock_(lock), 93 | thread_id_(thread_id) 94 | { 95 | } 96 | std::vector &possible_grasps_; 97 | std::vector &filtered_grasps_; 98 | std::vector &ik_solutions_; 99 | Eigen::Affine3d link_transform_; 100 | int grasps_id_start_; 101 | int grasps_id_end_; 102 | kinematics::KinematicsBaseConstPtr kin_solver_; 103 | bool filter_pregrasp_; 104 | std::string ee_parent_link_; 105 | double timeout_; 106 | boost::mutex *lock_; 107 | int thread_id_; 108 | }; 109 | 110 | 111 | // Class 112 | class GraspFilter 113 | { 114 | private: 115 | // State of robot 116 | robot_state::RobotState robot_state_; 117 | 118 | // threaded kinematic solvers 119 | std::map > kin_solvers_; 120 | 121 | // class for publishing stuff to rviz 122 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; 123 | 124 | bool verbose_; 125 | 126 | public: 127 | 128 | // Constructor 129 | GraspFilter( robot_state::RobotState robot_state, 130 | moveit_visual_tools::MoveItVisualToolsPtr& visual_tools ); 131 | 132 | // Destructor 133 | ~GraspFilter(); 134 | 135 | // Of an array of grasps, choose just one for use 136 | bool chooseBestGrasp( const std::vector& possible_grasps, 137 | moveit_msgs::Grasp& chosen ); 138 | 139 | // Take the nth grasp from the array 140 | bool filterNthGrasp(std::vector& possible_grasps, int n); 141 | 142 | /** 143 | * \brief Choose the 1st grasp that is kinematically feasible 144 | * \param 145 | * \param 146 | * \param whether to also check ik feasibility for the pregrasp position 147 | * \return true on success 148 | */ 149 | bool filterGrasps(std::vector& possible_grasps, 150 | std::vector& ik_solutions, 151 | bool filter_pregrasp, const std::string &ee_parent_link, 152 | const std::string& planning_group); 153 | 154 | private: 155 | 156 | /** 157 | * \brief Thread for checking part of the possible grasps list 158 | * \param 159 | */ 160 | void filterGraspThread(IkThreadStruct ik_thread_struct); 161 | 162 | 163 | }; // end of class 164 | 165 | typedef boost::shared_ptr GraspFilterPtr; 166 | typedef boost::shared_ptr GraspFilterConstPtr; 167 | 168 | } // namespace 169 | 170 | #endif 171 | -------------------------------------------------------------------------------- /include/moveit_simple_grasps/simple_grasps.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | // Author: Dave Coleman 36 | // Desc: Generates grasps for a cube 37 | 38 | #ifndef MOVEIT_SIMPLE_GRASPS__MOVEIT_SIMPLE_GRASPS_H_ 39 | #define MOVEIT_SIMPLE_GRASPS__MOVEIT_SIMPLE_GRASPS_H_ 40 | 41 | // ROS 42 | #include 43 | 44 | // TF 45 | #include 46 | 47 | // Msgs 48 | #include 49 | 50 | // MoveIt 51 | #include 52 | #include 53 | 54 | // Eigen 55 | #include 56 | #include 57 | #include 58 | 59 | // Visualization 60 | #include 61 | 62 | // C++ 63 | #include 64 | #define _USE_MATH_DEFINES 65 | 66 | #include 67 | 68 | namespace moveit_simple_grasps 69 | { 70 | 71 | static const double RAD2DEG = 57.2957795; 72 | 73 | // Grasp axis orientation 74 | enum grasp_axis_t {X_AXIS, Y_AXIS, Z_AXIS}; 75 | enum grasp_direction_t {UP, DOWN}; 76 | enum grasp_rotation_t {FULL, HALF}; 77 | 78 | // Class 79 | class SimpleGrasps 80 | { 81 | private: 82 | 83 | // class for publishing stuff to rviz 84 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; 85 | 86 | // Transform from frame of box to global frame 87 | Eigen::Affine3d object_global_transform_; 88 | 89 | // Display more output both in console and in Rviz (with arrows and markers) 90 | bool verbose_; 91 | 92 | public: 93 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Eigen requires 128-bit alignment for the Eigen::Vector2d's array (of 2 doubles). With GCC, this is done with a attribute ((aligned(16))). 94 | 95 | /** 96 | * \brief Constructor 97 | */ 98 | SimpleGrasps(moveit_visual_tools::MoveItVisualToolsPtr rviz_tools, bool verbose = false); 99 | 100 | /** 101 | * \brief Destructor 102 | */ 103 | ~SimpleGrasps(); 104 | 105 | /** 106 | * \brief Moved to generateBlockGrasps 107 | */ 108 | MOVEIT_DEPRECATED bool generateAllGrasps(const geometry_msgs::Pose& object_pose, const GraspData& grasp_data, 109 | std::vector& possible_grasps) 110 | { 111 | generateBlockGrasps(object_pose, grasp_data, possible_grasps); 112 | 113 | return true; 114 | } 115 | 116 | /** 117 | * \brief Create all possible grasp positions for a block 118 | * \param pose of block, where vector arrow is parallel to table plane 119 | * \param data describing end effector 120 | * \param resulting generated possible grasps 121 | * \return true if successful 122 | */ 123 | bool generateBlockGrasps(const geometry_msgs::Pose& object_pose, const GraspData& grasp_data, 124 | std::vector& possible_grasps); 125 | 126 | /** 127 | * \brief Create grasp positions in one axis around a single pose 128 | * Note: to visualize these grasps use moveit_visual_tools.publishAnimatedGrasps() function or 129 | * moveit_visual_tools.publishIKSolutions() with the resulting data 130 | * \param pose - center point of object to be grasped 131 | * \param axis - axis relative to object pose to rotate generated grasps around 132 | * \param direction - a parallel gripper is typically symetric such that it can perform the same grasp 133 | * 180 degree around. this option allows to generate a flipped grasp pose 134 | * \param rotation - amount to rotate around the object - 180 or 360 degrees 135 | * \param hand_roll - amount in radians to roll wrist with respect to center point of object during grasp. use 0 by default 136 | * \param grasp_data - parameters specific to the robot geometry 137 | * \param possible_grasps - the output solution vector of possible grasps to attempt. ok if pre-populated 138 | * \return true if successful 139 | */ 140 | bool generateAxisGrasps( 141 | const geometry_msgs::Pose& object_pose, 142 | grasp_axis_t axis, 143 | grasp_direction_t direction, 144 | grasp_rotation_t rotation, 145 | double hand_roll, 146 | const GraspData& grasp_data, 147 | std::vector& possible_grasps); 148 | 149 | /** 150 | * \brief Using an input grasp description, get the pregrasp pose 151 | * \param grasp description 152 | * \param name of parent link 153 | * \return pregrasp pose 154 | */ 155 | static geometry_msgs::PoseStamped getPreGraspPose(const moveit_msgs::Grasp &grasp, const std::string &ee_parent_link); 156 | 157 | /** 158 | * \brief Print debug info 159 | * DEPRECATRED: moved to grasp_data.cpp 160 | */ 161 | MOVEIT_DEPRECATED static void printObjectGraspData(const GraspData& data) 162 | { 163 | ROS_INFO_STREAM_NAMED("grasp","ROBOT GRASP DATA DEBUG OUTPUT ---------------------"); 164 | ROS_INFO_STREAM_NAMED("grasp","Base Link: " << data.base_link_); 165 | ROS_INFO_STREAM_NAMED("grasp","EE Parent Link: " << data.ee_parent_link_); 166 | ROS_INFO_STREAM_NAMED("grasp","Grasp Depth: " << data.grasp_depth_); 167 | ROS_INFO_STREAM_NAMED("grasp","Angle Resolution: " << data.angle_resolution_); 168 | ROS_INFO_STREAM_NAMED("grasp","Approach Retreat Desired Dist: " << data.approach_retreat_desired_dist_); 169 | ROS_INFO_STREAM_NAMED("grasp","Approach Retreat Min Dist: " << data.approach_retreat_min_dist_); 170 | ROS_INFO_STREAM_NAMED("grasp","Pregrasp Posture: \n" << data.pre_grasp_posture_); 171 | ROS_INFO_STREAM_NAMED("grasp","Grasp Posture: \n" << data.grasp_posture_); 172 | ROS_INFO_STREAM_NAMED("grasp","---------------------------------------------------\n"); 173 | } 174 | 175 | }; // end of class 176 | 177 | typedef boost::shared_ptr SimpleGraspsPtr; 178 | typedef boost::shared_ptr SimpleGraspsConstPtr; 179 | 180 | } // namespace 181 | 182 | #endif 183 | -------------------------------------------------------------------------------- /src/simple_grasps_server.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Modified BSD License) 3 | * 4 | * Copyright (c) 2013-2014, PAL Robotics, S.L. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of PAL Robotics, S.L. nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | /* Author: Bence Magyar 36 | Desc: Action server wrapper for object grasp generator. Currently only works for REEM robot, 37 | needs to be changed to work with yaml configuration file instead. 38 | */ 39 | 40 | // ROS 41 | #include 42 | #include 43 | #include 44 | 45 | // Grasp generation 46 | #include 47 | #include 48 | #include 49 | 50 | 51 | // Baxter specific properties 52 | #include 53 | #include 54 | 55 | namespace moveit_simple_grasps 56 | { 57 | 58 | bool graspGeneratorOptions2Inner( 59 | const moveit_simple_grasps::GraspGeneratorOptions &options, 60 | grasp_axis_t &axis, 61 | grasp_direction_t &direction, 62 | grasp_rotation_t &rotation) 63 | { 64 | switch(options.grasp_axis) 65 | { 66 | case GraspGeneratorOptions::GRASP_AXIS_X: 67 | axis = X_AXIS; 68 | break; 69 | case GraspGeneratorOptions::GRASP_AXIS_Y: 70 | axis = Y_AXIS; 71 | break; 72 | case GraspGeneratorOptions::GRASP_AXIS_Z: 73 | axis = Z_AXIS; 74 | break; 75 | default: 76 | assert(false); 77 | break; 78 | } 79 | 80 | switch(options.grasp_direction) 81 | { 82 | case GraspGeneratorOptions::GRASP_DIRECTION_UP: 83 | direction = UP; 84 | break; 85 | case GraspGeneratorOptions::GRASP_DIRECTION_DOWN: 86 | direction = DOWN; 87 | break; 88 | default: 89 | assert(false); 90 | break; 91 | } 92 | 93 | switch(options.grasp_rotation) 94 | { 95 | case GraspGeneratorOptions::GRASP_ROTATION_FULL: 96 | rotation = FULL; 97 | break; 98 | case GraspGeneratorOptions::GRASP_ROTATION_HALF: 99 | rotation = HALF; 100 | break; 101 | default: 102 | assert(false); 103 | break; 104 | } 105 | return true; 106 | } 107 | 108 | class GraspGeneratorServer 109 | { 110 | private: 111 | // A shared node handle 112 | ros::NodeHandle nh_; 113 | 114 | // Action server 115 | actionlib::SimpleActionServer as_; 116 | moveit_simple_grasps::GenerateGraspsResult result_; 117 | 118 | // Grasp generator 119 | moveit_simple_grasps::SimpleGraspsPtr simple_grasps_; 120 | 121 | // class for publishing stuff to rviz 122 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; 123 | 124 | // robot-specific data for generating grasps 125 | moveit_simple_grasps::GraspData grasp_data_; 126 | 127 | // which arm are we using 128 | std::string side_; 129 | std::string planning_group_name_; 130 | 131 | public: 132 | 133 | // Constructor 134 | GraspGeneratorServer(const std::string &name, const std::string &side) 135 | : nh_("~") 136 | , as_(nh_, name, boost::bind(&moveit_simple_grasps::GraspGeneratorServer::executeCB, this, _1), false) 137 | , side_(side) 138 | , planning_group_name_(side_+"_arm") 139 | { 140 | // --------------------------------------------------------------------------------------------- 141 | // Load grasp data specific to our robot 142 | if (!grasp_data_.loadRobotGraspData(nh_, side_)) 143 | ros::shutdown(); 144 | 145 | // --------------------------------------------------------------------------------------------- 146 | // Load the Robot Viz Tools for publishing to Rviz 147 | visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools(grasp_data_.base_link_)); 148 | visual_tools_->setLifetime(120.0); 149 | const robot_model::JointModelGroup* ee_jmg = visual_tools_->getRobotModel()->getJointModelGroup(grasp_data_.ee_group_); 150 | visual_tools_->loadEEMarker(ee_jmg); 151 | 152 | // --------------------------------------------------------------------------------------------- 153 | // Load grasp generator 154 | simple_grasps_.reset( new moveit_simple_grasps::SimpleGrasps(visual_tools_) ); 155 | as_.start(); 156 | } 157 | 158 | void executeCB(const moveit_simple_grasps::GenerateGraspsGoalConstPtr &goal) 159 | { 160 | // --------------------------------------------------------------------------------------------- 161 | // Remove previous results 162 | result_.grasps.clear(); 163 | 164 | // --------------------------------------------------------------------------------------------- 165 | // Set object width and generate grasps 166 | grasp_data_.object_size_ = goal->width; 167 | 168 | // Generate grasps for all options that were passed 169 | grasp_axis_t axis; 170 | grasp_direction_t direction; 171 | grasp_rotation_t rotation; 172 | for(size_t i=0; ioptions.size(); ++i) 173 | { 174 | graspGeneratorOptions2Inner(goal->options[i], axis, direction, rotation); 175 | simple_grasps_->generateAxisGrasps(goal->pose, axis, direction, rotation, 0, grasp_data_, result_.grasps); 176 | } 177 | // fallback behaviour, generate default grasps when no options were passed 178 | if(goal->options.empty()) 179 | { 180 | simple_grasps_->generateBlockGrasps(goal->pose, grasp_data_, result_.grasps); 181 | } 182 | 183 | // --------------------------------------------------------------------------------------------- 184 | // Publish results 185 | as_.setSucceeded(result_); 186 | } 187 | 188 | }; 189 | } 190 | 191 | 192 | int main(int argc, char *argv[]) 193 | { 194 | ros::init(argc, argv, "grasp_generator_server"); 195 | moveit_simple_grasps::GraspGeneratorServer grasp_generator_server("generate", "right"); 196 | ros::spin(); 197 | return 0; 198 | } 199 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SUPPORT FOR THIS PACKAGE HAS ENDED 2 | Sorry, too many things to maintain. I'll still merge PRs and am happy to share maintainership of this package with someone interested. 3 | 4 | MoveIt! Simple Grasps 5 | ==================== 6 | 7 | A basic grasp generator for simple objects such as blocks or cylinders for use with the MoveIt! pick and place pipeline. Does not consider friction cones or other dynamics. 8 | 9 | Its current implementation simple takes as input a pose vector (postition and orientation) and generates a large number of potential grasp approaches and directions. Also includes a grasp filter for removing kinematically infeasible grasps via threaded IK solvers. 10 | 11 | This package includes: 12 | 13 | - Simple pose-based grasp generator for a block 14 | - Separate grasp generators for custom objects such as rectanguar or cylindrical objects 15 | - Grasp filter 16 | - Test code and visualizations 17 | 18 | Developed by [Dave Coleman](http://dav.ee) at the Correll Robotics Lab, University of Colorado Boulder with outside contributors. 19 | 20 | 21 | 22 | ## Video Demo 23 | 24 | A simple demo with Baxter: 25 | 26 | [![Baxter Grasp Test](http://img.youtube.com/vi/WEDITCR2qH4/0.jpg)](https://www.youtube.com/watch?v=WEDITCR2qH4) 27 | 28 | ## Build Status 29 | 30 | [![Build Status](https://travis-ci.org/davetcoleman/moveit_simple_grasps.png?branch=hydro-devel)](https://travis-ci.org/davetcoleman/moveit_simple_grasps) 31 | 32 | ## Install 33 | 34 | ### Ubuntu Debian 35 | 36 | Hydro: 37 | ``` 38 | sudo apt-get install ros-hydro-moveit-simple-grasps 39 | ``` 40 | Indigo: 41 | ``` 42 | sudo apt-get install ros-indigo-moveit-simple-grasps 43 | ``` 44 | 45 | ### Install From Source 46 | 47 | Clone this repository into a catkin workspace, then use the rosdep install tool to automatically download its dependencies. Depending on your current version of ROS, use: 48 | 49 | Hydro: 50 | ``` 51 | rosdep install --from-paths src --ignore-src --rosdistro hydro 52 | ``` 53 | Indigo: 54 | ``` 55 | rosdep install --from-paths src --ignore-src --rosdistro indigo 56 | ``` 57 | 58 | ## Robot-Agnostic Configuration 59 | 60 | You will first need a configuration file that described your robot's end effector geometry. Currently an example format can be seen in this repository at [config/baxter_grasp_data.yaml](https://github.com/davetcoleman/moveit_simple_grasps/blob/hydro-devel/config/baxter_grasp_data.yaml). See the comments within that file for explanations. 61 | 62 | To load that file at launch, you copy the example in the file [launch/grasp_test.launch](https://github.com/davetcoleman/moveit_simple_grasps/blob/hydro-devel/launch/grasp_test.launch) where you should see the line ````. 63 | 64 | ## Code Usage 65 | 66 | Note: You might find the moveit_blocks.h example, discussed at the bottom of this page, most helpful. 67 | 68 | We will discuss how to use the generation, filtering, and visualization components. 69 | 70 | Within your robot's ROS package, add this package to your package.xml, CMakeLists.txt. Then in whatever C++ file add this to your includes: 71 | ``` 72 | // Grasp generation and visualization 73 | #include 74 | #include 75 | #include 76 | ``` 77 | 78 | Add to your class's member variables the following: 79 | ``` 80 | // Grasp generator 81 | moveit_simple_grasps::SimpleGraspsPtr simple_grasps_; 82 | 83 | // class for publishing stuff to rviz 84 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; 85 | 86 | // robot-specific data for generating grasps 87 | moveit_simple_grasps::GraspData grasp_data_; 88 | ``` 89 | 90 | In your class' constructor initialize the visualization tools; 91 | ``` 92 | // Load the Robot Viz Tools for publishing to Rviz 93 | visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools("base_link")); 94 | ``` 95 | Change the first parameter of visual tools to the name of your robot's base link. For more information on that package, see [moveit_visual_tools](https://github.com/davetcoleman/moveit_visual_tools). 96 | 97 | Then load your robot's custom .yaml grasp data file: 98 | ``` 99 | // Load grasp data specific to our robot 100 | ros::NodeHandle nh("~"); 101 | if (!grasp_data_.loadRobotGraspData(nh, "left_hand")) 102 | ros::shutdown(); 103 | ``` 104 | Where "left_hand" is the name of one your SRDF-defined MoveIt! end effectors from the Setup Assistant. This data is loaded from a file that you must load to the parameter server within a roslaunch file, as desribed above. 105 | 106 | Now load grasp generator: 107 | ``` 108 | // Load grasp generator 109 | simple_grasps_.reset( new moveit_simple_grasps::SimpleGrasps(visual_tools_) ); 110 | ``` 111 | 112 | To generate grasps, you first need the pose of the object you want to grasp, such as a block. Here's an example pose: 113 | ``` 114 | geometry_msgs::Pose object_pose; 115 | object_pose.position.x = 0.4; 116 | object_pose.position.y = -0.2; 117 | object_pose.position.z = 0.0; 118 | 119 | // Orientation 120 | double angle = M_PI / 1.5; 121 | Eigen::Quaterniond quat(Eigen::AngleAxis(double(angle), Eigen::Vector3d::UnitZ())); 122 | object_pose.orientation.x = quat.x(); 123 | object_pose.orientation.y = quat.y(); 124 | object_pose.orientation.z = quat.z(); 125 | object_pose.orientation.w = quat.w(); 126 | ``` 127 | 128 | If you want to visualize this object pose as a block: 129 | ``` 130 | visual_tools_->publishBlock(object_pose, rviz_visual_tools::BLUE, 0.04); 131 | ``` 132 | 133 | Now generate the grasps: 134 | ``` 135 | std::vector possible_grasps; 136 | simple_grasps_->generateBlockGrasps( object_pose, grasp_data_, possible_grasps); 137 | ``` 138 | 139 | To visualize: 140 | ``` 141 | visual_tools_->publishAnimatedGrasps(possible_grasps, grasp_data_.ee_parent_link_); 142 | ``` 143 | 144 | ## Grasp Filter Usage 145 | 146 | This component creates several threads and tests a large number of potential grasps for kinematic feasibility. 147 | 148 | 149 | 150 | To filter grasps after generating them: 151 | ``` 152 | // Filter the grasp for only the ones that are reachable 153 | bool filter_pregrasps = true; 154 | std::vector ik_solutions; // save each grasps ik solution for visualization 155 | grasp_filter_->filterGrasps(possible_grasps, ik_solutions, filter_pregrasps, grasp_data_.ee_parent_link_, planning_group_name_); 156 | ``` 157 | 158 | To view the filtered grasps along with the planning group pose: 159 | ``` 160 | visual_tools_->publishIKSolutions(ik_solutions, planning_group_name_, 0.25); 161 | ``` 162 | 163 | There is more that is undocumented but I'm tired of writing this. 164 | 165 | ## Tested Robots 166 | 167 | - [Baxter](https://github.com/davetcoleman/baxter_cpp) 168 | - [REEM](http://wiki.ros.org/Robots/REEM) 169 | 170 | ## Example Code 171 | 172 | A new (still in development) example tool is ``moveit_blocks.h`` located in the ``include`` folder. It gives you a complete pick and place pipeline using this package and MoveIt, and all you need is the appropriate config file and launch file. An example launch file can be found [here](https://github.com/davetcoleman/clam/blob/master/clam_pick_place/launch/pick_place.launch). 173 | 174 | There are currently example implementations: 175 | 176 | - [baxter_pick_place](https://github.com/davetcoleman/baxter_cpp/tree/hydro-devel/baxter_pick_place) 177 | - [reem_tabletop_grasping](https://github.com/pal-robotics/reem_tabletop_grasping) 178 | - [clam_pick_place](https://github.com/davetcoleman/clam/tree/master/clam_pick_place) 179 | 180 | ## Testing 181 | 182 | There are two tests scripts in this package. To view the tests, first start Rviz with: 183 | 184 | ``` 185 | roslaunch moveit_simple_grasps grasp_test_rviz.launch 186 | ``` 187 | 188 | To test just grasp generation for randomly placed blocks: 189 | ``` 190 | roslaunch moveit_simple_grasps grasp_test.launch 191 | ``` 192 | 193 | To also test the IK grasp filtering: 194 | ``` 195 | roslaunch moveit_simple_grasps grasp_filter_test.launch 196 | ``` 197 | 198 | ## TODO 199 | 200 | Features we'd like to see added to this project: 201 | 202 | - Ability to reason about any shape, not just centroid of a bounding box 203 | - Input arbitrary meshes 204 | - Auto create a bounding box around that mesh 205 | - Better reasoning about support surfaces (table) 206 | - Integrate collision checking to verify feasibility of grasp 207 | - Support non-parallel gripper end effectors 208 | - Make grasp quality metric better informed 209 | - Make this project easier to setup for new robots 210 | - Integrate into Setup Assistant GUI 211 | - Improve simple pick and place pipline header file 212 | 213 | ## Contributors 214 | 215 | - Dave Coleman, CU Boulder @davetcoleman 216 | - Bence Magyar, PAL Robotics @bmagyar 217 | -------------------------------------------------------------------------------- /src/simple_grasps_test.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Tests the grasp generator 37 | */ 38 | 39 | // ROS 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | // Grasp generation 46 | #include 47 | 48 | // Baxter specific properties 49 | #include 50 | 51 | namespace baxter_pick_place 52 | { 53 | 54 | static const double BLOCK_SIZE = 0.04; 55 | 56 | class GraspGeneratorTest 57 | { 58 | private: 59 | // A shared node handle 60 | ros::NodeHandle nh_; 61 | 62 | // Grasp generator 63 | moveit_simple_grasps::SimpleGraspsPtr simple_grasps_; 64 | 65 | // class for publishing stuff to rviz 66 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; 67 | 68 | // robot-specific data for generating grasps 69 | moveit_simple_grasps::GraspData grasp_data_; 70 | 71 | // which baxter arm are we using 72 | std::string arm_; 73 | std::string ee_group_name_; 74 | std::string planning_group_name_; 75 | 76 | public: 77 | 78 | // Constructor 79 | GraspGeneratorTest(int num_tests) 80 | : nh_("~") 81 | { 82 | nh_.param("arm", arm_, std::string("left")); 83 | nh_.param("ee_group_name", ee_group_name_, std::string(arm_ + "_hand")); 84 | planning_group_name_ = arm_ + "_arm"; 85 | 86 | ROS_INFO_STREAM_NAMED("test","Arm: " << arm_); 87 | ROS_INFO_STREAM_NAMED("test","End Effector: " << ee_group_name_); 88 | ROS_INFO_STREAM_NAMED("test","Planning Group: " << planning_group_name_); 89 | 90 | // --------------------------------------------------------------------------------------------- 91 | // Load grasp data specific to our robot 92 | if (!grasp_data_.loadRobotGraspData(nh_, ee_group_name_)) 93 | ros::shutdown(); 94 | 95 | // --------------------------------------------------------------------------------------------- 96 | // Load the Robot Viz Tools for publishing to Rviz 97 | visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools(grasp_data_.base_link_)); 98 | visual_tools_->setLifetime(120.0); 99 | visual_tools_->loadMarkerPub(); 100 | 101 | // --------------------------------------------------------------------------------------------- 102 | // Load grasp generator 103 | simple_grasps_.reset( new moveit_simple_grasps::SimpleGrasps(visual_tools_) ); 104 | 105 | geometry_msgs::Pose pose; 106 | visual_tools_->generateEmptyPose(pose); 107 | 108 | // --------------------------------------------------------------------------------------------- 109 | // Animate open and closing end effector 110 | 111 | for (std::size_t i = 0; i < 4; ++i) 112 | { 113 | // Test visualization of end effector in OPEN position 114 | grasp_data_.setRobotStatePreGrasp( visual_tools_->getSharedRobotState() ); 115 | const robot_model::JointModelGroup* ee_jmg = visual_tools_->getRobotModel()->getJointModelGroup(grasp_data_.ee_group_); 116 | visual_tools_->loadEEMarker(ee_jmg); 117 | visual_tools_->publishEEMarkers(pose, ee_jmg, rviz_visual_tools::ORANGE, "test_eef"); 118 | ros::Duration(1.0).sleep(); 119 | 120 | // Test visualization of end effector in CLOSED position 121 | grasp_data_.setRobotStateGrasp( visual_tools_->getSharedRobotState() ); 122 | visual_tools_->loadEEMarker(ee_jmg); 123 | visual_tools_->publishEEMarkers(pose, ee_jmg, rviz_visual_tools::GREEN, "test_eef"); 124 | ros::Duration(1.0).sleep(); 125 | } 126 | 127 | // --------------------------------------------------------------------------------------------- 128 | // Generate grasps for a bunch of random objects 129 | geometry_msgs::Pose object_pose; 130 | std::vector possible_grasps; 131 | 132 | // Loop 133 | int i = 0; 134 | while(ros::ok()) 135 | { 136 | ROS_INFO_STREAM_NAMED("test","Adding random object " << i+1 << " of " << num_tests); 137 | 138 | // Remove randomness when we are only running one test 139 | if (num_tests == 1) 140 | generateTestObject(object_pose); 141 | else 142 | generateRandomObject(object_pose); 143 | 144 | // Show the block 145 | visual_tools_->publishCuboid(object_pose, BLOCK_SIZE, BLOCK_SIZE, BLOCK_SIZE, rviz_visual_tools::BLUE); 146 | 147 | possible_grasps.clear(); 148 | 149 | // Generate set of grasps for one object 150 | simple_grasps_->generateBlockGrasps(object_pose, grasp_data_, possible_grasps); 151 | 152 | // Visualize them 153 | const robot_model::JointModelGroup* ee_jmg = visual_tools_->getRobotModel()->getJointModelGroup(grasp_data_.ee_group_); 154 | visual_tools_->publishAnimatedGrasps(possible_grasps, ee_jmg); 155 | //visual_tools_->publishGrasps(possible_grasps, grasp_data_.ee_parent_link_); 156 | 157 | // Test if done 158 | ++i; 159 | if( i >= num_tests ) 160 | break; 161 | } 162 | } 163 | 164 | void generateTestObject(geometry_msgs::Pose& object_pose) 165 | { 166 | // Position 167 | geometry_msgs::Pose start_object_pose; 168 | 169 | start_object_pose.position.x = 0.4; 170 | start_object_pose.position.y = -0.2; 171 | start_object_pose.position.z = 0.0; 172 | 173 | // Orientation 174 | double angle = M_PI / 1.5; 175 | Eigen::Quaterniond quat(Eigen::AngleAxis(double(angle), Eigen::Vector3d::UnitZ())); 176 | start_object_pose.orientation.x = quat.x(); 177 | start_object_pose.orientation.y = quat.y(); 178 | start_object_pose.orientation.z = quat.z(); 179 | start_object_pose.orientation.w = quat.w(); 180 | 181 | // Choose which object to test 182 | object_pose = start_object_pose; 183 | 184 | //visual_tools_->publishObject( object_pose, OBJECT_SIZE, true ); 185 | } 186 | 187 | void generateRandomObject(geometry_msgs::Pose& object_pose) 188 | { 189 | // Position 190 | object_pose.position.x = fRand(0.1,0.9); //0.55); 191 | object_pose.position.y = fRand(-0.28,0.28); 192 | object_pose.position.z = 0.02; 193 | 194 | // Orientation 195 | double angle = M_PI * fRand(0.1,1); 196 | Eigen::Quaterniond quat(Eigen::AngleAxis(double(angle), Eigen::Vector3d::UnitZ())); 197 | object_pose.orientation.x = quat.x(); 198 | object_pose.orientation.y = quat.y(); 199 | object_pose.orientation.z = quat.z(); 200 | object_pose.orientation.w = quat.w(); 201 | } 202 | 203 | double fRand(double fMin, double fMax) 204 | { 205 | double f = (double)rand() / RAND_MAX; 206 | return fMin + f * (fMax - fMin); 207 | } 208 | 209 | }; // end of class 210 | 211 | } // namespace 212 | 213 | 214 | int main(int argc, char *argv[]) 215 | { 216 | int num_tests = 10; 217 | ros::init(argc, argv, "grasp_generator_test"); 218 | 219 | ROS_INFO_STREAM_NAMED("main","Simple Grasps Test"); 220 | 221 | ros::AsyncSpinner spinner(2); 222 | spinner.start(); 223 | 224 | // Seed random 225 | srand(ros::Time::now().toSec()); 226 | 227 | // Benchmark time 228 | ros::Time start_time; 229 | start_time = ros::Time::now(); 230 | 231 | // Run Tests 232 | baxter_pick_place::GraspGeneratorTest tester(num_tests); 233 | 234 | // Benchmark time 235 | double duration = (ros::Time::now() - start_time).toNSec() * 1e-6; 236 | ROS_INFO_STREAM_NAMED("","Total time: " << duration); 237 | //std::cout << duration << "\t" << num_tests << std::endl; 238 | 239 | ros::Duration(1.0).sleep(); // let rviz markers finish publishing 240 | 241 | return 0; 242 | } 243 | -------------------------------------------------------------------------------- /src/grasp_filter_test.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Tests the grasp generator filter 37 | */ 38 | 39 | // ROS 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | // MoveIt 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | // Rviz 56 | #include 57 | #include 58 | 59 | // Grasp 60 | #include 61 | #include 62 | #include 63 | 64 | // Baxter specific properties 65 | #include 66 | #include 67 | 68 | namespace moveit_simple_grasps 69 | { 70 | 71 | // Table dimensions 72 | static const double TABLE_HEIGHT = .92; 73 | static const double TABLE_WIDTH = .85; 74 | static const double TABLE_DEPTH = .47; 75 | static const double TABLE_X = 0.66; 76 | static const double TABLE_Y = 0; 77 | static const double TABLE_Z = -0.9/2+0.01; 78 | 79 | static const double BLOCK_SIZE = 0.04; 80 | 81 | class GraspGeneratorTest 82 | { 83 | private: 84 | // A shared node handle 85 | ros::NodeHandle nh_; 86 | 87 | // Grasp generator 88 | moveit_simple_grasps::SimpleGraspsPtr simple_grasps_; 89 | 90 | // Tool for visualizing things in Rviz 91 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; 92 | 93 | // Grasp filter 94 | moveit_simple_grasps::GraspFilterPtr grasp_filter_; 95 | 96 | // data for generating grasps 97 | moveit_simple_grasps::GraspData grasp_data_; 98 | 99 | // Shared planning scene (load once for everything) 100 | planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; 101 | 102 | // which baxter arm are we using 103 | std::string arm_; 104 | std::string ee_group_name_; 105 | std::string planning_group_name_; 106 | 107 | public: 108 | 109 | // Constructor 110 | GraspGeneratorTest(int num_tests) 111 | : nh_("~") 112 | { 113 | // Get arm info from param server 114 | nh_.param("arm", arm_, std::string("left")); 115 | nh_.param("ee_group_name", ee_group_name_, std::string(arm_ + "_hand")); 116 | planning_group_name_ = arm_ + "_arm"; 117 | 118 | ROS_INFO_STREAM_NAMED("test","Arm: " << arm_); 119 | ROS_INFO_STREAM_NAMED("test","End Effector: " << ee_group_name_); 120 | ROS_INFO_STREAM_NAMED("test","Planning Group: " << planning_group_name_); 121 | 122 | // --------------------------------------------------------------------------------------------- 123 | // Load grasp data 124 | if (!grasp_data_.loadRobotGraspData(nh_, ee_group_name_)) 125 | ros::shutdown(); 126 | 127 | // --------------------------------------------------------------------------------------------- 128 | // Load planning scene to share 129 | planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description")); 130 | 131 | // --------------------------------------------------------------------------------------------- 132 | // Load the Robot Viz Tools for publishing to Rviz 133 | const robot_model::JointModelGroup* ee_jmg = planning_scene_monitor_->getRobotModel()->getJointModelGroup(grasp_data_.ee_group_); 134 | 135 | visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools(grasp_data_.base_link_, "/end_effector_marker", planning_scene_monitor_)); 136 | visual_tools_->setLifetime(40.0); 137 | visual_tools_->loadEEMarker(ee_jmg); 138 | visual_tools_->setFloorToBaseHeight(-0.9); 139 | 140 | // Clear out old collision objects just because 141 | //visual_tools_->removeAllCollisionObjects(); 142 | 143 | // Create a collision table for fun 144 | visual_tools_->publishCollisionTable(TABLE_X, TABLE_Y, 0, 0, TABLE_WIDTH, TABLE_HEIGHT, TABLE_DEPTH, "table"); 145 | 146 | // --------------------------------------------------------------------------------------------- 147 | // Load grasp generator 148 | simple_grasps_.reset( new moveit_simple_grasps::SimpleGrasps(visual_tools_) ); 149 | 150 | // --------------------------------------------------------------------------------------------- 151 | // Load grasp filter 152 | robot_state::RobotState robot_state = planning_scene_monitor_->getPlanningScene()->getCurrentState(); 153 | grasp_filter_.reset(new moveit_simple_grasps::GraspFilter(robot_state, visual_tools_) ); 154 | 155 | // --------------------------------------------------------------------------------------------- 156 | // Generate grasps for a bunch of random objects 157 | geometry_msgs::Pose object_pose; 158 | std::vector possible_grasps; 159 | std::vector ik_solutions; // save each grasps ik solution for visualization 160 | 161 | // Loop 162 | for (int i = 0; i < num_tests; ++i) 163 | { 164 | ROS_INFO_STREAM_NAMED("test","Adding random object " << i+1 << " of " << num_tests); 165 | 166 | // Remove randomness when we are only running one test 167 | if (num_tests == 1) 168 | generateTestObject(object_pose); 169 | else 170 | generateRandomObject(object_pose); 171 | 172 | // Show the block 173 | visual_tools_->publishCuboid(object_pose, BLOCK_SIZE, BLOCK_SIZE, BLOCK_SIZE, rviz_visual_tools::BLUE); 174 | 175 | possible_grasps.clear(); 176 | ik_solutions.clear(); 177 | 178 | // Generate set of grasps for one object 179 | simple_grasps_->generateBlockGrasps( object_pose, grasp_data_, possible_grasps); 180 | 181 | // Filter the grasp for only the ones that are reachable 182 | bool filter_pregrasps = true; 183 | grasp_filter_->filterGrasps(possible_grasps, ik_solutions, filter_pregrasps, grasp_data_.ee_parent_link_, planning_group_name_); 184 | 185 | // Visualize them 186 | visual_tools_->publishAnimatedGrasps(possible_grasps, ee_jmg); 187 | const robot_model::JointModelGroup* arm_jmg = planning_scene_monitor_->getRobotModel()->getJointModelGroup(planning_group_name_); 188 | visual_tools_->publishIKSolutions(ik_solutions, arm_jmg, 0.25); 189 | 190 | // Make sure ros is still going 191 | if(!ros::ok()) 192 | break; 193 | } 194 | 195 | 196 | } 197 | 198 | void generateTestObject(geometry_msgs::Pose& object_pose) 199 | { 200 | // Position 201 | geometry_msgs::Pose start_object_pose; 202 | geometry_msgs::Pose end_object_pose; 203 | 204 | start_object_pose.position.x = 0.8; 205 | start_object_pose.position.y = -0.5; 206 | start_object_pose.position.z = 0.02; 207 | 208 | end_object_pose.position.x = 0.25; 209 | end_object_pose.position.y = 0.15; 210 | end_object_pose.position.z = 0.02; 211 | 212 | // Orientation 213 | double angle = M_PI / 1.5; 214 | Eigen::Quaterniond quat(Eigen::AngleAxis(double(angle), Eigen::Vector3d::UnitZ())); 215 | start_object_pose.orientation.x = quat.x(); 216 | start_object_pose.orientation.y = quat.y(); 217 | start_object_pose.orientation.z = quat.z(); 218 | start_object_pose.orientation.w = quat.w(); 219 | 220 | angle = M_PI / 1.1; 221 | quat = Eigen::Quaterniond(Eigen::AngleAxis(double(angle), Eigen::Vector3d::UnitZ())); 222 | end_object_pose.orientation.x = quat.x(); 223 | end_object_pose.orientation.y = quat.y(); 224 | end_object_pose.orientation.z = quat.z(); 225 | end_object_pose.orientation.w = quat.w(); 226 | 227 | // Choose which object to test 228 | object_pose = start_object_pose; 229 | } 230 | 231 | void generateRandomObject(geometry_msgs::Pose& object_pose) 232 | { 233 | // Position 234 | object_pose.position.x = visual_tools_->dRand(0.7,TABLE_DEPTH); 235 | object_pose.position.y = visual_tools_->dRand(-TABLE_WIDTH/2,-0.1); 236 | object_pose.position.z = TABLE_Z + TABLE_HEIGHT / 2.0 + BLOCK_SIZE / 2.0; 237 | 238 | // Orientation 239 | double angle = M_PI * visual_tools_->dRand(0.1,1); 240 | Eigen::Quaterniond quat(Eigen::AngleAxis(double(angle), Eigen::Vector3d::UnitZ())); 241 | object_pose.orientation.x = quat.x(); 242 | object_pose.orientation.y = quat.y(); 243 | object_pose.orientation.z = quat.z(); 244 | object_pose.orientation.w = quat.w(); 245 | } 246 | 247 | }; // end of class 248 | 249 | } // namespace 250 | 251 | 252 | int main(int argc, char *argv[]) 253 | { 254 | int num_tests = 5; 255 | 256 | ros::init(argc, argv, "grasp_generator_test"); 257 | 258 | // Allow the action server to recieve and send ros messages 259 | ros::AsyncSpinner spinner(2); 260 | spinner.start(); 261 | 262 | // Check for verbose flag 263 | bool verbose = false; 264 | if (argc > 1) 265 | { 266 | for (std::size_t i = 0; i < argc; ++i) 267 | { 268 | if (strcmp(argv[i], "--verbose") == 0) 269 | { 270 | ROS_INFO_STREAM_NAMED("main","Running in VERBOSE mode (much slower)"); 271 | verbose = true; 272 | } 273 | } 274 | } 275 | 276 | // Seed random 277 | srand(ros::Time::now().toSec()); 278 | 279 | // Benchmark time 280 | ros::Time start_time; 281 | start_time = ros::Time::now(); 282 | 283 | // Run Tests 284 | moveit_simple_grasps::GraspGeneratorTest tester(num_tests); 285 | 286 | // Benchmark time 287 | double duration = (ros::Time::now() - start_time).toNSec() * 1e-6; 288 | ROS_INFO_STREAM_NAMED("","Total time: " << duration); 289 | std::cout << duration << "\t" << num_tests << std::endl; 290 | 291 | ros::Duration(1.0).sleep(); // let rviz markers finish publishing 292 | 293 | return 0; 294 | } 295 | -------------------------------------------------------------------------------- /src/grasp_filter.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | #include 37 | 38 | // Conversions 39 | #include // add to pkg TODO 40 | 41 | namespace moveit_simple_grasps 42 | { 43 | 44 | // Constructor 45 | GraspFilter::GraspFilter( robot_state::RobotState robot_state, 46 | moveit_visual_tools::MoveItVisualToolsPtr& visual_tools ): 47 | robot_state_(robot_state), 48 | visual_tools_(visual_tools), 49 | verbose_(false) 50 | { 51 | ROS_DEBUG_STREAM_NAMED("filter","Loaded simple grasp filter"); 52 | } 53 | 54 | GraspFilter::~GraspFilter() 55 | { 56 | } 57 | 58 | bool GraspFilter::chooseBestGrasp( const std::vector& possible_grasps, moveit_msgs::Grasp& chosen ) 59 | { 60 | // TODO: better logic here 61 | if( possible_grasps.empty() ) 62 | { 63 | ROS_ERROR_NAMED("filter","There are no grasps to choose from"); 64 | return false; 65 | } 66 | chosen = possible_grasps[0]; // just choose first one 67 | return true; 68 | } 69 | 70 | // Return grasps that are kinematically feasible 71 | bool GraspFilter::filterGrasps(std::vector& possible_grasps, 72 | std::vector& ik_solutions, bool filter_pregrasp, 73 | const std::string &ee_parent_link, const std::string& planning_group) 74 | { 75 | // ----------------------------------------------------------------------------------------------- 76 | // Error check 77 | if( possible_grasps.empty() ) 78 | { 79 | ROS_ERROR_NAMED("filter","Unable to filter grasps because vector is empty"); 80 | return false; 81 | } 82 | 83 | // ----------------------------------------------------------------------------------------------- 84 | // how many cores does this computer have and how many do we need? 85 | int num_threads = boost::thread::hardware_concurrency(); 86 | if( num_threads > possible_grasps.size() ) 87 | num_threads = possible_grasps.size(); 88 | 89 | if(false) 90 | { 91 | num_threads = 1; 92 | ROS_WARN_STREAM_NAMED("grasp_filter","Using only " << num_threads << " threads"); 93 | } 94 | 95 | // ----------------------------------------------------------------------------------------------- 96 | // Get the solver timeout from kinematics.yaml 97 | double timeout = robot_state_.getRobotModel()->getJointModelGroup( planning_group )->getDefaultIKTimeout(); 98 | timeout = 0.05; 99 | ROS_DEBUG_STREAM_NAMED("grasp_filter","Grasp filter IK timeout " << timeout); 100 | 101 | // ----------------------------------------------------------------------------------------------- 102 | // Load kinematic solvers if not already loaded 103 | if( kin_solvers_[planning_group].size() != num_threads ) 104 | { 105 | kin_solvers_[planning_group].clear(); 106 | 107 | const robot_model::JointModelGroup* jmg = robot_state_.getRobotModel()->getJointModelGroup(planning_group); 108 | 109 | // Create an ik solver for every thread 110 | for (int i = 0; i < num_threads; ++i) 111 | { 112 | //ROS_INFO_STREAM_NAMED("filter","Creating ik solver " << i); 113 | 114 | kin_solvers_[planning_group].push_back(jmg->getSolverInstance()); 115 | 116 | // Test to make sure we have a valid kinematics solver 117 | if( !kin_solvers_[planning_group][i] ) 118 | { 119 | ROS_ERROR_STREAM_NAMED("grasp_filter","No kinematic solver found"); 120 | return false; 121 | } 122 | } 123 | } 124 | 125 | // Transform poses ------------------------------------------------------------------------------- 126 | // bring the pose to the frame of the IK solver 127 | const std::string &ik_frame = kin_solvers_[planning_group][0]->getBaseFrame(); 128 | Eigen::Affine3d link_transform; 129 | if (!moveit::core::Transforms::sameFrame(ik_frame, robot_state_.getRobotModel()->getModelFrame())) 130 | { 131 | const robot_model::LinkModel *lm = robot_state_.getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame); 132 | if (!lm) 133 | return false; 134 | //pose = getGlobalLinkTransform(lm).inverse() * pose; 135 | link_transform = robot_state_.getGlobalLinkTransform(lm).inverse(); 136 | } 137 | 138 | // Benchmark time 139 | ros::Time start_time; 140 | start_time = ros::Time::now(); 141 | 142 | // ----------------------------------------------------------------------------------------------- 143 | // Loop through poses and find those that are kinematically feasible 144 | std::vector filtered_grasps; 145 | 146 | boost::thread_group bgroup; // create a group of threads 147 | boost::mutex lock; // used for sharing the same data structures 148 | 149 | ROS_INFO_STREAM_NAMED("filter", "Filtering possible grasps with " << num_threads << " threads"); 150 | 151 | // split up the work between threads 152 | double num_grasps_per_thread = double(possible_grasps.size()) / num_threads; 153 | //ROS_INFO_STREAM("total grasps " << possible_grasps.size() << " per thead: " << num_grasps_per_thread); 154 | 155 | int grasps_id_start; 156 | int grasps_id_end = 0; 157 | 158 | for(int i = 0; i < num_threads; ++i) 159 | { 160 | grasps_id_start = grasps_id_end; 161 | grasps_id_end = ceil(num_grasps_per_thread*(i+1)); 162 | if( grasps_id_end >= possible_grasps.size() ) 163 | grasps_id_end = possible_grasps.size(); 164 | //ROS_INFO_STREAM_NAMED("filter","low " << grasps_id_start << " high " << grasps_id_end); 165 | 166 | IkThreadStruct tc(possible_grasps, filtered_grasps, ik_solutions, link_transform, grasps_id_start, 167 | grasps_id_end, kin_solvers_[planning_group][i], filter_pregrasp, ee_parent_link, timeout, &lock, i); 168 | bgroup.create_thread( boost::bind( &GraspFilter::filterGraspThread, this, tc ) ); 169 | } 170 | 171 | ROS_DEBUG_STREAM_NAMED("filter","Waiting to join " << num_threads << " ik threads..."); 172 | bgroup.join_all(); // wait for all threads to finish 173 | 174 | ROS_INFO_STREAM_NAMED("filter", "Grasp filter complete, found " << filtered_grasps.size() << " IK solutions out of " << 175 | possible_grasps.size() ); 176 | 177 | possible_grasps = filtered_grasps; 178 | 179 | if (verbose_) 180 | { 181 | // End Benchmark time 182 | double duration = (ros::Time::now() - start_time).toNSec() * 1e-6; 183 | ROS_INFO_STREAM_NAMED("filter","Grasp generator IK grasp filtering benchmark time:"); 184 | std::cout << duration << "\t" << possible_grasps.size() << "\n"; 185 | 186 | ROS_INFO_STREAM_NAMED("filter","Possible grasps filtered to " << possible_grasps.size() << " options."); 187 | } 188 | 189 | return true; 190 | } 191 | 192 | void GraspFilter::filterGraspThread(IkThreadStruct ik_thread_struct) 193 | { 194 | // Seed state - start at zero 195 | std::vector ik_seed_state(7); // fill with zeros 196 | // TODO do not assume 7 dof 197 | 198 | std::vector solution; 199 | moveit_msgs::MoveItErrorCodes error_code; 200 | geometry_msgs::PoseStamped ik_pose; 201 | 202 | // Process the assigned grasps 203 | for( int i = ik_thread_struct.grasps_id_start_; i < ik_thread_struct.grasps_id_end_; ++i ) 204 | { 205 | //ROS_DEBUG_STREAM_NAMED("filter", "Checking grasp #" << i); 206 | 207 | // Clear out previous solution just in case - not sure if this is needed 208 | solution.clear(); 209 | 210 | // Transform current pose to frame of planning group 211 | ik_pose = ik_thread_struct.possible_grasps_[i].grasp_pose; 212 | Eigen::Affine3d eigen_pose; 213 | tf::poseMsgToEigen(ik_pose.pose, eigen_pose); 214 | eigen_pose = ik_thread_struct.link_transform_ * eigen_pose; 215 | tf::poseEigenToMsg(eigen_pose, ik_pose.pose); 216 | 217 | // Test it with IK 218 | ik_thread_struct.kin_solver_-> 219 | searchPositionIK(ik_pose.pose, ik_seed_state, ik_thread_struct.timeout_, solution, error_code); 220 | 221 | // Results 222 | if( error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS ) 223 | { 224 | //ROS_INFO_STREAM_NAMED("filter","Found IK Solution"); 225 | 226 | // Copy solution to seed state so that next solution is faster 227 | ik_seed_state = solution; 228 | 229 | // Start pre-grasp section ---------------------------------------------------------- 230 | if (ik_thread_struct.filter_pregrasp_) // optionally check the pregrasp 231 | { 232 | // Convert to a pre-grasp 233 | ik_pose = SimpleGrasps::getPreGraspPose(ik_thread_struct.possible_grasps_[i], ik_thread_struct.ee_parent_link_); 234 | 235 | // Transform current pose to frame of planning group 236 | Eigen::Affine3d eigen_pose; 237 | tf::poseMsgToEigen(ik_pose.pose, eigen_pose); 238 | eigen_pose = ik_thread_struct.link_transform_ * eigen_pose; 239 | tf::poseEigenToMsg(eigen_pose, ik_pose.pose); 240 | 241 | // Test it with IK 242 | ik_thread_struct.kin_solver_-> 243 | searchPositionIK(ik_pose.pose, ik_seed_state, ik_thread_struct.timeout_, solution, error_code); 244 | 245 | // Results 246 | if( error_code.val == moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION ) 247 | { 248 | ROS_WARN_STREAM_NAMED("filter","Unable to find IK solution for pre-grasp pose."); 249 | continue; 250 | } 251 | else if( error_code.val == moveit_msgs::MoveItErrorCodes::TIMED_OUT ) 252 | { 253 | //ROS_DEBUG_STREAM_NAMED("filter","Unable to find IK solution for pre-grasp pose: Timed Out."); 254 | continue; 255 | } 256 | else if( error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS ) 257 | { 258 | ROS_INFO_STREAM_NAMED("filter","IK solution error for pre-grasp: MoveItErrorCodes.msg = " << error_code); 259 | continue; 260 | } 261 | } 262 | // Both grasp and pre-grasp have passed 263 | // Lock the result vector so we can add to it for a second 264 | { 265 | boost::mutex::scoped_lock slock(*ik_thread_struct.lock_); 266 | ik_thread_struct.filtered_grasps_.push_back( ik_thread_struct.possible_grasps_[i] ); 267 | 268 | trajectory_msgs::JointTrajectoryPoint point; 269 | //point.positions = ik_seed_state; // show the grasp solution 270 | point.positions = solution; // show the pre-grasp solution 271 | 272 | // Copy solution so that we can optionally use it later 273 | ik_thread_struct.ik_solutions_.push_back(point); 274 | } 275 | 276 | // End pre-grasp section ------------------------------------------------------- 277 | } 278 | else if( error_code.val == moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION ) 279 | ROS_WARN_STREAM_NAMED("filter","Unable to find IK solution for pose."); 280 | else if( error_code.val == moveit_msgs::MoveItErrorCodes::TIMED_OUT ) 281 | { 282 | //ROS_DEBUG_STREAM_NAMED("filter","Unable to find IK solution for pose: Timed Out."); 283 | } 284 | else 285 | ROS_INFO_STREAM_NAMED("filter","IK solution error: MoveItErrorCodes.msg = " << error_code); 286 | } 287 | 288 | //ROS_DEBUG_STREAM_NAMED("filter","Thread " << ik_thread_struct.thread_id_ << " finished"); 289 | } 290 | 291 | } // namespace 292 | -------------------------------------------------------------------------------- /src/grasp_data.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Modified BSD License) 3 | * 4 | * Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Univ of CO, Boulder, PAL Robotics, S.L. 18 | * nor the names of its contributors may be used to endorse or 19 | * promote products derived from this software without specific 20 | * prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | /* Authors: Bence Magyar, Dave Coleman 37 | Description: Data class used by the grasp generator. 38 | */ 39 | 40 | #include 41 | 42 | // Eigen 43 | #include 44 | #include 45 | #include 46 | 47 | // C++ 48 | #include 49 | #define _USE_MATH_DEFINES 50 | 51 | namespace moveit_simple_grasps 52 | { 53 | GraspData::GraspData() : 54 | // Fill in default values where possible: 55 | base_link_("/base_link"), 56 | grasp_depth_(0.12), 57 | angle_resolution_(16), 58 | approach_retreat_desired_dist_(0.6), 59 | approach_retreat_min_dist_(0.4), 60 | object_size_(0.04) 61 | {} 62 | 63 | bool GraspData::loadRobotGraspData(const ros::NodeHandle& nh, const std::string& end_effector) 64 | { 65 | std::vector joint_names; 66 | std::vector pre_grasp_posture; // todo: remove all underscore post-fixes 67 | std::vector grasp_posture; 68 | std::vector grasp_pose_to_eef; 69 | std::vector grasp_pose_to_eef_rotation; 70 | double pregrasp_time_from_start; 71 | double grasp_time_from_start; 72 | std::string end_effector_name; 73 | std::string end_effector_parent_link; 74 | 75 | // Load a param 76 | if (!nh.hasParam("base_link")) 77 | { 78 | ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `base_link` missing from rosparam server. Did you load your end effector's configuration yaml file? Searching in namespace: " << nh.getNamespace()); 79 | return false; 80 | } 81 | nh.getParam("base_link", base_link_); 82 | 83 | // Search within the sub-namespace of this end effector name 84 | ros::NodeHandle child_nh(nh, end_effector); 85 | 86 | // Load a param 87 | if (!child_nh.hasParam("pregrasp_time_from_start")) 88 | { 89 | ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `pregrasp_time_from_start` missing from rosparam server. Did you load your end effector's configuration yaml file? Searching in namespace: " << child_nh.getNamespace()); 90 | return false; 91 | } 92 | child_nh.getParam("pregrasp_time_from_start", pregrasp_time_from_start); 93 | 94 | // Load a param 95 | if (!child_nh.hasParam("grasp_time_from_start")) 96 | { 97 | ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `grasp_time_from_start` missing from rosparam server. Did you load your end effector's configuration yaml file?"); 98 | return false; 99 | } 100 | child_nh.getParam("grasp_time_from_start", grasp_time_from_start); 101 | 102 | // Load a param 103 | if (!child_nh.hasParam("end_effector_name")) 104 | { 105 | ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `end_effector_name` missing from rosparam server. Did you load your end effector's configuration yaml file?"); 106 | return false; 107 | } 108 | child_nh.getParam("end_effector_name", end_effector_name); 109 | 110 | // Load a param 111 | if (!child_nh.hasParam("end_effector_parent_link")) 112 | { 113 | ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `end_effector_parent_link` missing from rosparam server. Did you load your end effector's configuration yaml file?"); 114 | return false; 115 | } 116 | child_nh.getParam("end_effector_parent_link", end_effector_parent_link); 117 | 118 | // Load a param 119 | if (!child_nh.hasParam("joints")) 120 | { 121 | ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `joints` missing from rosparam server. Did you load your end effector's configuration yaml file?"); 122 | return false; 123 | } 124 | XmlRpc::XmlRpcValue joint_list; 125 | child_nh.getParam("joints", joint_list); 126 | if (joint_list.getType() == XmlRpc::XmlRpcValue::TypeArray) 127 | for (int32_t i = 0; i < joint_list.size(); ++i) 128 | { 129 | ROS_ASSERT(joint_list[i].getType() == XmlRpc::XmlRpcValue::TypeString); 130 | joint_names.push_back(static_cast(joint_list[i])); 131 | } 132 | else 133 | ROS_ERROR_STREAM_NAMED("temp","joint list type is not type array???"); 134 | 135 | if(child_nh.hasParam("pregrasp_posture")) 136 | { 137 | XmlRpc::XmlRpcValue preg_posture_list; 138 | child_nh.getParam("pregrasp_posture", preg_posture_list); 139 | ROS_ASSERT(preg_posture_list.getType() == XmlRpc::XmlRpcValue::TypeArray); 140 | for (int32_t i = 0; i < preg_posture_list.size(); ++i) 141 | { 142 | ROS_ASSERT(preg_posture_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); 143 | pre_grasp_posture.push_back(static_cast(preg_posture_list[i])); 144 | } 145 | } 146 | 147 | ROS_ASSERT(child_nh.hasParam("grasp_posture")); 148 | XmlRpc::XmlRpcValue grasp_posture_list; 149 | child_nh.getParam("grasp_posture", grasp_posture_list); 150 | ROS_ASSERT(grasp_posture_list.getType() == XmlRpc::XmlRpcValue::TypeArray); 151 | for (int32_t i = 0; i < grasp_posture_list.size(); ++i) 152 | { 153 | ROS_ASSERT(grasp_posture_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); 154 | grasp_posture.push_back(static_cast(grasp_posture_list[i])); 155 | } 156 | 157 | ROS_ASSERT(child_nh.hasParam("grasp_pose_to_eef")); 158 | XmlRpc::XmlRpcValue g_to_eef_list; 159 | child_nh.getParam("grasp_pose_to_eef", g_to_eef_list); 160 | ROS_ASSERT(g_to_eef_list.getType() == XmlRpc::XmlRpcValue::TypeArray); 161 | for (int32_t i = 0; i < g_to_eef_list.size(); ++i) 162 | { 163 | // Cast to double OR int 164 | if (g_to_eef_list[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) 165 | { 166 | if (g_to_eef_list[i].getType() != XmlRpc::XmlRpcValue::TypeInt ) 167 | { 168 | ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `grasp_pose_to_eef` wrong data type - int or double required."); 169 | return false; 170 | } 171 | else 172 | grasp_pose_to_eef.push_back(static_cast(g_to_eef_list[i])); 173 | } 174 | else 175 | grasp_pose_to_eef.push_back(static_cast(g_to_eef_list[i])); 176 | } 177 | 178 | ROS_ASSERT(child_nh.hasParam("grasp_pose_to_eef_rotation")); 179 | XmlRpc::XmlRpcValue g_to_eef_rotation_list; 180 | child_nh.getParam("grasp_pose_to_eef_rotation", g_to_eef_rotation_list); 181 | ROS_ASSERT(g_to_eef_rotation_list.getType() == XmlRpc::XmlRpcValue::TypeArray); 182 | for (int32_t i = 0; i < g_to_eef_rotation_list.size(); ++i) 183 | { 184 | // Cast to double OR int 185 | if (g_to_eef_rotation_list[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) 186 | { 187 | if (g_to_eef_rotation_list[i].getType() != XmlRpc::XmlRpcValue::TypeInt ) 188 | { 189 | ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `grasp_pose_to_eef_rotation` wrong data type - int or double required."); 190 | return false; 191 | } 192 | else 193 | grasp_pose_to_eef_rotation.push_back(static_cast(g_to_eef_rotation_list[i])); 194 | } 195 | else 196 | grasp_pose_to_eef_rotation.push_back(static_cast(g_to_eef_rotation_list[i])); 197 | } 198 | 199 | // ------------------------------- 200 | // Convert generic grasp pose to this end effector's frame of reference, approach direction for short 201 | 202 | // Orientation 203 | ROS_ASSERT(grasp_pose_to_eef_rotation.size() == 3); 204 | Eigen::Quaterniond quat(Eigen::AngleAxis(double(grasp_pose_to_eef_rotation[1]), Eigen::Vector3d::UnitY())); // turn on Z axis 205 | // TODO: rotate for roll and yaw also, not just pitch (unit y) 206 | // but i don't need that feature right now and it might be tricky 207 | grasp_pose_to_eef_pose_.orientation.x = quat.x(); 208 | grasp_pose_to_eef_pose_.orientation.y = quat.y(); 209 | grasp_pose_to_eef_pose_.orientation.z = quat.z(); 210 | grasp_pose_to_eef_pose_.orientation.w = quat.w(); 211 | 212 | // Position // approach vector? 213 | ROS_ASSERT(grasp_pose_to_eef.size() == 3); 214 | grasp_pose_to_eef_pose_.position.x = grasp_pose_to_eef[0]; 215 | grasp_pose_to_eef_pose_.position.y = grasp_pose_to_eef[1]; 216 | grasp_pose_to_eef_pose_.position.z = grasp_pose_to_eef[2]; 217 | 218 | // ------------------------------- 219 | // Create pre-grasp posture if specified 220 | if(!pre_grasp_posture.empty()) 221 | { 222 | pre_grasp_posture_.header.frame_id = base_link_; 223 | pre_grasp_posture_.header.stamp = ros::Time::now(); 224 | // Name of joints: 225 | pre_grasp_posture_.joint_names = joint_names; 226 | // Position of joints 227 | pre_grasp_posture_.points.resize(1); 228 | pre_grasp_posture_.points[0].positions = pre_grasp_posture; 229 | pre_grasp_posture_.points[0].time_from_start = ros::Duration(pregrasp_time_from_start); 230 | } 231 | // ------------------------------- 232 | // Create grasp posture 233 | grasp_posture_.header.frame_id = base_link_; 234 | grasp_posture_.header.stamp = ros::Time::now(); 235 | // Name of joints: 236 | grasp_posture_.joint_names = joint_names; 237 | // Position of joints 238 | grasp_posture_.points.resize(1); 239 | grasp_posture_.points[0].positions = grasp_posture; 240 | grasp_posture_.points[0].time_from_start = ros::Duration(grasp_time_from_start); 241 | 242 | // ------------------------------- 243 | // SRDF Info 244 | ee_parent_link_ = end_effector_parent_link; 245 | ee_group_ = end_effector_name; 246 | 247 | // ------------------------------- 248 | // Nums 249 | approach_retreat_desired_dist_ = 0.2; // 0.3; 250 | approach_retreat_min_dist_ = 0.06; 251 | // distance from center point of object to end effector 252 | grasp_depth_ = 0.06;// in negative or 0 this makes the grasps on the other side of the object! (like from below) 253 | 254 | // generate grasps at PI/angle_resolution increments 255 | angle_resolution_ = 16; //TODO parametrize this, or move to action interface 256 | 257 | // Debug 258 | //moveit_simple_grasps::SimpleGrasps::printObjectGraspData(grasp_data); 259 | 260 | return true; 261 | } 262 | 263 | bool GraspData::setRobotStatePreGrasp( robot_state::RobotStatePtr &robot_state ) 264 | { 265 | return setRobotState( robot_state, pre_grasp_posture_ ); 266 | } 267 | bool GraspData::setRobotStateGrasp( robot_state::RobotStatePtr &robot_state ) 268 | { 269 | return setRobotState( robot_state, grasp_posture_ ); 270 | } 271 | 272 | bool GraspData::setRobotState( robot_state::RobotStatePtr &robot_state, const trajectory_msgs::JointTrajectory &posture ) 273 | { 274 | // Do for every joint in end effector 275 | for (std::size_t i = 0; i < posture.joint_names.size(); ++i) 276 | { 277 | // Debug 278 | std::cout << "Setting joint " << posture.joint_names[i] << " to value " 279 | << posture.points[i].positions[0] << std::endl; 280 | 281 | // Set joint position 282 | robot_state->setJointPositions( posture.joint_names[i], 283 | posture.points[i].positions ); 284 | } 285 | } 286 | 287 | void GraspData::print() 288 | { 289 | ROS_WARN_STREAM_NAMED("grasp_data","Debug Grasp Data variable values:"); 290 | std::cout << "grasp_pose_to_eef_pose_: \n" < 36 | 37 | namespace moveit_simple_grasps 38 | { 39 | 40 | // Constructor 41 | SimpleGrasps::SimpleGrasps(moveit_visual_tools::MoveItVisualToolsPtr visual_tools, bool verbose) : 42 | visual_tools_(visual_tools), 43 | verbose_(verbose) 44 | { 45 | ROS_DEBUG_STREAM_NAMED("grasps","Loaded simple grasp generator"); 46 | } 47 | 48 | // Deconstructor 49 | SimpleGrasps::~SimpleGrasps() 50 | { 51 | } 52 | 53 | // Create all possible grasp positions for a object 54 | bool SimpleGrasps::generateBlockGrasps(const geometry_msgs::Pose& object_pose, const GraspData& grasp_data, 55 | std::vector& possible_grasps) 56 | { 57 | // --------------------------------------------------------------------------------------------- 58 | // Calculate grasps in two axis in both directions 59 | generateAxisGrasps( object_pose, X_AXIS, DOWN, HALF, 0, grasp_data, possible_grasps); // got no grasps with this alone 60 | generateAxisGrasps( object_pose, X_AXIS, UP, HALF, 0, grasp_data, possible_grasps); // gives some grasps... looks ugly 61 | generateAxisGrasps( object_pose, Y_AXIS, DOWN, HALF, 0, grasp_data, possible_grasps); // GOOD ONES! 62 | generateAxisGrasps( object_pose, Y_AXIS, UP, HALF, 0, grasp_data, possible_grasps); // gave a grasp from top... bad 63 | 64 | return true; 65 | } 66 | 67 | // Create grasp positions in one axis 68 | bool SimpleGrasps::generateAxisGrasps( 69 | const geometry_msgs::Pose& object_pose, 70 | grasp_axis_t axis, 71 | grasp_direction_t direction, 72 | grasp_rotation_t rotation, 73 | double hand_roll, 74 | const GraspData& grasp_data, 75 | std::vector& possible_grasps) 76 | { 77 | // --------------------------------------------------------------------------------------------- 78 | // Create a transform from the object's frame (center of object) to /base_link 79 | tf::poseMsgToEigen(object_pose, object_global_transform_); 80 | 81 | // --------------------------------------------------------------------------------------------- 82 | // Grasp parameters 83 | 84 | // Create re-usable approach motion 85 | moveit_msgs::GripperTranslation pre_grasp_approach; 86 | pre_grasp_approach.direction.header.stamp = ros::Time::now(); 87 | pre_grasp_approach.desired_distance = grasp_data.approach_retreat_desired_dist_; // The distance the origin of a robot link needs to travel 88 | pre_grasp_approach.min_distance = grasp_data.approach_retreat_min_dist_; // half of the desired? Untested. 89 | 90 | // Create re-usable retreat motion 91 | moveit_msgs::GripperTranslation post_grasp_retreat; 92 | post_grasp_retreat.direction.header.stamp = ros::Time::now(); 93 | post_grasp_retreat.desired_distance = grasp_data.approach_retreat_desired_dist_; // The distance the origin of a robot link needs to travel 94 | post_grasp_retreat.min_distance = grasp_data.approach_retreat_min_dist_; // half of the desired? Untested. 95 | 96 | // Create re-usable blank pose 97 | geometry_msgs::PoseStamped grasp_pose_msg; 98 | grasp_pose_msg.header.stamp = ros::Time::now(); 99 | grasp_pose_msg.header.frame_id = grasp_data.base_link_; 100 | 101 | // --------------------------------------------------------------------------------------------- 102 | // Angle calculations 103 | double radius = grasp_data.grasp_depth_; //0.12 104 | double xb; 105 | double yb = 0.0; // stay in the y plane of the object 106 | double zb; 107 | double theta1 = 0.0; // Where the point is located around the object 108 | double theta2 = 0.0; // UP 'direction' 109 | 110 | // Gripper direction (UP/DOWN) rotation. UP set by default 111 | if( direction == DOWN ) 112 | { 113 | theta2 = M_PI; 114 | } 115 | 116 | // --------------------------------------------------------------------------------------------- 117 | // --------------------------------------------------------------------------------------------- 118 | // Begin Grasp Generator Loop 119 | // --------------------------------------------------------------------------------------------- 120 | // --------------------------------------------------------------------------------------------- 121 | 122 | /* Developer Note: 123 | * Create angles 180 degrees around the chosen axis at given resolution 124 | * We create the grasps in the reference frame of the object, then later convert it to the base link 125 | */ 126 | for(int i = 0; i <= grasp_data.angle_resolution_; ++i) 127 | { 128 | // Create a Grasp message 129 | moveit_msgs::Grasp new_grasp; 130 | 131 | // Calculate grasp pose 132 | xb = radius*cos(theta1); 133 | zb = radius*sin(theta1); 134 | 135 | Eigen::Affine3d grasp_pose; 136 | 137 | switch(axis) 138 | { 139 | case X_AXIS: 140 | grasp_pose = Eigen::AngleAxisd(theta1, Eigen::Vector3d::UnitX()) 141 | * Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitZ()) 142 | * Eigen::AngleAxisd(theta2, Eigen::Vector3d::UnitX()); // Flip 'direction' 143 | 144 | grasp_pose.translation() = Eigen::Vector3d( yb, xb ,zb); 145 | 146 | break; 147 | case Y_AXIS: 148 | grasp_pose = 149 | Eigen::AngleAxisd(M_PI - theta1, Eigen::Vector3d::UnitY()) 150 | *Eigen::AngleAxisd(theta2, Eigen::Vector3d::UnitX()); // Flip 'direction' 151 | 152 | grasp_pose.translation() = Eigen::Vector3d( xb, yb ,zb); 153 | 154 | break; 155 | case Z_AXIS: 156 | grasp_pose = 157 | Eigen::AngleAxisd(M_PI - theta1, Eigen::Vector3d::UnitZ()) 158 | *Eigen::AngleAxisd(theta2, Eigen::Vector3d::UnitX()); // Flip 'direction' 159 | 160 | grasp_pose.translation() = Eigen::Vector3d( xb, yb ,zb); 161 | 162 | break; 163 | } 164 | 165 | /* The estimated probability of success for this grasp, or some other measure of how "good" it is. 166 | * Here we base bias the score based on how far the wrist is from the surface, preferring a greater 167 | * distance to prevent wrist/end effector collision with the table 168 | */ 169 | double score = sin(theta1); 170 | new_grasp.grasp_quality = std::max(score,0.1); // don't allow score to drop below 0.1 b/c all grasps are ok 171 | 172 | // Calculate the theta1 for next time 173 | if (rotation == HALF) 174 | theta1 += M_PI / grasp_data.angle_resolution_; 175 | else 176 | { 177 | theta1 += 2*M_PI / grasp_data.angle_resolution_; 178 | } 179 | 180 | // A name for this grasp 181 | static int grasp_id = 0; 182 | new_grasp.id = "Grasp" + boost::lexical_cast(grasp_id); 183 | ++grasp_id; 184 | 185 | // PreGrasp and Grasp Postures -------------------------------------------------------------------------- 186 | 187 | // The internal posture of the hand for the pre-grasp only positions are used 188 | new_grasp.pre_grasp_posture = grasp_data.pre_grasp_posture_; 189 | 190 | // The internal posture of the hand for the grasp positions and efforts are used 191 | new_grasp.grasp_posture = grasp_data.grasp_posture_; 192 | 193 | // Grasp ------------------------------------------------------------------------------------------------ 194 | 195 | 196 | // DEBUG - show original grasp pose before tranform to gripper frame 197 | if( verbose_ ) 198 | { 199 | tf::poseEigenToMsg(object_global_transform_ * grasp_pose, grasp_pose_msg.pose); 200 | visual_tools_->publishArrow(grasp_pose_msg.pose, rviz_visual_tools::GREEN); 201 | } 202 | 203 | // ------------------------------------------------------------------------ 204 | // Optionally roll wrist with respect to object pose 205 | Eigen::Affine3d roll_gripper; 206 | roll_gripper = Eigen::AngleAxisd(hand_roll, Eigen::Vector3d::UnitX()); 207 | grasp_pose = grasp_pose * roll_gripper; 208 | 209 | // ------------------------------------------------------------------------ 210 | // Change grasp to frame of reference of this custom end effector 211 | 212 | // Convert to Eigen 213 | Eigen::Affine3d eef_conversion_pose; 214 | tf::poseMsgToEigen(grasp_data.grasp_pose_to_eef_pose_, eef_conversion_pose); 215 | 216 | // Transform the grasp pose 217 | grasp_pose = grasp_pose * eef_conversion_pose; 218 | 219 | // ------------------------------------------------------------------------ 220 | // Convert pose to global frame (base_link) 221 | tf::poseEigenToMsg(object_global_transform_ * grasp_pose, grasp_pose_msg.pose); 222 | 223 | // The position of the end-effector for the grasp relative to a reference frame (that is always specified elsewhere, not in this message) 224 | new_grasp.grasp_pose = grasp_pose_msg; 225 | 226 | // Other ------------------------------------------------------------------------------------------------ 227 | 228 | // the maximum contact force to use while grasping (<=0 to disable) 229 | new_grasp.max_contact_force = 0; 230 | 231 | // ------------------------------------------------------------------------------------------------------- 232 | // ------------------------------------------------------------------------------------------------------- 233 | // Approach and retreat 234 | // ------------------------------------------------------------------------------------------------------- 235 | // ------------------------------------------------------------------------------------------------------- 236 | 237 | // Straight down --------------------------------------------------------------------------------------- 238 | // With respect to the base link/world frame 239 | 240 | // Approach 241 | pre_grasp_approach.direction.header.frame_id = grasp_data.base_link_; 242 | pre_grasp_approach.direction.vector.x = 0; 243 | pre_grasp_approach.direction.vector.y = 0; 244 | pre_grasp_approach.direction.vector.z = -1; // Approach direction (negative z axis) // TODO: document this assumption 245 | new_grasp.pre_grasp_approach = pre_grasp_approach; 246 | 247 | // Retreat 248 | post_grasp_retreat.direction.header.frame_id = grasp_data.base_link_; 249 | post_grasp_retreat.direction.vector.x = 0; 250 | post_grasp_retreat.direction.vector.y = 0; 251 | post_grasp_retreat.direction.vector.z = 1; // Retreat direction (pos z axis) 252 | new_grasp.post_grasp_retreat = post_grasp_retreat; 253 | 254 | // Add to vector 255 | possible_grasps.push_back(new_grasp); 256 | 257 | // Angled with pose ------------------------------------------------------------------------------------- 258 | // Approach with respect to end effector orientation 259 | 260 | // Approach 261 | pre_grasp_approach.direction.header.frame_id = grasp_data.ee_parent_link_; 262 | pre_grasp_approach.direction.vector.x = 0; 263 | pre_grasp_approach.direction.vector.y = 0; 264 | pre_grasp_approach.direction.vector.z = 1; 265 | new_grasp.pre_grasp_approach = pre_grasp_approach; 266 | 267 | // Retreat 268 | post_grasp_retreat.direction.header.frame_id = grasp_data.ee_parent_link_; 269 | post_grasp_retreat.direction.vector.x = 0; 270 | post_grasp_retreat.direction.vector.y = 0; 271 | post_grasp_retreat.direction.vector.z = -1; 272 | new_grasp.post_grasp_retreat = post_grasp_retreat; 273 | 274 | // Add to vector 275 | possible_grasps.push_back(new_grasp); 276 | 277 | } 278 | 279 | ROS_INFO_STREAM_NAMED("grasp", "Generated " << possible_grasps.size() << " grasps." ); 280 | 281 | return true; 282 | } 283 | 284 | geometry_msgs::PoseStamped SimpleGrasps::getPreGraspPose(const moveit_msgs::Grasp &grasp, const std::string &ee_parent_link) 285 | { 286 | // Grasp Pose Variables 287 | geometry_msgs::PoseStamped grasp_pose = grasp.grasp_pose; 288 | Eigen::Affine3d grasp_pose_eigen; 289 | tf::poseMsgToEigen(grasp_pose.pose, grasp_pose_eigen); 290 | 291 | // Get pre-grasp pose first 292 | geometry_msgs::PoseStamped pre_grasp_pose; 293 | Eigen::Affine3d pre_grasp_pose_eigen = grasp_pose_eigen; // Copy original grasp pose to pre-grasp pose 294 | 295 | // Approach direction variables 296 | Eigen::Vector3d pre_grasp_approach_direction_local; 297 | 298 | // The direction of the pre-grasp 299 | // Calculate the current animation position based on the percent 300 | Eigen::Vector3d pre_grasp_approach_direction = Eigen::Vector3d( 301 | -1 * grasp.pre_grasp_approach.direction.vector.x * grasp.pre_grasp_approach.desired_distance, 302 | -1 * grasp.pre_grasp_approach.direction.vector.y * grasp.pre_grasp_approach.desired_distance, 303 | -1 * grasp.pre_grasp_approach.direction.vector.z * grasp.pre_grasp_approach.desired_distance 304 | ); 305 | 306 | // Decide if we need to change the approach_direction to the local frame of the end effector orientation 307 | if( grasp.pre_grasp_approach.direction.header.frame_id == ee_parent_link ) 308 | { 309 | // Apply/compute the approach_direction vector in the local frame of the grasp_pose orientation 310 | pre_grasp_approach_direction_local = grasp_pose_eigen.rotation() * pre_grasp_approach_direction; 311 | } 312 | else 313 | { 314 | pre_grasp_approach_direction_local = pre_grasp_approach_direction; //grasp_pose_eigen.rotation() * pre_grasp_approach_direction; 315 | } 316 | 317 | // Update the grasp matrix usign the new locally-framed approach_direction 318 | pre_grasp_pose_eigen.translation() += pre_grasp_approach_direction_local; 319 | 320 | // Convert eigen pre-grasp position back to regular message 321 | tf::poseEigenToMsg(pre_grasp_pose_eigen, pre_grasp_pose.pose); 322 | 323 | // Copy original header to new grasp 324 | pre_grasp_pose.header = grasp_pose.header; 325 | 326 | return pre_grasp_pose; 327 | } 328 | 329 | 330 | 331 | } // namespace 332 | -------------------------------------------------------------------------------- /include/moveit_simple_grasps/moveit_blocks.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, CU Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of CU Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /** 36 | * \brief Simple pick place for blocks 37 | * \author Dave Coleman 38 | */ 39 | 40 | // ROS 41 | #include 42 | 43 | // MoveIt! 44 | #include 45 | 46 | // Grasp generation 47 | #include 48 | #include 49 | #include // simple tool for showing graspsp 50 | 51 | namespace moveit_simple_grasps 52 | { 53 | 54 | static const double BLOCK_SIZE = 0.04; 55 | 56 | struct MetaBlock 57 | { 58 | std::string name; 59 | geometry_msgs::Pose start_pose; 60 | geometry_msgs::Pose goal_pose; 61 | }; 62 | 63 | class MoveItBlocks 64 | { 65 | public: 66 | 67 | // grasp generator 68 | moveit_simple_grasps::SimpleGraspsPtr simple_grasps_; 69 | 70 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; 71 | 72 | // data for generating grasps 73 | moveit_simple_grasps::GraspData grasp_data_; 74 | 75 | // our interface with MoveIt 76 | boost::scoped_ptr move_group_; 77 | 78 | // which arm are we using 79 | std::string ee_group_name_; 80 | std::string planning_group_name_; 81 | 82 | 83 | ros::NodeHandle nh_; 84 | 85 | // settings 86 | bool auto_reset_; 87 | int auto_reset_sec_; 88 | int pick_place_count_; // tracks how many pick_places have run 89 | 90 | MoveItBlocks() 91 | : auto_reset_(false), 92 | auto_reset_sec_(4), 93 | pick_place_count_(0), 94 | nh_("~") 95 | { 96 | ROS_INFO_STREAM_NAMED("moveit_blocks","Starting MoveIt Blocks"); 97 | 98 | // Get arm info from param server 99 | nh_.param("ee_group_name", ee_group_name_, std::string("unknown")); 100 | nh_.param("planning_group_name", planning_group_name_, std::string("unknown")); 101 | 102 | ROS_INFO_STREAM_NAMED("moveit_blocks","End Effector: " << ee_group_name_); 103 | ROS_INFO_STREAM_NAMED("moveit_blocks","Planning Group: " << planning_group_name_); 104 | 105 | // Create MoveGroup for one of the planning groups 106 | move_group_.reset(new move_group_interface::MoveGroup(planning_group_name_)); 107 | move_group_->setPlanningTime(30.0); 108 | 109 | // Load grasp generator 110 | if (!grasp_data_.loadRobotGraspData(nh_, ee_group_name_)) 111 | ros::shutdown(); 112 | 113 | // Load the Robot Viz Tools for publishing to rviz 114 | visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools( grasp_data_.base_link_)); 115 | visual_tools_->setFloorToBaseHeight(-0.9); 116 | visual_tools_->loadEEMarker(grasp_data_.ee_group_, planning_group_name_); 117 | 118 | simple_grasps_.reset(new moveit_simple_grasps::SimpleGrasps(visual_tools_)); 119 | 120 | // Let everything load 121 | ros::Duration(1.0).sleep(); 122 | 123 | // Do it. 124 | startRoutine(); 125 | } 126 | 127 | bool startRoutine() 128 | { 129 | // Debug - calculate and output table surface dimensions 130 | /* 131 | if( false ) 132 | { 133 | double y_min, y_max, x_min, x_max; 134 | getTableWidthRange(y_min, y_max); 135 | getTableDepthRange(x_min, x_max); 136 | ROS_INFO_STREAM_NAMED("table","Blocks width range: " << y_min << " <= y <= " << y_max); 137 | ROS_INFO_STREAM_NAMED("table","Blocks depth range: " << x_min << " <= x <= " << x_max); 138 | } 139 | */ 140 | 141 | // Create start block positions (hard coded) 142 | std::vector blocks; 143 | double block_y = 0.1; 144 | double block_x = 0.35; 145 | // Flip the side of the table the blocks are on depending on which arm we are using 146 | //if( arm_.compare("right") == 0 ) 147 | // block_y *= -1; 148 | blocks.push_back( createStartBlock(block_x, block_y, "Block1") ); 149 | blocks.push_back( createStartBlock(block_x+0.1, block_y, "Block2") ); 150 | blocks.push_back( createStartBlock(block_x+0.2, block_y, "Block3") ); 151 | 152 | // The goal for each block is simply translating them on the y axis 153 | for (std::size_t i = 0; i < blocks.size(); ++i) 154 | { 155 | blocks[i].goal_pose = blocks[i].start_pose; 156 | blocks[i].goal_pose.position.y += 0.2; 157 | } 158 | 159 | // Show grasp visualizations or not 160 | visual_tools_->setMuted(false); 161 | 162 | // Create the walls and tables 163 | //createEnvironment(visual_tools_); 164 | 165 | // -------------------------------------------------------------------------------------------------------- 166 | // Repeat pick and place forever 167 | while(ros::ok()) 168 | { 169 | // -------------------------------------------------------------------------------------------- 170 | // Re-add all blocks 171 | for (std::size_t i = 0; i < blocks.size(); ++i) 172 | { 173 | resetBlock(blocks[i]); 174 | } 175 | 176 | // Place on left side, then back on right side 177 | for (std::size_t side = 0; side < 2; ++side) 178 | { 179 | 180 | // Do for all blocks 181 | for (std::size_t block_id = 0; block_id < blocks.size(); ++block_id) 182 | { 183 | // Pick ------------------------------------------------------------------------------------- 184 | while(ros::ok()) 185 | { 186 | ROS_INFO_STREAM_NAMED("pick_place","Picking '" << blocks[block_id].name << "'"); 187 | 188 | // Visualize the block we are about to pick 189 | visual_tools_->publishCuboid(blocks[block_id].start_pose, BLOCK_SIZE, BLOCK_SIZE, BLOCK_SIZE, rviz_visual_tools::BLUE); 190 | 191 | if( !pick(blocks[block_id].start_pose, blocks[block_id].name) ) 192 | { 193 | ROS_ERROR_STREAM_NAMED("pick_place","Pick failed."); 194 | 195 | // Ask user if we should try again 196 | if( !promptUser() ) 197 | exit(0); 198 | 199 | // Retry 200 | resetBlock(blocks[block_id]); 201 | } 202 | else 203 | { 204 | ROS_INFO_STREAM_NAMED("pick_place","Done with pick ---------------------------"); 205 | break; 206 | } 207 | } 208 | 209 | // Place ------------------------------------------------------------------------------------- 210 | while(ros::ok()) 211 | { 212 | ROS_INFO_STREAM_NAMED("pick_place","Placing '" << blocks[block_id].name << "'"); 213 | 214 | // Publish goal block location 215 | visual_tools_->publishCuboid(blocks[block_id].goal_pose, BLOCK_SIZE, BLOCK_SIZE, BLOCK_SIZE, rviz_visual_tools::BLUE); 216 | 217 | if( !place(blocks[block_id].goal_pose, blocks[block_id].name) ) 218 | { 219 | ROS_ERROR_STREAM_NAMED("pick_place","Place failed."); 220 | 221 | // Determine if the attached collision body as already been removed, in which case 222 | // we can ignore the failure and just resume picking 223 | /* 224 | if( !move_group_->hasAttachedObject(blocks[block_id].name) ) 225 | { 226 | ROS_WARN_STREAM_NAMED("pick_place","Collision object already detached, so auto resuming pick place."); 227 | 228 | // Ask user if we should try again 229 | if( !promptUser() ) 230 | break; // resume picking 231 | } 232 | */ 233 | 234 | // Ask user if we should try again 235 | if( !promptUser() ) 236 | exit(0); 237 | } 238 | else 239 | { 240 | ROS_INFO_STREAM_NAMED("pick_place","Done with place ----------------------------"); 241 | break; 242 | } 243 | } // place retry loop 244 | 245 | // Swap this block's start and end pose so that we can then move them back to position 246 | geometry_msgs::Pose temp = blocks[block_id].start_pose; 247 | blocks[block_id].start_pose = blocks[block_id].goal_pose; 248 | blocks[block_id].goal_pose = temp; 249 | 250 | pick_place_count_++; 251 | 252 | } // loop through 3 blocks 253 | 254 | ROS_INFO_STREAM_NAMED("pick_place","Finished picking and placing " << blocks.size() 255 | << " blocks. Total pick_places: " << pick_place_count_); 256 | 257 | } // place on both sides of table 258 | 259 | // Ask user if we should repeat 260 | //if( !promptUser() ) 261 | // break; 262 | ros::Duration(1.0).sleep(); 263 | 264 | } 265 | 266 | // Everything worked! 267 | return true; 268 | } 269 | 270 | void resetBlock(MetaBlock block) 271 | { 272 | // Remove attached object 273 | visual_tools_->cleanupACO(block.name); 274 | 275 | // Remove collision object 276 | visual_tools_->cleanupCO(block.name); 277 | 278 | // Add the collision block 279 | visual_tools_->publishCollisionBlock(block.start_pose, block.name, BLOCK_SIZE); 280 | } 281 | 282 | MetaBlock createStartBlock(double x, double y, const std::string name) 283 | { 284 | MetaBlock start_block; 285 | start_block.name = name; 286 | 287 | // Position 288 | start_block.start_pose.position.x = x; 289 | start_block.start_pose.position.y = y; 290 | start_block.start_pose.position.z = BLOCK_SIZE / 2.0; //getTableHeight(-0.9); 291 | 292 | // Orientation 293 | double angle = 0; // M_PI / 1.5; 294 | Eigen::Quaterniond quat(Eigen::AngleAxis(double(angle), Eigen::Vector3d::UnitZ())); 295 | start_block.start_pose.orientation.x = quat.x(); 296 | start_block.start_pose.orientation.y = quat.y(); 297 | start_block.start_pose.orientation.z = quat.z(); 298 | start_block.start_pose.orientation.w = quat.w(); 299 | 300 | return start_block; 301 | } 302 | 303 | bool pick(const geometry_msgs::Pose& block_pose, std::string block_name) 304 | { 305 | std::vector possible_grasps; 306 | 307 | // Pick grasp 308 | simple_grasps_->generateBlockGrasps( block_pose, grasp_data_, possible_grasps ); 309 | 310 | // Visualize them 311 | //visual_tools_->publishAnimatedGrasps(possible_grasps, grasp_data_.ee_parent_link_); 312 | visual_tools_->publishGrasps(possible_grasps, grasp_data_.ee_parent_link_); 313 | 314 | // Prevent collision with table 315 | //move_group_->setSupportSurfaceName(SUPPORT_SURFACE3_NAME); 316 | 317 | // Allow blocks to be touched by end effector 318 | { 319 | // an optional list of obstacles that we have semantic information about and that can be touched/pushed/moved in the course of grasping 320 | std::vector allowed_touch_objects; 321 | allowed_touch_objects.push_back("Block1"); 322 | allowed_touch_objects.push_back("Block2"); 323 | allowed_touch_objects.push_back("Block3"); 324 | allowed_touch_objects.push_back("Block4"); 325 | 326 | // Add this list to all grasps 327 | for (std::size_t i = 0; i < possible_grasps.size(); ++i) 328 | { 329 | possible_grasps[i].allowed_touch_objects = allowed_touch_objects; 330 | } 331 | } 332 | 333 | //ROS_INFO_STREAM_NAMED("","Grasp 0\n" << possible_grasps[0]); 334 | 335 | return move_group_->pick(block_name, possible_grasps); 336 | } 337 | 338 | bool place(const geometry_msgs::Pose& goal_block_pose, std::string block_name) 339 | { 340 | ROS_WARN_STREAM_NAMED("place","Placing '"<< block_name << "'"); 341 | 342 | std::vector place_locations; 343 | 344 | // Re-usable datastruct 345 | geometry_msgs::PoseStamped pose_stamped; 346 | pose_stamped.header.frame_id = grasp_data_.base_link_; 347 | pose_stamped.header.stamp = ros::Time::now(); 348 | 349 | // Create 360 degrees of place location rotated around a center 350 | for (double angle = 0; angle < 2*M_PI; angle += M_PI/2) 351 | { 352 | pose_stamped.pose = goal_block_pose; 353 | 354 | // Orientation 355 | Eigen::Quaterniond quat(Eigen::AngleAxis(double(angle), Eigen::Vector3d::UnitZ())); 356 | pose_stamped.pose.orientation.x = quat.x(); 357 | pose_stamped.pose.orientation.y = quat.y(); 358 | pose_stamped.pose.orientation.z = quat.z(); 359 | pose_stamped.pose.orientation.w = quat.w(); 360 | 361 | // Create new place location 362 | moveit_msgs::PlaceLocation place_loc; 363 | 364 | place_loc.place_pose = pose_stamped; 365 | 366 | visual_tools_->publishCuboid(place_loc.place_pose.pose, BLOCK_SIZE, BLOCK_SIZE, BLOCK_SIZE, rviz_visual_tools::BLUE); 367 | 368 | // Approach 369 | moveit_msgs::GripperTranslation pre_place_approach; 370 | pre_place_approach.direction.header.stamp = ros::Time::now(); 371 | pre_place_approach.desired_distance = grasp_data_.approach_retreat_desired_dist_; // The distance the origin of a robot link needs to travel 372 | pre_place_approach.min_distance = grasp_data_.approach_retreat_min_dist_; // half of the desired? Untested. 373 | pre_place_approach.direction.header.frame_id = grasp_data_.base_link_; 374 | pre_place_approach.direction.vector.x = 0; 375 | pre_place_approach.direction.vector.y = 0; 376 | pre_place_approach.direction.vector.z = -1; // Approach direction (negative z axis) // TODO: document this assumption 377 | place_loc.pre_place_approach = pre_place_approach; 378 | 379 | // Retreat 380 | moveit_msgs::GripperTranslation post_place_retreat; 381 | post_place_retreat.direction.header.stamp = ros::Time::now(); 382 | post_place_retreat.desired_distance = grasp_data_.approach_retreat_desired_dist_; // The distance the origin of a robot link needs to travel 383 | post_place_retreat.min_distance = grasp_data_.approach_retreat_min_dist_; // half of the desired? Untested. 384 | post_place_retreat.direction.header.frame_id = grasp_data_.base_link_; 385 | post_place_retreat.direction.vector.x = 0; 386 | post_place_retreat.direction.vector.y = 0; 387 | post_place_retreat.direction.vector.z = 1; // Retreat direction (pos z axis) 388 | place_loc.post_place_retreat = post_place_retreat; 389 | 390 | // Post place posture - use same as pre-grasp posture (the OPEN command) 391 | place_loc.post_place_posture = grasp_data_.pre_grasp_posture_; 392 | 393 | place_locations.push_back(place_loc); 394 | } 395 | 396 | // Prevent collision with table 397 | //move_group_->setSupportSurfaceName(SUPPORT_SURFACE3_NAME); 398 | 399 | move_group_->setPlannerId("RRTConnectkConfigDefault"); 400 | 401 | return move_group_->place(block_name, place_locations); 402 | } 403 | 404 | bool promptUser() 405 | { 406 | // Make sure ROS is still with us 407 | if( !ros::ok() ) 408 | return false; 409 | 410 | if( auto_reset_ ) 411 | { 412 | ROS_INFO_STREAM_NAMED("pick_place","Auto-retrying in " << auto_reset_sec_ << " seconds"); 413 | ros::Duration(auto_reset_sec_).sleep(); 414 | } 415 | else 416 | { 417 | ROS_INFO_STREAM_NAMED("pick_place","Retry? (y/n)"); 418 | char input; // used for prompting yes/no 419 | std::cin >> input; 420 | if( input == 'n' ) 421 | return false; 422 | } 423 | return true; 424 | } 425 | 426 | }; 427 | 428 | } //namespace 429 | -------------------------------------------------------------------------------- /launch/simple_grasps.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Marker1 10 | - /MotionPlanning1/Planned Path1 11 | Splitter Ratio: 0.5 12 | Tree Height: 738 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.03 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Class: rviz/Marker 54 | Enabled: true 55 | Marker Topic: /rviz_visual_tools 56 | Name: Marker 57 | Namespaces: 58 | Text: true 59 | end_effector: true 60 | Queue Size: 100 61 | Value: true 62 | - Class: moveit_rviz_plugin/MotionPlanning 63 | Enabled: false 64 | Move Group Namespace: "" 65 | MoveIt_Goal_Tolerance: 0 66 | MoveIt_Planning_Attempts: 10 67 | MoveIt_Planning_Time: 5 68 | MoveIt_Use_Constraint_Aware_IK: true 69 | MoveIt_Warehouse_Host: 127.0.0.1 70 | MoveIt_Warehouse_Port: 33829 71 | Name: MotionPlanning 72 | Planned Path: 73 | Links: 74 | All Links Enabled: true 75 | Expand Joint Details: false 76 | Expand Link Details: false 77 | Expand Tree: false 78 | Link Tree Style: Links in Alphabetic Order 79 | base: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | collision_head_link_1: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | collision_head_link_2: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | display: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | dummyhead1: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | head: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | head_camera: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | left_arm_itb: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | left_arm_mount: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | left_gripper: 120 | Alpha: 1 121 | Show Axes: false 122 | Show Trail: false 123 | Value: true 124 | left_gripper_base: 125 | Alpha: 1 126 | Show Axes: false 127 | Show Trail: false 128 | Value: true 129 | left_gripper_l_finger: 130 | Alpha: 1 131 | Show Axes: false 132 | Show Trail: false 133 | Value: true 134 | left_gripper_r_finger: 135 | Alpha: 1 136 | Show Axes: false 137 | Show Trail: false 138 | Value: true 139 | left_hand: 140 | Alpha: 1 141 | Show Axes: false 142 | Show Trail: false 143 | Value: true 144 | left_hand_accelerometer: 145 | Alpha: 1 146 | Show Axes: false 147 | Show Trail: false 148 | Value: true 149 | left_hand_camera: 150 | Alpha: 1 151 | Show Axes: false 152 | Show Trail: false 153 | Value: true 154 | left_hand_camera_axis: 155 | Alpha: 1 156 | Show Axes: false 157 | Show Trail: false 158 | left_hand_range: 159 | Alpha: 1 160 | Show Axes: false 161 | Show Trail: false 162 | Value: true 163 | left_lower_elbow: 164 | Alpha: 1 165 | Show Axes: false 166 | Show Trail: false 167 | Value: true 168 | left_lower_forearm: 169 | Alpha: 1 170 | Show Axes: false 171 | Show Trail: false 172 | Value: true 173 | left_lower_shoulder: 174 | Alpha: 1 175 | Show Axes: false 176 | Show Trail: false 177 | Value: true 178 | left_torso_itb: 179 | Alpha: 1 180 | Show Axes: false 181 | Show Trail: false 182 | left_upper_elbow: 183 | Alpha: 1 184 | Show Axes: false 185 | Show Trail: false 186 | Value: true 187 | left_upper_elbow_visual: 188 | Alpha: 1 189 | Show Axes: false 190 | Show Trail: false 191 | Value: true 192 | left_upper_forearm: 193 | Alpha: 1 194 | Show Axes: false 195 | Show Trail: false 196 | Value: true 197 | left_upper_forearm_visual: 198 | Alpha: 1 199 | Show Axes: false 200 | Show Trail: false 201 | Value: true 202 | left_upper_shoulder: 203 | Alpha: 1 204 | Show Axes: false 205 | Show Trail: false 206 | Value: true 207 | left_wrist: 208 | Alpha: 1 209 | Show Axes: false 210 | Show Trail: false 211 | Value: true 212 | pedestal: 213 | Alpha: 1 214 | Show Axes: false 215 | Show Trail: false 216 | Value: true 217 | right_arm_itb: 218 | Alpha: 1 219 | Show Axes: false 220 | Show Trail: false 221 | right_arm_mount: 222 | Alpha: 1 223 | Show Axes: false 224 | Show Trail: false 225 | right_gripper: 226 | Alpha: 1 227 | Show Axes: false 228 | Show Trail: false 229 | Value: true 230 | right_gripper_base: 231 | Alpha: 1 232 | Show Axes: false 233 | Show Trail: false 234 | Value: true 235 | right_gripper_l_finger: 236 | Alpha: 1 237 | Show Axes: false 238 | Show Trail: false 239 | Value: true 240 | right_gripper_r_finger: 241 | Alpha: 1 242 | Show Axes: false 243 | Show Trail: false 244 | Value: true 245 | right_hand: 246 | Alpha: 1 247 | Show Axes: false 248 | Show Trail: false 249 | Value: true 250 | right_hand_accelerometer: 251 | Alpha: 1 252 | Show Axes: false 253 | Show Trail: false 254 | Value: true 255 | right_hand_camera: 256 | Alpha: 1 257 | Show Axes: false 258 | Show Trail: false 259 | Value: true 260 | right_hand_camera_axis: 261 | Alpha: 1 262 | Show Axes: false 263 | Show Trail: false 264 | right_hand_range: 265 | Alpha: 1 266 | Show Axes: false 267 | Show Trail: false 268 | Value: true 269 | right_lower_elbow: 270 | Alpha: 1 271 | Show Axes: false 272 | Show Trail: false 273 | Value: true 274 | right_lower_forearm: 275 | Alpha: 1 276 | Show Axes: false 277 | Show Trail: false 278 | Value: true 279 | right_lower_shoulder: 280 | Alpha: 1 281 | Show Axes: false 282 | Show Trail: false 283 | Value: true 284 | right_torso_itb: 285 | Alpha: 1 286 | Show Axes: false 287 | Show Trail: false 288 | right_upper_elbow: 289 | Alpha: 1 290 | Show Axes: false 291 | Show Trail: false 292 | Value: true 293 | right_upper_elbow_visual: 294 | Alpha: 1 295 | Show Axes: false 296 | Show Trail: false 297 | Value: true 298 | right_upper_forearm: 299 | Alpha: 1 300 | Show Axes: false 301 | Show Trail: false 302 | Value: true 303 | right_upper_forearm_visual: 304 | Alpha: 1 305 | Show Axes: false 306 | Show Trail: false 307 | Value: true 308 | right_upper_shoulder: 309 | Alpha: 1 310 | Show Axes: false 311 | Show Trail: false 312 | Value: true 313 | right_wrist: 314 | Alpha: 1 315 | Show Axes: false 316 | Show Trail: false 317 | Value: true 318 | screen: 319 | Alpha: 1 320 | Show Axes: false 321 | Show Trail: false 322 | Value: true 323 | sonar_ring: 324 | Alpha: 1 325 | Show Axes: false 326 | Show Trail: false 327 | Value: true 328 | torso: 329 | Alpha: 1 330 | Show Axes: false 331 | Show Trail: false 332 | Value: true 333 | Loop Animation: false 334 | Robot Alpha: 0.5 335 | Show Robot Collision: false 336 | Show Robot Visual: true 337 | Show Trail: false 338 | State Display Time: REALTIME 339 | Trajectory Topic: /move_group/display_planned_path 340 | Planning Metrics: 341 | Payload: 1 342 | Show Joint Torques: false 343 | Show Manipulability: false 344 | Show Manipulability Index: false 345 | Show Weight Limit: false 346 | TextHeight: 0.08 347 | Planning Request: 348 | Colliding Link Color: 255; 0; 0 349 | Goal State Alpha: 1 350 | Goal State Color: 250; 128; 0 351 | Interactive Marker Size: 0 352 | Joint Violation Color: 255; 0; 255 353 | Planning Group: both_arms 354 | Query Goal State: false 355 | Query Start State: false 356 | Show Workspace: false 357 | Start State Alpha: 1 358 | Start State Color: 0; 255; 0 359 | Planning Scene Topic: planning_scene 360 | Robot Description: robot_description 361 | Scene Geometry: 362 | Scene Alpha: 0.9 363 | Scene Color: 50; 230; 50 364 | Scene Display Time: 0.2 365 | Show Scene Geometry: false 366 | Voxel Coloring: Z-Axis 367 | Voxel Rendering: Occupied Voxels 368 | Scene Robot: 369 | Attached Body Color: 150; 50; 150 370 | Links: 371 | All Links Enabled: true 372 | Expand Joint Details: false 373 | Expand Link Details: false 374 | Expand Tree: false 375 | Link Tree Style: Links in Alphabetic Order 376 | base: 377 | Alpha: 1 378 | Show Axes: false 379 | Show Trail: false 380 | collision_head_link_1: 381 | Alpha: 1 382 | Show Axes: false 383 | Show Trail: false 384 | Value: true 385 | collision_head_link_2: 386 | Alpha: 1 387 | Show Axes: false 388 | Show Trail: false 389 | Value: true 390 | display: 391 | Alpha: 1 392 | Show Axes: false 393 | Show Trail: false 394 | Value: true 395 | dummyhead1: 396 | Alpha: 1 397 | Show Axes: false 398 | Show Trail: false 399 | head: 400 | Alpha: 1 401 | Show Axes: false 402 | Show Trail: false 403 | Value: true 404 | head_camera: 405 | Alpha: 1 406 | Show Axes: false 407 | Show Trail: false 408 | left_arm_itb: 409 | Alpha: 1 410 | Show Axes: false 411 | Show Trail: false 412 | left_arm_mount: 413 | Alpha: 1 414 | Show Axes: false 415 | Show Trail: false 416 | left_gripper: 417 | Alpha: 1 418 | Show Axes: false 419 | Show Trail: false 420 | Value: true 421 | left_gripper_base: 422 | Alpha: 1 423 | Show Axes: false 424 | Show Trail: false 425 | Value: true 426 | left_gripper_l_finger: 427 | Alpha: 1 428 | Show Axes: false 429 | Show Trail: false 430 | Value: true 431 | left_gripper_r_finger: 432 | Alpha: 1 433 | Show Axes: false 434 | Show Trail: false 435 | Value: true 436 | left_hand: 437 | Alpha: 1 438 | Show Axes: false 439 | Show Trail: false 440 | Value: true 441 | left_hand_accelerometer: 442 | Alpha: 1 443 | Show Axes: false 444 | Show Trail: false 445 | Value: true 446 | left_hand_camera: 447 | Alpha: 1 448 | Show Axes: false 449 | Show Trail: false 450 | Value: true 451 | left_hand_camera_axis: 452 | Alpha: 1 453 | Show Axes: false 454 | Show Trail: false 455 | left_hand_range: 456 | Alpha: 1 457 | Show Axes: false 458 | Show Trail: false 459 | Value: true 460 | left_lower_elbow: 461 | Alpha: 1 462 | Show Axes: false 463 | Show Trail: false 464 | Value: true 465 | left_lower_forearm: 466 | Alpha: 1 467 | Show Axes: false 468 | Show Trail: false 469 | Value: true 470 | left_lower_shoulder: 471 | Alpha: 1 472 | Show Axes: false 473 | Show Trail: false 474 | Value: true 475 | left_torso_itb: 476 | Alpha: 1 477 | Show Axes: false 478 | Show Trail: false 479 | left_upper_elbow: 480 | Alpha: 1 481 | Show Axes: false 482 | Show Trail: false 483 | Value: true 484 | left_upper_elbow_visual: 485 | Alpha: 1 486 | Show Axes: false 487 | Show Trail: false 488 | Value: true 489 | left_upper_forearm: 490 | Alpha: 1 491 | Show Axes: false 492 | Show Trail: false 493 | Value: true 494 | left_upper_forearm_visual: 495 | Alpha: 1 496 | Show Axes: false 497 | Show Trail: false 498 | Value: true 499 | left_upper_shoulder: 500 | Alpha: 1 501 | Show Axes: false 502 | Show Trail: false 503 | Value: true 504 | left_wrist: 505 | Alpha: 1 506 | Show Axes: false 507 | Show Trail: false 508 | Value: true 509 | pedestal: 510 | Alpha: 1 511 | Show Axes: false 512 | Show Trail: false 513 | Value: true 514 | right_arm_itb: 515 | Alpha: 1 516 | Show Axes: false 517 | Show Trail: false 518 | right_arm_mount: 519 | Alpha: 1 520 | Show Axes: false 521 | Show Trail: false 522 | right_gripper: 523 | Alpha: 1 524 | Show Axes: false 525 | Show Trail: false 526 | Value: true 527 | right_gripper_base: 528 | Alpha: 1 529 | Show Axes: false 530 | Show Trail: false 531 | Value: true 532 | right_gripper_l_finger: 533 | Alpha: 1 534 | Show Axes: false 535 | Show Trail: false 536 | Value: true 537 | right_gripper_r_finger: 538 | Alpha: 1 539 | Show Axes: false 540 | Show Trail: false 541 | Value: true 542 | right_hand: 543 | Alpha: 1 544 | Show Axes: false 545 | Show Trail: false 546 | Value: true 547 | right_hand_accelerometer: 548 | Alpha: 1 549 | Show Axes: false 550 | Show Trail: false 551 | Value: true 552 | right_hand_camera: 553 | Alpha: 1 554 | Show Axes: false 555 | Show Trail: false 556 | Value: true 557 | right_hand_camera_axis: 558 | Alpha: 1 559 | Show Axes: false 560 | Show Trail: false 561 | right_hand_range: 562 | Alpha: 1 563 | Show Axes: false 564 | Show Trail: false 565 | Value: true 566 | right_lower_elbow: 567 | Alpha: 1 568 | Show Axes: false 569 | Show Trail: false 570 | Value: true 571 | right_lower_forearm: 572 | Alpha: 1 573 | Show Axes: false 574 | Show Trail: false 575 | Value: true 576 | right_lower_shoulder: 577 | Alpha: 1 578 | Show Axes: false 579 | Show Trail: false 580 | Value: true 581 | right_torso_itb: 582 | Alpha: 1 583 | Show Axes: false 584 | Show Trail: false 585 | right_upper_elbow: 586 | Alpha: 1 587 | Show Axes: false 588 | Show Trail: false 589 | Value: true 590 | right_upper_elbow_visual: 591 | Alpha: 1 592 | Show Axes: false 593 | Show Trail: false 594 | Value: true 595 | right_upper_forearm: 596 | Alpha: 1 597 | Show Axes: false 598 | Show Trail: false 599 | Value: true 600 | right_upper_forearm_visual: 601 | Alpha: 1 602 | Show Axes: false 603 | Show Trail: false 604 | Value: true 605 | right_upper_shoulder: 606 | Alpha: 1 607 | Show Axes: false 608 | Show Trail: false 609 | Value: true 610 | right_wrist: 611 | Alpha: 1 612 | Show Axes: false 613 | Show Trail: false 614 | Value: true 615 | screen: 616 | Alpha: 1 617 | Show Axes: false 618 | Show Trail: false 619 | Value: true 620 | sonar_ring: 621 | Alpha: 1 622 | Show Axes: false 623 | Show Trail: false 624 | Value: true 625 | torso: 626 | Alpha: 1 627 | Show Axes: false 628 | Show Trail: false 629 | Value: true 630 | Robot Alpha: 0.5 631 | Show Robot Collision: false 632 | Show Robot Visual: true 633 | Value: false 634 | Enabled: true 635 | Global Options: 636 | Background Color: 48; 48; 48 637 | Fixed Frame: base 638 | Frame Rate: 30 639 | Name: root 640 | Tools: 641 | - Class: rviz/Interact 642 | Hide Inactive Objects: true 643 | - Class: rviz/MoveCamera 644 | - Class: rviz/Select 645 | - Class: rviz/FocusCamera 646 | - Class: rviz/Measure 647 | - Class: rviz/SetInitialPose 648 | Topic: /initialpose 649 | - Class: rviz/SetGoal 650 | Topic: /move_base_simple/goal 651 | - Class: rviz/PublishPoint 652 | Single click: true 653 | Topic: /clicked_point 654 | Value: true 655 | Views: 656 | Current: 657 | Class: rviz/Orbit 658 | Distance: 1.9896 659 | Enable Stereo Rendering: 660 | Stereo Eye Separation: 0.06 661 | Stereo Focal Distance: 1 662 | Swap Stereo Eyes: false 663 | Value: false 664 | Focal Point: 665 | X: 0.374866 666 | Y: -0.131657 667 | Z: 0.0590967 668 | Name: Current View 669 | Near Clip Distance: 0.01 670 | Pitch: 0.575398 671 | Target Frame: 672 | Value: Orbit (rviz) 673 | Yaw: 4.49857 674 | Saved: ~ 675 | Window Geometry: 676 | Displays: 677 | collapsed: false 678 | Height: 1004 679 | Hide Left Dock: false 680 | Hide Right Dock: false 681 | Motion Planning: 682 | collapsed: false 683 | QMainWindow State: 000000ff00000000fd00000004000000000000010f0000038dfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000371000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000003b8000000160000001600000016000000010000010f000003f1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000003f1000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006380000003efc0100000002fb0000000800540069006d0065000000000000000638000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000066b0000038d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 684 | Selection: 685 | collapsed: false 686 | Time: 687 | collapsed: false 688 | Tool Properties: 689 | collapsed: false 690 | Views: 691 | collapsed: false 692 | Width: 1920 693 | X: 1909 694 | Y: 17 695 | --------------------------------------------------------------------------------