Open source cleanup (moveit#29)

* rename files

* desired state

* use panda for demos

* fixing demo files

* changing grasp_finger_width and suction_cup_stroke to grasp_max_depth

* fixing grasp_generator_demo

* fixing bug in distance from palm score

* fixup panda loading

* changing rviz default

* fixing grasp poses visualizer demo

* improving grasp scorer documentation

* fixing grasp_pose_to_eef_pose_ usage

* removig eef transform hack and restoring waitForNextStep

* fixing grasp_filter demo

* fixing up grasp pose demo and refactoring out grasp waypoints from the planner

* getting mesh_bounding_box_demo to run (segfaults because it needs the stls)

* updating example grasp_data yaml

* clang

* removing mesh_bounding_box_demo

* pr fixup

* improving grasp_generator_demo

* fixing two bugs. One with edge grasps moving in the wrong direction and the other with division by zero when max_translation == min_translation

* clang

* tests

* removing unused actions and msgs

* removing unused parameters

* test cleanup

* adding better demo

* README

* removing dead code

CMakeLists.txt

@@ -17,8 +17,6 @@ find_package(OpenMP)
 
 # Load catkin and all dependencies required for this package
 find_package(catkin REQUIRED COMPONENTS
-  actionlib
-  actionlib_msgs
   eigen_conversions
   geometry_msgs
   message_generation
@@ -36,28 +34,12 @@ find_package(catkin REQUIRED COMPONENTS
   trajectory_msgs
 )
 
-add_message_files(DIRECTORY msg FILES
-  GraspGeneratorOptions.msg
-)
-
-add_action_files(DIRECTORY action FILES
-  GenerateGrasps.action
-)
-
-generate_messages(DEPENDENCIES
-  actionlib_msgs
-  geometry_msgs
-  moveit_msgs
-  std_msgs
-)
-
 # Catkin
 catkin_package(
   LIBRARIES
     ${PROJECT_NAME}
     ${PROJECT_NAME}_filter
   CATKIN_DEPENDS
-    actionlib_msgs
     geometry_msgs
     message_runtime
     moveit_msgs
@@ -106,33 +88,31 @@ set_target_properties(${PROJECT_NAME}_filter PROPERTIES COMPILE_FLAGS "${CMAKE_C
 set_target_properties(${PROJECT_NAME}_filter PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")
 
 # Demo filter executable
-add_executable(${PROJECT_NAME}_filter_test src/demo/grasp_filter_test.cpp)
-target_link_libraries(${PROJECT_NAME}_filter_test
+add_executable(${PROJECT_NAME}_grasp_filter_demo src/demo/grasp_filter_demo.cpp)
+target_link_libraries(${PROJECT_NAME}_grasp_filter_demo
   ${PROJECT_NAME} ${PROJECT_NAME}_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES}
 )
 
 # Demo grasp executable
-add_executable(${PROJECT_NAME}_test src/demo/grasp_generator_demo.cpp)
-target_link_libraries(${PROJECT_NAME}_test
-  ${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-  ${Boost_LIBRARIES}
-)
-
-# Demo mesh bounding box
-add_executable(${PROJECT_NAME}_bounding_box_test src/demo/mesh_bounding_box_test.cpp)
-target_link_libraries(${PROJECT_NAME}_bounding_box_test
+add_executable(${PROJECT_NAME}_grasp_generator_demo src/demo/grasp_generator_demo.cpp)
+target_link_libraries(${PROJECT_NAME}_grasp_generator_demo
   ${PROJECT_NAME}
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
 )
 
 # Demo grasp data settings
-add_executable(${PROJECT_NAME}_grasp_poses_visualizer src/grasp_poses_visualizer.cpp)
-target_link_libraries(${PROJECT_NAME}_grasp_poses_visualizer
+add_executable(${PROJECT_NAME}_grasp_poses_visualizer_demo src/demo/grasp_poses_visualizer_demo.cpp)
+target_link_libraries(${PROJECT_NAME}_grasp_poses_visualizer_demo
   ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}
 )
 
+# Demo grasp pipeline
+add_executable(${PROJECT_NAME}_grasp_pipeline_demo src/demo/grasp_pipeline_demo.cpp)
+target_link_libraries(${PROJECT_NAME}_grasp_pipeline_demo
+  ${PROJECT_NAME} ${PROJECT_NAME}_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES}
+)
+
 #############
 ## INSTALL ##
 #############
@@ -152,10 +132,10 @@ install(DIRECTORY resources DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
 # Install executables
 install(TARGETS
-  ${PROJECT_NAME}_bounding_box_test
-  ${PROJECT_NAME}_filter_test
-  ${PROJECT_NAME}_grasp_poses_visualizer
-  ${PROJECT_NAME}_test
+  ${PROJECT_NAME}_grasp_filter_demo
+  ${PROJECT_NAME}_grasp_generator_demo
+  ${PROJECT_NAME}_grasp_poses_visualizer_demo
+  ${PROJECT_NAME}_grasp_pipeline_demo
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 

README.md

@@ -9,7 +9,7 @@ This package includes:
  - Pose-based grasp generator for a block
  - Separate grasp generators for custom objects such as rectanguar or cylindrical objects
  - Grasp filter
- - Test code and visualizations
+ - Demo code and visualizations
 
 <img src="https://picknik.ai/images/logo.jpg" width="100">
 
@@ -42,70 +42,48 @@ rosdep install --from-paths src --ignore-src --rosdistro kinetic
 
 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_robot/baxter_grasp_data.yaml](https://github.com/PickNikRobotics/moveit_grasps/blob/kinetic-devel/config_robot/baxter_grasp_data.yaml). See the comments within that file for explanations.
 
-To load that file at launch, you copy the example in the file [launch/grasp_test.launch](https://github.com/PickNikRobotics/moveit_grasps/blob/kinetic-devel/launch/grasp_test.launch) where you should see the line ``<rosparam command="load" file="$(find moveit_grasps)/config_robot/baxter_grasp_data.yaml"/>``.
+To load that file at launch, you copy the example in the file [launch/grasp_test.launch](https://github.com/PickNikRobotics/moveit_grasps/blob/kinetic-devel/launch/load_panda.launch) where you should see the line ``<rosparam command="load" file="$(find moveit_grasps)/config_robot/panda_grasp_data.yaml"/>``.
 
-Within that file you can specify the following (example taken from jaco):
+Within that file you will find all of the gripper specific parameters necessary for customizing MoveIt! Grasps with any suction or finger gripper
 
-    # ee group name as defined in the MoveIt! SRDF
-    end_effector_name: 'gripper'
-
-    # actuated joints in end effector
-    joints : ['jaco_joint_finger_1','jaco_joint_finger_2','jaco_joint_finger_3']
-
-    # open position (pre-grasp)
-    pregrasp_posture : [0.697, 0.697, 0.697]
-
-    # time to wait before grasping
-    pregrasp_time_from_start : 4.0
-
-    # close position (grasp)
-    grasp_posture : [0.0, 0.0, 0.0]
-
-    # time to wait after grasping
-    grasp_time_from_start : 4.0
-
-    # desired pose from end effector to grasp - [x,y,z]
-    grasp_pose_to_eef_translation :  [-0.05, 0, 0]
-
-    # desired pose from end effector to grasp - [roll, pitch, yall], in standard 3,2,1 notation
-    grasp_pose_to_eef_rotation : [1.5707, 0, 0]
-
-    # max depth of fingers - distance from finger tip to inner palm
-    finger_to_palm_depth : 0.11
-
-These values can be visualized by launching `grasp_test_rviz.launch` and `grasp_pose_visualizer.launch`.
+These values can be visualized by launching `grasp_generator_demo.launch`, `grasp_poses_visualizer_demo.launch`, and `grasp_pipeline_demo.launch`.
 The result should look like the following:
 
 ![Grasp Poses Visualization](https://raw.githubusercontent.com/PickNikRobotics/moveit_grasps/kinetic-devel/resources/moveit_grasps_poses.jpeg)
 
-Poses Visualized: Object, Grasp, EE
-Distances: `finger_to_palm_depth`, `pre(post)grasp_distance`, `pre(post)grasp_min_distance`
+### Some Important Parameters:
 
-### grasp_pose_to_eef
+#### grasp_pose_to_eef_transform
 
-The ``grasp_pose_to_eef`` translation is to allow different URDF end effectors to all work the same. In MoveIt! the EE always has a parent link, typically the wrist link or palm link. That parent link should have its Z-axis pointing towards the object you want to grasp i.e. where your pointer finger is pointing. This is the convention laid out in "Robotics" by John Craig in 1955. However, a lot of URDFs do not follow this convention, so this transform allows you to fix it.
+The `grasp_pose_to_eef_transform` represents the transform from the wrist to the end-effector. This parameter is provided to allow different URDF end effectors to all work together without recompiling code. In MoveIt! the EE always has a parent link, typically the wrist link or palm link. That parent link should have its Z-axis pointing towards the object you want to grasp i.e. where your pointer finger is pointing. This is the convention laid out in "Robotics" by John Craig in 1955. However, a lot of URDFs do not follow this convention, so this transform allows you to fix it.
 
 Additionally, the x-axis should be pointing up along the grasped object, i.e. the circular axis of a (beer) bottle if you were holding it. The y-axis should be point towards one of the fingers.
 
-### Switch from Bin to Shelf Picking with ``ideal_grasp_orientation_rpy``
+#### Switch from Bin to Shelf Picking with ``setIdealGraspPoseRPY`` and ``setIdealGraspPose``
 
-The ``ideal_grasp_orientation_rpy`` parameter in ``moveit_grasps_config.yaml`` can be used to select an ideal grasp orientation for picking. This parameter is used to score grasp candidates favoring grasps that are closer to the desired orientation. This is useful in applications such as bin and shelf picking where you would want to pick the objects from a bin with a grasp that is vertically alligned and you would want to pick obejects from a shelf with a grasp that is horozontally alligned.
-
-You can visualize the ``ideal_grasp_orientation_rpy`` by setting ``show_ideal_grasp_orientation: true``.
+The ``setIdealGraspPoseRPY`` and ``setIdealGraspPose`` methods in GraspGenerator can be used to select an ideal grasp orientation for picking. These methods is used to score grasp candidates favoring grasps that are closer to the desired orientation. This is useful in applications such as bin and shelf picking where you would want to pick the objects from a bin with a grasp that is vertically alligned and you would want to pick obejects from a shelf with a grasp that is horozontally alligned.
 
 ## Demo Scripts
 
-There are two demo scripts in this package. To view the tests, first start Rviz with:
+There are four demo scripts in this package. To view the tests, first start Rviz with:
 
     roslaunch moveit_grasps rviz.launch
 
+To see the entire MoveIt! Grasps pipeline in actoin:
+
+    roslaunch moveit_grasps grasp_pipeline_demo.launch
+
+To visualize gripper specific parameters:
+
+    roslaunch moveit_grasps grasp_poses_visualizer_demo.launch
+
 To test just grasp generation for randomly placed blocks:
 
-    roslaunch moveit_grasps test_grasp_generator.launch panda:=true
+    roslaunch moveit_grasps demo_grasp_generator.launch
 
-To also test the grasp filtering:
+To test the grasp filtering:
 
-    roslaunch moveit_grasps test_filter.launch baxter:=true
+    roslaunch moveit_grasps demo_filter.launch
 
 ### Grasp Filter
 
@@ -119,25 +97,19 @@ When filtered, the colors represent the following:
     CYAN - pregrasp filtered by collision
     GREEN - valid
 
-### Bounding Box From Mesh
-
-    roslaunch moveit_grasps rviz.launch
-    roslaunch moveit_grasps test_bounding_box.launch
-
-Each mesh in the products folder will be displayed with the calculated bounding box. Hit `enter` to move to the next mesh.
-
 ## Tested Robots
 
  - UR5
  - Jaco2
  - [Baxter](https://github.com/davetcoleman/baxter_cpp)
  - [REEM](http://wiki.ros.org/Robots/REEM)
+ - Panda
 
 ## Example Code
 
-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).
+The most current example for using MoveIt! Grasps is the `grasp_pipeline_demo` which can be found [here](https://github.com/PickNikRobotics//moveit_grasps/kinetic-devel/src/grasp_pipeline_demo.cpp).
 
-There are currently example implementations:
+There are other example implementations:
 
  - [baxter_pick_place](https://github.com/davetcoleman/baxter_cpp/tree/kinetic-devel/baxter_pick_place)
  - [reem_tabletop_grasping](https://github.com/pal-robotics/reem_tabletop_grasping)

config/moveit_grasps_config.yaml

@@ -1,30 +1,27 @@
 moveit_grasps:
+  # These settings are used by the grasp generator which generates GraspCandidates
   generator:
     # Other verbose stuff
     verbose: false
-    # Show the generated grasp arrows
-    # show_grasp_arrows: true
-    # show_grasp_arrows_speed: 0.1
+
     # Show hand of generated grasps
     show_prefiltered_grasps: false
     show_prefiltered_grasps_speed: 0.01
 
-    # Don't know if this should go here... seems like config_robot/*.yaml is a better place
-    depth_score_weight: 3.0
-    width_score_weight: 1.0
-    orientation_x_score_weight: 0.0
-    orientation_y_score_weight: 5.0
-    orientation_z_score_weight: 2.0
-    translation_x_score_weight: 0.0
-    translation_y_score_weight: 0.0
-    translation_z_score_weight: 4.0
-    overhang_x_score_weight: 0.0
-    overhang_y_score_weight: 0.0
+    ###########################
+    ## finger gripper settings
+    ###########################
+    # None yet
 
-    # Ideal grasp orientation (RPY)
-    ideal_grasp_orientation_rpy: [0, 3.14159, 3.14159]
-    show_ideal_grasp_orientation: true
+    ###########################
+    ## suction gripper settings
+    ###########################
+    # Visual debug options
+    debug_top_grasps: false
+    # Show suction gripper overhang
+    show_grasp_overhang: false
 
+  # The grasp filter filters out unreachable GraspCandidates and populates valid GraspCandidates' pregrasp_ik_solution_ and grasp_ik_solution_'s
   filter:
     # Show summary statistics
     statistics_verbose: true
@@ -34,16 +31,19 @@ moveit_grasps:
     collision_verbose: false
     collision_verbose_speed: 0.01
     # Collision checking in verbose on second pass (after all failed)
-    show_grasp_filter_collision_if_failed: true
+    show_grasp_filter_collision_if_failed: false
     # Show post-filter arrows
     show_filtered_grasps: false
     # Show pose-filter arm ik solutions
     show_filtered_arm_solutions: false
     show_filtered_arm_solutions_pregrasp_speed: 0.25
     show_filtered_arm_solutions_speed: 0.5
 
+  # The GraspPlanner generates approach, lift and retreat paths for a GraspCandidate.
+  # If the GraspPlanner is unable to plan 100% of the approach path and at least ~90% of the lift and retreat paths, then it considers the GraspCandidate to be infeasible
   planner:
     # Cartesian planning
+    statistics_verbose: false
     verbose_cartesian_filtering: false
-    show_cartesian_waypoints: true
+    show_cartesian_waypoints: false
     collision_checking_verbose: false