├── CMakeLists.txt
├── GPD_Files
    ├── README.md
    ├── gen3.launch
    ├── gen3_robotiq_2f_85.srdf.xacro
    └── ros_eigen_params.cfg
├── README.md
├── imgs
    ├── gpd_example.gif
    ├── gpd_example.png
    └── real_gpd_example.gif
├── include
    ├── manipulation_class.hpp
    └── perception_class.hpp
├── launch
    ├── gpd_grasping_node.launch
    ├── perception_test.launch
    └── real_gen3_grasping.launch
├── package.xml
└── src
    ├── functions
        ├── gpd.cpp
        ├── manipulation_class.cpp
        └── perception_class.cpp
    └── nodes
        ├── gpd_grasping_node.cpp
        └── perception_test.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.0.2)
 2 | project(uml_hri_nerve_pick_and_place)
 3 | 
 4 | add_compile_options(-std=c++11)
 5 | 
 6 | find_package(Eigen3 REQUIRED)
 7 | 
 8 | if(NOT EIGEN3_INCLUDE_DIRS)
 9 |   set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
10 | endif()
11 | 
12 | find_package(catkin REQUIRED COMPONENTS
13 |   interactive_markers
14 |   geometry_msgs
15 |   moveit_core
16 |   moveit_visual_tools
17 |   moveit_ros_planning
18 |   moveit_ros_planning_interface
19 |   pluginlib
20 |   geometric_shapes
21 |   pcl_conversions
22 |   pcl_ros
23 |   roscpp
24 |   rospy
25 |   sensor_msgs
26 |   std_msgs
27 |   tf
28 |   rosbag
29 |   tf2_ros
30 |   tf2_eigen
31 |   tf2_geometry_msgs
32 |   gpd_ros
33 |   control_msgs
34 | )
35 | 
36 | catkin_package(
37 | )
38 | 
39 | include_directories(include
40 |   ${catkin_INCLUDE_DIRS}
41 | )
42 | 
43 | add_executable(gpd_grasping_node
44 | 	src/nodes/gpd_grasping_node.cpp
45 | 	src/functions/manipulation_class.cpp
46 | 	src/functions/perception_class.cpp
47 |   src/functions/gpd.cpp
48 | )
49 | 
50 | add_executable(pick_place_tests
51 |   src/nodes/perception_test.cpp
52 | 	src/functions/perception_class.cpp
53 |   src/functions/manipulation_class.cpp
54 |   src/functions/gpd.cpp
55 | )
56 | 
57 | target_link_libraries(gpd_grasping_node ${catkin_LIBRARIES})
58 | target_link_libraries(pick_place_tests ${catkin_LIBRARIES})
59 | 
60 | add_dependencies(gpd_grasping_node ${catkin_EXPORTED_TARGETS})
61 | add_dependencies(pick_place_tests ${catkin_EXPORTED_TARGETS})
62 | 


--------------------------------------------------------------------------------
/GPD_Files/README.md:
--------------------------------------------------------------------------------
 1 | # Misc files that may be needed for Gen3 configuration 
 2 | 
 3 | ## gen3.launch:  
 4 | Launch file for the GPD_ROS package (`catkin_ws/src/gpd_ros/launch`). Replaces `urf.launch`
 5 | 
 6 | ## ros_eigen_params.cfg
 7 | Gen3 params for GPD, should be palced in `gpd-2.0.0/cfg`
 8 | 
 9 | ## gen3_robotiq_2f_85.srdf.xacro
10 | srdf to be placed in `ros_kortex/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof`  
11 | Contains modified arm and end-effector group names that are neccesary for the code in this repo to function with the Gen3. Can also just change the planning groups in the code to match your srdf.


--------------------------------------------------------------------------------
/GPD_Files/gen3.launch:
--------------------------------------------------------------------------------
 1 | <!-- Gen3 GPD_ROS LAUNCH FILE, REPLACES THEIR UR5.LAUNCH --> 
 2 | <launch>
 3 |   <node name="detect_grasps" pkg="gpd_ros" type="detect_grasps" output="screen">
 4 | 
 5 |     <!-- If sequential importance sampling is used (default: false) -->
 6 | <!--
 7 |     <param name="use_importance_sampling" value="false" />
 8 | -->
 9 | 
10 |     <!-- What type of point cloud is used and what ROS topic it comes from -->
11 |     <param name="cloud_type" value="0" /> <!-- 0: PointCloud2, 1: CloudIndexed, 2: CloudSamples -->
12 |     <param name="cloud_topic" value="/cloud_combined" />
13 | 
14 |     <!-- (optional) The ROS topic that the samples come from (default: an empty string) -->
15 |     <param name="samples_topic" value="" />
16 | 
17 |     <!-- Filepath to the configuration file for GPD -->
18 |     <!-- <param name="config_file" value="/home/ur5/projects/gpd/cfg/ros_vino_params.cfg" /> -->
19 |     <param name="config_file" value="/home/kyassini/gpd-2.0.0/cfg/ros_eigen_params.cfg" />
20 |     <param name="rviz_topic" value="plot_grasps" />
21 | 
22 |   </node>
23 | 
24 | </launch>
25 | 


--------------------------------------------------------------------------------
/GPD_Files/gen3_robotiq_2f_85.srdf.xacro:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" ?>
  2 | <!--This does not replace URDF, and is not an extension of URDF.
  3 |     This is a format for representing semantic information about the robot structure.
  4 |     A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
  5 | -->
  6 | <robot name="gen3_robotiq_2f_85"
  7 |     xmlns:xacro="http://www.ros.org/wiki/xacro">
  8 |     <xacro:arg name="prefix" default="" />
  9 |     <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
 10 |     <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
 11 |     <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
 12 |     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
 13 |     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
 14 | 
 15 |     <group name="gen3_arm_gripper">
 16 |         <group name="arm" />
 17 |         <group name="gripper" />
 18 |     </group>
 19 | 
 20 |     <group name="arm">
 21 |         <joint name="$(arg prefix)joint_1" />
 22 |         <joint name="$(arg prefix)joint_2" />
 23 |         <joint name="$(arg prefix)joint_3" />
 24 |         <joint name="$(arg prefix)joint_4" />
 25 |         <joint name="$(arg prefix)joint_5" />
 26 |         <joint name="$(arg prefix)joint_6" />
 27 |         <joint name="$(arg prefix)joint_7" />
 28 |         <joint name="$(arg prefix)end_effector" />
 29 |         <joint name="$(arg prefix)tool_frame_joint" />
 30 |     </group>
 31 | 
 32 |     <group name="gripper">
 33 |         <joint name="$(arg prefix)gripper_base_joint" />
 34 |         <joint name="$(arg prefix)finger_joint" />
 35 |         <joint name="$(arg prefix)left_outer_finger_joint" />
 36 |         <joint name="$(arg prefix)left_inner_finger_joint" />
 37 |         <joint name="$(arg prefix)left_inner_finger_pad_joint" />
 38 |         <joint name="$(arg prefix)left_inner_knuckle_joint" />
 39 |         <joint name="$(arg prefix)right_inner_knuckle_joint" />
 40 |         <joint name="$(arg prefix)right_outer_knuckle_joint" />
 41 |         <joint name="$(arg prefix)right_outer_finger_joint" />
 42 |         <joint name="$(arg prefix)right_inner_finger_joint" />
 43 |         <joint name="$(arg prefix)right_inner_finger_pad_joint" />
 44 |     </group>
 45 |     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
 46 |     <group_state name="home" group="arm">
 47 |         <joint name="$(arg prefix)joint_1" value="0" />
 48 |         <joint name="$(arg prefix)joint_2" value="0.26" />
 49 |         <joint name="$(arg prefix)joint_3" value="3.14" />
 50 |         <joint name="$(arg prefix)joint_4" value="-2.27" />
 51 |         <joint name="$(arg prefix)joint_5" value="0" />
 52 |         <joint name="$(arg prefix)joint_6" value="0.96" />
 53 |         <joint name="$(arg prefix)joint_7" value="1.57" />
 54 |     </group_state>
 55 |     <group_state name="retract" group="arm">
 56 |         <joint name="$(arg prefix)joint_1" value="0" />
 57 |         <joint name="$(arg prefix)joint_2" value="-0.35" />
 58 |         <joint name="$(arg prefix)joint_3" value="3.14" />
 59 |         <joint name="$(arg prefix)joint_4" value="-2.54" />
 60 |         <joint name="$(arg prefix)joint_5" value="0" />
 61 |         <joint name="$(arg prefix)joint_6" value="-0.87" />
 62 |         <joint name="$(arg prefix)joint_7" value="1.57" />
 63 |     </group_state>
 64 |     <group_state name="vertical" group="arm">
 65 |         <joint name="$(arg prefix)joint_1" value="0" />
 66 |         <joint name="$(arg prefix)joint_2" value="0" />
 67 |         <joint name="$(arg prefix)joint_3" value="0" />
 68 |         <joint name="$(arg prefix)joint_4" value="0" />
 69 |         <joint name="$(arg prefix)joint_5" value="0" />
 70 |         <joint name="$(arg prefix)joint_6" value="0" />
 71 |         <joint name="$(arg prefix)joint_7" value="0" />
 72 |     </group_state>
 73 |     <group_state name="opened" group="gripper">
 74 |         <joint name="$(arg prefix)left_inner_finger_joint" value="0" />
 75 |         <joint name="$(arg prefix)right_inner_knuckle_joint" value="0" />
 76 |         <joint name="$(arg prefix)finger_joint" value="0" />
 77 |         <joint name="$(arg prefix)right_inner_finger_joint" value="0" />
 78 |         <joint name="$(arg prefix)left_inner_knuckle_joint" value="0" />
 79 |         <joint name="$(arg prefix)right_outer_knuckle_joint" value="0" />
 80 |     </group_state>
 81 |     <group_state name="closed" group="gripper">
 82 |         <joint name="$(arg prefix)left_inner_finger_joint" value="0.8" />
 83 |         <joint name="$(arg prefix)right_inner_knuckle_joint" value="0.8" />
 84 |         <joint name="$(arg prefix)finger_joint" value="0.8" />
 85 |         <joint name="$(arg prefix)right_inner_finger_joint" value="-0.8" />
 86 |         <joint name="$(arg prefix)left_inner_knuckle_joint" value="0.8" />
 87 |         <joint name="$(arg prefix)right_outer_knuckle_joint" value="0.8" />
 88 |     </group_state>
 89 |     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
 90 |     <end_effector group="gripper" name="gripper" parent_group="arm" parent_link="$(arg prefix)end_effector_link"/>
 91 |     <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
 92 |     <passive_joint name="$(arg prefix)left_inner_finger_joint" />
 93 |     <passive_joint name="$(arg prefix)left_inner_knuckle_joint" />
 94 |     <passive_joint name="$(arg prefix)right_inner_knuckle_joint" />
 95 |     <passive_joint name="$(arg prefix)right_outer_knuckle_joint" />
 96 |     <passive_joint name="$(arg prefix)right_inner_finger_joint" />
 97 |     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
 98 |     <disable_collisions link1="$(arg prefix)base_link" link2="$(arg prefix)forearm_link" reason="Never" />
 99 |     <disable_collisions link1="$(arg prefix)base_link" link2="$(arg prefix)half_arm_1_link" reason="Never" />
100 |     <disable_collisions link1="$(arg prefix)base_link" link2="$(arg prefix)half_arm_2_link" reason="Never" />
101 |     <disable_collisions link1="$(arg prefix)base_link" link2="$(arg prefix)shoulder_link" reason="Adjacent" />
102 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)forearm_link" reason="Never" />
103 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)left_inner_finger" reason="Never" />
104 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)left_inner_finger_pad" reason="Never" />
105 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)left_inner_knuckle" reason="Never" />
106 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)left_outer_finger" reason="Never" />
107 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)left_outer_knuckle" reason="Never" />
108 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)right_inner_finger" reason="Never" />
109 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)right_inner_finger_pad" reason="Never" />
110 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)right_inner_knuckle" reason="Never" />
111 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)right_outer_finger" reason="Never" />
112 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)right_outer_knuckle" reason="Never" />
113 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)robotiq_arg2f_base_link" reason="Adjacent" />
114 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
115 |     <disable_collisions link1="$(arg prefix)bracelet_link" link2="$(arg prefix)spherical_wrist_2_link" reason="Adjacent" />
116 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)half_arm_1_link" reason="Never" />
117 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)half_arm_2_link" reason="Adjacent" />
118 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)left_inner_finger" reason="Never" />
119 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)left_inner_finger_pad" reason="Never" />
120 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)left_inner_knuckle" reason="Never" />
121 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)left_outer_finger" reason="Never" />
122 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)left_outer_knuckle" reason="Never" />
123 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)right_inner_finger" reason="Never" />
124 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)right_inner_finger_pad" reason="Never" />
125 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)right_inner_knuckle" reason="Never" />
126 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)right_outer_finger" reason="Never" />
127 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)right_outer_knuckle" reason="Never" />
128 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)robotiq_arg2f_base_link" reason="Never" />
129 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)shoulder_link" reason="Never" />
130 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)spherical_wrist_1_link" reason="Adjacent" />
131 |     <disable_collisions link1="$(arg prefix)forearm_link" link2="$(arg prefix)spherical_wrist_2_link" reason="Never" />
132 |     <disable_collisions link1="$(arg prefix)half_arm_1_link" link2="$(arg prefix)half_arm_2_link" reason="Adjacent" />
133 |     <disable_collisions link1="$(arg prefix)half_arm_1_link" link2="$(arg prefix)shoulder_link" reason="Adjacent" />
134 |     <disable_collisions link1="$(arg prefix)half_arm_1_link" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
135 |     <disable_collisions link1="$(arg prefix)half_arm_2_link" link2="$(arg prefix)left_inner_finger" reason="Never" />
136 |     <disable_collisions link1="$(arg prefix)half_arm_2_link" link2="$(arg prefix)left_inner_finger_pad" reason="Never" />
137 |     <disable_collisions link1="$(arg prefix)half_arm_2_link" link2="$(arg prefix)left_inner_knuckle" reason="Never" />
138 |     <disable_collisions link1="$(arg prefix)half_arm_2_link" link2="$(arg prefix)left_outer_finger" reason="Never" />
139 |     <disable_collisions link1="$(arg prefix)half_arm_2_link" link2="$(arg prefix)left_outer_knuckle" reason="Never" />
140 |     <disable_collisions link1="$(arg prefix)half_arm_2_link" link2="$(arg prefix)right_inner_finger" reason="Never" />
141 |     <disable_collisions link1="$(arg prefix)half_arm_2_link" link2="$(arg prefix)right_inner_finger_pad" reason="Never" />
142 |     <disable_collisions link1="$(arg prefix)half_arm_2_link" link2="$(arg prefix)right_inner_knuckle" reason="Never" />
143 |     <disable_collisions link1="$(arg prefix)half_arm_2_link" link2="$(arg prefix)robotiq_arg2f_base_link" reason="Never" />
144 |     <disable_collisions link1="$(arg prefix)half_arm_2_link" link2="$(arg prefix)shoulder_link" reason="Never" />
145 |     <disable_collisions link1="$(arg prefix)half_arm_2_link" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
146 |     <disable_collisions link1="$(arg prefix)half_arm_2_link" link2="$(arg prefix)spherical_wrist_2_link" reason="Never" />
147 |     <disable_collisions link1="$(arg prefix)left_inner_finger" link2="$(arg prefix)left_inner_finger_pad" reason="Adjacent" />
148 |     <disable_collisions link1="$(arg prefix)left_inner_finger" link2="$(arg prefix)left_inner_knuckle" reason="Default" />
149 |     <disable_collisions link1="$(arg prefix)left_inner_finger" link2="$(arg prefix)left_outer_finger" reason="Adjacent" />
150 |     <disable_collisions link1="$(arg prefix)left_inner_finger" link2="$(arg prefix)left_outer_knuckle" reason="Never" />
151 |     <disable_collisions link1="$(arg prefix)left_inner_finger" link2="$(arg prefix)right_inner_finger" reason="Never" />
152 |     <disable_collisions link1="$(arg prefix)left_inner_finger" link2="$(arg prefix)right_inner_finger_pad" reason="Never" />
153 |     <disable_collisions link1="$(arg prefix)left_inner_finger" link2="$(arg prefix)right_inner_knuckle" reason="Never" />
154 |     <disable_collisions link1="$(arg prefix)left_inner_finger" link2="$(arg prefix)right_outer_finger" reason="Never" />
155 |     <disable_collisions link1="$(arg prefix)left_inner_finger" link2="$(arg prefix)right_outer_knuckle" reason="Never" />
156 |     <disable_collisions link1="$(arg prefix)left_inner_finger" link2="$(arg prefix)robotiq_arg2f_base_link" reason="Never" />
157 |     <disable_collisions link1="$(arg prefix)left_inner_finger" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
158 |     <disable_collisions link1="$(arg prefix)left_inner_finger" link2="$(arg prefix)spherical_wrist_2_link" reason="Never" />
159 |     <disable_collisions link1="$(arg prefix)left_inner_finger_pad" link2="$(arg prefix)left_inner_knuckle" reason="Never" />
160 |     <disable_collisions link1="$(arg prefix)left_inner_finger_pad" link2="$(arg prefix)left_outer_finger" reason="Never" />
161 |     <disable_collisions link1="$(arg prefix)left_inner_finger_pad" link2="$(arg prefix)left_outer_knuckle" reason="Never" />
162 |     <disable_collisions link1="$(arg prefix)left_inner_finger_pad" link2="$(arg prefix)right_inner_finger" reason="Never" />
163 |     <disable_collisions link1="$(arg prefix)left_inner_finger_pad" link2="$(arg prefix)right_inner_finger_pad" reason="Never" />
164 |     <disable_collisions link1="$(arg prefix)left_inner_finger_pad" link2="$(arg prefix)right_inner_knuckle" reason="Never" />
165 |     <disable_collisions link1="$(arg prefix)left_inner_finger_pad" link2="$(arg prefix)right_outer_finger" reason="Never" />
166 |     <disable_collisions link1="$(arg prefix)left_inner_finger_pad" link2="$(arg prefix)right_outer_knuckle" reason="Never" />
167 |     <disable_collisions link1="$(arg prefix)left_inner_finger_pad" link2="$(arg prefix)robotiq_arg2f_base_link" reason="Never" />
168 |     <disable_collisions link1="$(arg prefix)left_inner_finger_pad" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
169 |     <disable_collisions link1="$(arg prefix)left_inner_finger_pad" link2="$(arg prefix)spherical_wrist_2_link" reason="Never" />
170 |     <disable_collisions link1="$(arg prefix)left_inner_knuckle" link2="$(arg prefix)left_outer_finger" reason="Never" />
171 |     <disable_collisions link1="$(arg prefix)left_inner_knuckle" link2="$(arg prefix)left_outer_knuckle" reason="Never" />
172 |     <disable_collisions link1="$(arg prefix)left_inner_knuckle" link2="$(arg prefix)right_inner_finger" reason="Never" />
173 |     <disable_collisions link1="$(arg prefix)left_inner_knuckle" link2="$(arg prefix)right_inner_finger_pad" reason="Never" />
174 |     <disable_collisions link1="$(arg prefix)left_inner_knuckle" link2="$(arg prefix)right_inner_knuckle" reason="Never" />
175 |     <disable_collisions link1="$(arg prefix)left_inner_knuckle" link2="$(arg prefix)right_outer_finger" reason="Never" />
176 |     <disable_collisions link1="$(arg prefix)left_inner_knuckle" link2="$(arg prefix)right_outer_knuckle" reason="Never" />
177 |     <disable_collisions link1="$(arg prefix)left_inner_knuckle" link2="$(arg prefix)robotiq_arg2f_base_link" reason="Adjacent" />
178 |     <disable_collisions link1="$(arg prefix)left_inner_knuckle" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
179 |     <disable_collisions link1="$(arg prefix)left_inner_knuckle" link2="$(arg prefix)spherical_wrist_2_link" reason="Never" />
180 |     <disable_collisions link1="$(arg prefix)left_outer_finger" link2="$(arg prefix)left_outer_knuckle" reason="Adjacent" />
181 |     <disable_collisions link1="$(arg prefix)left_outer_finger" link2="$(arg prefix)right_inner_finger" reason="Never" />
182 |     <disable_collisions link1="$(arg prefix)left_outer_finger" link2="$(arg prefix)right_inner_finger_pad" reason="Never" />
183 |     <disable_collisions link1="$(arg prefix)left_outer_finger" link2="$(arg prefix)right_inner_knuckle" reason="Never" />
184 |     <disable_collisions link1="$(arg prefix)left_outer_finger" link2="$(arg prefix)right_outer_finger" reason="Never" />
185 |     <disable_collisions link1="$(arg prefix)left_outer_finger" link2="$(arg prefix)right_outer_knuckle" reason="Never" />
186 |     <disable_collisions link1="$(arg prefix)left_outer_finger" link2="$(arg prefix)robotiq_arg2f_base_link" reason="Never" />
187 |     <disable_collisions link1="$(arg prefix)left_outer_finger" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
188 |     <disable_collisions link1="$(arg prefix)left_outer_finger" link2="$(arg prefix)spherical_wrist_2_link" reason="Never" />
189 |     <disable_collisions link1="$(arg prefix)left_outer_knuckle" link2="$(arg prefix)right_inner_finger" reason="Never" />
190 |     <disable_collisions link1="$(arg prefix)left_outer_knuckle" link2="$(arg prefix)right_inner_finger_pad" reason="Never" />
191 |     <disable_collisions link1="$(arg prefix)left_outer_knuckle" link2="$(arg prefix)right_inner_knuckle" reason="Never" />
192 |     <disable_collisions link1="$(arg prefix)left_outer_knuckle" link2="$(arg prefix)right_outer_finger" reason="Never" />
193 |     <disable_collisions link1="$(arg prefix)left_outer_knuckle" link2="$(arg prefix)right_outer_knuckle" reason="Never" />
194 |     <disable_collisions link1="$(arg prefix)left_outer_knuckle" link2="$(arg prefix)robotiq_arg2f_base_link" reason="Adjacent" />
195 |     <disable_collisions link1="$(arg prefix)left_outer_knuckle" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
196 |     <disable_collisions link1="$(arg prefix)left_outer_knuckle" link2="$(arg prefix)spherical_wrist_2_link" reason="Never" />
197 |     <disable_collisions link1="$(arg prefix)right_inner_finger" link2="$(arg prefix)right_inner_finger_pad" reason="Adjacent" />
198 |     <disable_collisions link1="$(arg prefix)right_inner_finger" link2="$(arg prefix)right_inner_knuckle" reason="Default" />
199 |     <disable_collisions link1="$(arg prefix)right_inner_finger" link2="$(arg prefix)right_outer_finger" reason="Adjacent" />
200 |     <disable_collisions link1="$(arg prefix)right_inner_finger" link2="$(arg prefix)right_outer_knuckle" reason="Never" />
201 |     <disable_collisions link1="$(arg prefix)right_inner_finger" link2="$(arg prefix)robotiq_arg2f_base_link" reason="Never" />
202 |     <disable_collisions link1="$(arg prefix)right_inner_finger" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
203 |     <disable_collisions link1="$(arg prefix)right_inner_finger" link2="$(arg prefix)spherical_wrist_2_link" reason="Never" />
204 |     <disable_collisions link1="$(arg prefix)right_inner_finger_pad" link2="$(arg prefix)right_inner_knuckle" reason="Never" />
205 |     <disable_collisions link1="$(arg prefix)right_inner_finger_pad" link2="$(arg prefix)right_outer_finger" reason="Never" />
206 |     <disable_collisions link1="$(arg prefix)right_inner_finger_pad" link2="$(arg prefix)right_outer_knuckle" reason="Never" />
207 |     <disable_collisions link1="$(arg prefix)right_inner_finger_pad" link2="$(arg prefix)robotiq_arg2f_base_link" reason="Never" />
208 |     <disable_collisions link1="$(arg prefix)right_inner_finger_pad" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
209 |     <disable_collisions link1="$(arg prefix)right_inner_finger_pad" link2="$(arg prefix)spherical_wrist_2_link" reason="Never" />
210 |     <disable_collisions link1="$(arg prefix)right_inner_knuckle" link2="$(arg prefix)right_outer_finger" reason="Never" />
211 |     <disable_collisions link1="$(arg prefix)right_inner_knuckle" link2="$(arg prefix)right_outer_knuckle" reason="Never" />
212 |     <disable_collisions link1="$(arg prefix)right_inner_knuckle" link2="$(arg prefix)robotiq_arg2f_base_link" reason="Adjacent" />
213 |     <disable_collisions link1="$(arg prefix)right_inner_knuckle" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
214 |     <disable_collisions link1="$(arg prefix)right_inner_knuckle" link2="$(arg prefix)spherical_wrist_2_link" reason="Never" />
215 |     <disable_collisions link1="$(arg prefix)right_outer_finger" link2="$(arg prefix)right_outer_knuckle" reason="Adjacent" />
216 |     <disable_collisions link1="$(arg prefix)right_outer_finger" link2="$(arg prefix)robotiq_arg2f_base_link" reason="Never" />
217 |     <disable_collisions link1="$(arg prefix)right_outer_finger" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
218 |     <disable_collisions link1="$(arg prefix)right_outer_finger" link2="$(arg prefix)spherical_wrist_2_link" reason="Never" />
219 |     <disable_collisions link1="$(arg prefix)right_outer_knuckle" link2="$(arg prefix)robotiq_arg2f_base_link" reason="Adjacent" />
220 |     <disable_collisions link1="$(arg prefix)right_outer_knuckle" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
221 |     <disable_collisions link1="$(arg prefix)right_outer_knuckle" link2="$(arg prefix)spherical_wrist_2_link" reason="Never" />
222 |     <disable_collisions link1="$(arg prefix)robotiq_arg2f_base_link" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
223 |     <disable_collisions link1="$(arg prefix)robotiq_arg2f_base_link" link2="$(arg prefix)spherical_wrist_2_link" reason="Never" />
224 |     <disable_collisions link1="$(arg prefix)shoulder_link" link2="$(arg prefix)spherical_wrist_1_link" reason="Never" />
225 |     <disable_collisions link1="$(arg prefix)spherical_wrist_1_link" link2="$(arg prefix)spherical_wrist_2_link" reason="Adjacent" />
226 | </robot>
227 | 


--------------------------------------------------------------------------------
/GPD_Files/ros_eigen_params.cfg:
--------------------------------------------------------------------------------
 1 | # Path to config file for robot hand geometry
 2 | hand_geometry_filename = 0
 3 | 
 4 | # Path to config file for volume and image geometry
 5 | image_geometry_filename = 0
 6 | 
 7 | # ==== Robot Hand Geometry ====
 8 | #   finger_width: the width of the finger
 9 | #   outer_diameter: the diameter of the robot hand (= maximum aperture + 2 * finger width)
10 | #   hand_depth: the finger length (measured from hand base to finger tip)
11 | #   hand_height: the height of the hand
12 | #   init_bite: the minimum amount of the object to be covered by the hand
13 | finger_width = 0.01
14 | hand_outer_diameter = 0.105
15 | hand_depth = 0.06
16 | hand_height = 0.02
17 | init_bite = 0.01
18 | 
19 | # ==== Grasp Descriptor ====
20 | #   volume_width: the width of the cube inside the robot hand
21 | #   volume_depth: the depth of the cube inside the robot hand
22 | #   volume_height: the height of the cube inside the robot hand
23 | #   image_size: the size of the image (width and height; image is square)
24 | #   image_num_channels: the number of image channels
25 | volume_width = 0.10
26 | volume_depth = 0.06
27 | volume_height = 0.02
28 | image_size = 60
29 | image_num_channels = 15
30 | 
31 | # (OpenVINO) Path to directory that contains neural network parameters
32 | weights_file = /home/kyassini/gpd-2.0.0/models/lenet/15channels/params/
33 | 
34 | # Preprocessing of point cloud
35 | #   voxelize: if the cloud gets voxelized/downsampled
36 | #   remove_outliers: if statistical outliers are removed from the cloud (used to remove noise)
37 | #   workspace: the workspace of the robot (dimensions of a cube centered at origin of point cloud)
38 | #   camera_position: the position of the camera from which the cloud was taken
39 | #   sample_above_plane: only draws samples which do not belong to the table plane
40 | voxelize = 1
41 | remove_outliers = 1
42 | workspace = -1.0 1.0 -1.0 1.0 -1.0 1.0
43 | camera_position = 0 0 0
44 | sample_above_plane = 1
45 | 
46 | # Grasp candidate generation
47 | #   num_samples: the number of samples to be drawn from the point cloud
48 | #   num_threads: the number of CPU threads to be used
49 | #   nn_radius: the radius for the neighborhood search
50 | #   num_orientations: the number of robot hand orientations to evaluate
51 | #   rotation_axes: the axes about which the point neighborhood gets rotated
52 | num_samples = 500
53 | num_threads = 8
54 | nn_radius = 0.01
55 | num_orientations = 8
56 | num_finger_placements = 10
57 | hand_axes = 2
58 | deepen_hand = 1
59 | 
60 | # Filtering of candidates
61 | #   min_aperture: the minimum gripper width
62 | #   max_aperture: the maximum gripper width
63 | #   workspace_grasps: dimensions of a cube centered at origin of point cloud; should be smaller than <workspace>
64 | min_aperture = 0.0
65 | max_aperture = 0.85
66 | workspace_grasps = -1 1 -1 1 -1 1
67 | 
68 | # Filtering of candidates based on their approach direction
69 | #   filter_approach_direction: turn filtering on/off
70 | #   direction: the direction to compare against
71 | #   angle_thresh: angle in radians above which grasps are filtered
72 | filter_approach_direction = 1
73 | direction = 0 0 -1
74 | thresh_rad = 1.0
75 | 
76 | # Clustering of grasps
77 | #   min_inliers: minimum number of inliers per cluster; set to 0 to turn off clustering
78 | min_inliers = 0
79 | 
80 | # Grasp selection
81 | #   num_selected: number of selected grasps (sorted by score)
82 | num_selected = 10
83 | 
84 | # Visualization
85 | #   plot_normals: plot the surface normals
86 | #   plot_samples: plot the samples
87 | #   plot_candidates: plot the grasp candidates
88 | #   plot_filtered_candidates: plot the grasp candidates which remain after filtering
89 | #   plot_valid_grasps: plot the candidates that are identified as valid grasps
90 | #   plot_clustered_grasps: plot the grasps that after clustering
91 | #   plot_selected_grasps: plot the selected grasps (final output)
92 | plot_normals = 0
93 | plot_samples = 0
94 | plot_candidates = 0
95 | plot_filtered_candidates = 0
96 | plot_valid_grasps = 0
97 | plot_clustered_grasps = 0
98 | plot_selected_grasps = 0
99 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Description
 2 | This repo contains ROS packages in separate branches for testing two grasping algorithms (**GPD 2.0.0** [1] and GQCNN 1.1.0) on the Kinova Gen3. Specifically, for grasping comparison experiments.
 3 | All testing was done on Ubuntu 16.04 w/ ROS Kinetic in Simulation and with a Real workstation. 
 4 | 
 5 | ## Required ROS packages for the Gen3:
 6 | * [uml_hri_nerve_armada_workstation](https://github.com/uml-robotics/uml_hri_nerve_armada_workstation)
 7 | * [ros_kortex](https://github.com/Kinovarobotics/ros_kortex)
 8 | * [ros_kortex_vision](https://github.com/Kinovarobotics/ros_kortex_vision)
 9 | * [kinova_ros](https://github.com/Kinovarobotics/kinova-ros)  
10 | More details here: https://github.com/uml-robotics/uml_hri_nerve_pick_and_place
11 | 
12 | ## Setting Up the Gen3 through Ethernet:  
13 | [Gen3 Manual](https://www.kinovarobotics.com/sites/default/files/UG-014_KINOVA_Gen3_Ultra_lightweight_robot-User_guide_EN_R01.pdf)
14 | 1. Reset robot to factory settings (Hold power button for 10 seconds)
15 | 2. Connect Ethernet cable from arm to PC
16 | 2. On Ubuntu: Add Ethernet Network Connection -> IPv4 Settings -> Address = 192.168.1.11, Mask = 255.255.255.0
17 | 3. Go to 192.168.1.10 on a browser to confirm that the arm is connected
18 | 4. Use the arm in RVIZ: `roslaunch kortex_driver kortex_driver.launch arm:=gen3 gripper:=robotiq_2f_85 ip_address:=192.168.1.10`  
19 | 
20 | ## Setting up GPD_ROS
21 | 1. [Follow the GPD Install Instructions](https://github.com/atenpas/gpd/tree/2.0.0#install)
22 |    1. Make sure to pull the GPD 2.0.0 release
23 |    1. Use PCL version 1.7.2 (Solves PCL error when using GPD_ROS)
24 |    1. Use OpenCV version 3.4.0.14
25 |    1. Had to comment out lines 769:773 in `src/gpd/util/plot.cpp` to solve a "not found" error
26 |    1. Might have to remove the `improc.h` include that causes a build error
27 | 1. Change paths in `cfg/ros_eigen_params.cfg` Example:
28 |     ```
29 |     weights_file = /home/kyassini/gpd-2.0.0/models/lenet/15channels/params/
30 |     ```
31 | 1. Make sure `./detect_grasps ../cfg/eigen_params.cfg ../tutorials/krylon.pcd` works without any errors and the GUI pops up. (Press E to see the grasps)
32 |    1. If anything fails then make sure to resolve it before moving on
33 | 1. Clone [GPD_ROS](https://github.com/atenpas/gpd_ros/) to your catkin_ws/src
34 | 1. Replace `ur5.launch` with desired params for the arm and your install location of GPD 2.0.0.
35 |    1. `Gen3.launch` example included in this repo
36 | 1. `Catkin build`
37 |    1. NOTE: You may have to `catkin clean`, then build ONLY gpd_ros first. After, you can build everything else. Doing this solved a "bad date error" for me.   
38 | 
39 | ## Running the GPD Grasping node
40 | 1. Need a running kortex diver:
41 |    1. For simulation (Can toggle sim_workstation):  
42 |    `roslaunch kortex_gazebo spawn_kortex_robot.launch arm:=gen3 gripper:=robotiq_2f_85 sim_workstation:=false dof:=7`
43 |    1. For the real Arm:  
44 |    `roslaunch kortex_driver kortex_driver.launch arm:=gen3 gripper:=robotiq_2f_85 ip_address:=192.168.1.10`  
45 |    `roslaunch kinova_vision kinova_vision.launch device:=192.168.1.10`
46 | 1. Build and launch the appropriate launch file
47 |    1. `gpd_grasping_node.launch`: launches GPD and the grasping node
48 |    1. `real_gen3_grasping`: launches the above and the Kinova vision node  
49 | 
50 | If everything runs correctly, you will get the following behavior:
51 |    
52 | <p align="center">
53 | <img src="imgs/real_gpd_example.gif" width="250"><img src="imgs/gpd_example.gif" width="250"><img src="imgs/gpd_example.png" width="150"> 
54 | </p>
55 | 
56 | ## Troubleshooting
57 | First start with the simulated arm and the sim_workstation disabled to reduced other variables. View the published `Combined_Cloud` in rviz. 
58 | Occasionally in simulation the cloud will not be transformed correctly, not sure how to fix this as the same code works 100% of the time with the real depth camera.
59 | * Combined_Cloud is empty or not correct:
60 |   * Make sure the correct pass-through filter values are being used for your workspace and that the correct topic is being used in the cloud transform. 
61 |   Use `perception_test.launch` to test the perception code and visualize the published cloud.  
62 |   **NOTE:** the clouds generated in simulation are unreliable sometimes and not as consistent as the clouds using the real arm. Sometimes they will be transformed in wierd ways and can get totally cutoff due to the pass-through values.
63 | * The cloud looks fine and only contains the object, but no grasps are being generated.
64 |   * Enable `plot_selected_grasps = 1` in `gpd/cfg/ros_eigen_params.cfg`. Make sure the GPD grasps look good. You will have to play around with the config values for the arm to get it right.
65 | * GPD shows OK grasps, but the arm isn't finding a plan to the grasp.
66 |   * Make sure the transform topic is correct and that there are no inaccurate collision objects added.
67 | 
68 | ## Basic Grasping Node Structure
69 | 1. Move to a set position
70 | 2. Take a "screenshot" of the point cloud from the wrist depth camera
71 | 3. Filter and potentially concatenate the clouds
72 | 4. Publish cloud to GPD_ROS node, get a grasp msg in return
73 | 5. Calculate pre and post grasp positions along approach vector from the grasp msg
74 | 6. Plan to Pre-grasp, move in to grasp object, and then move out to a post-grasp
75 | 7. Move to a "hold" position for 5 seconds to verify that the object was adequately grasped
76 | 
77 | ## Notes
78 | * Ran into several issues installing GPD. The versions and details specified above should fix them all, but it is possible that something is missing since I installed multiple versions.
79 | Pay attention to any build errors.
80 | * Could not get the [MoveIt pick() pipeline](http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/pick_place/pick_place_tutorial.html) to work correctly. 
81 | Had to call move() to the pose_sample which removed the ability to use a pre and post grasp as needed for actual grasping.
82 | So I Replaced it with using the pose generated from the GPD grasp message directly and [this `createPickingEEFPose` function](https://gist.github.com/tkelestemur/60401be131344dae98671b95d46060f8#file-hsr_gpd_sample-cpp-L9)
83 | to generate a pre and grasp pose directly from the approach vector.
84 | * Had an issue with the group names in the `gen3_robotiq_2f_85.srdf.xacro`. This was fixed by editing the srdf which can be found in this repo under GPD_Files. However, might not be needed since pick() isn't being used anymore. 
85 | 
86 | ## References
87 | [1] Andreas ten Pas, Marcus Gualtieri, Kate Saenko, and Robert Platt. Grasp Pose Detection in Point Clouds. The International Journal of Robotics Research, Vol 36, Issue 13-14, pp. 1455-1473. October 2017.


--------------------------------------------------------------------------------
/imgs/gpd_example.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/kyassini/manipulation_experiments/287baa7767744cdfe966db02b3f5e203ca513980/imgs/gpd_example.gif


--------------------------------------------------------------------------------
/imgs/gpd_example.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/kyassini/manipulation_experiments/287baa7767744cdfe966db02b3f5e203ca513980/imgs/gpd_example.png


--------------------------------------------------------------------------------
/imgs/real_gpd_example.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/kyassini/manipulation_experiments/287baa7767744cdfe966db02b3f5e203ca513980/imgs/real_gpd_example.gif


--------------------------------------------------------------------------------
/include/manipulation_class.hpp:
--------------------------------------------------------------------------------
  1 | /*****************************************************************
  2 |   Manipulation Class Definition
  3 | *****************************************************************/
  4 | 
  5 | #ifndef MANIPULATION_CLASS_HPP
  6 | #define MANIPULATION_CLASS_HPP
  7 | 
  8 | #include <ros/ros.h>
  9 | #include <boost/filesystem.hpp>
 10 | 
 11 | #include <moveit/move_group_interface/move_group_interface.h>
 12 | #include <moveit/planning_scene_interface/planning_scene_interface.h>
 13 | #include <moveit_msgs/DisplayRobotState.h>
 14 | #include <moveit_msgs/DisplayTrajectory.h>
 15 | #include <moveit_visual_tools/moveit_visual_tools.h>
 16 | #include <moveit_msgs/PickupAction.h>
 17 | 
 18 | #include <gpd_ros/GraspConfigList.h>
 19 | #include <gpd_ros/GraspConfig.h>
 20 | 
 21 | #include <std_msgs/Float32.h>
 22 | 
 23 | #include <geometry_msgs/Pose.h>
 24 | #include <geometry_msgs/Point.h>
 25 | #include <geometry_msgs/Vector3.h>
 26 | #include <geometry_msgs/PoseArray.h>
 27 | 
 28 | #include <tf2/LinearMath/Quaternion.h>
 29 | #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 30 | 
 31 | #include <control_msgs/GripperCommandActionGoal.h>
 32 | 
 33 | #include <fstream>
 34 | 
 35 | // Dynamic pointers for manipulation
 36 | typedef boost::shared_ptr<moveit::planning_interface::MoveGroupInterface> MoveGroupPtr;
 37 | typedef boost::shared_ptr<moveit::planning_interface::PlanningSceneInterface> PlanningScenePtr;
 38 | 
 39 | class Manipulation
 40 | {
 41 | private:
 42 |   ros::Subscriber grasp_config; // GPD msg subscriber
 43 | 
 44 |   const double pi = std::acos(-1);  // Pi constant
 45 | 
 46 |   // Move arm functions:
 47 |   void setJointGroup(double j0, double j1, double j2,
 48 |                      double j3, double j4, double j5, double j6);
 49 |   void getCurrentState();
 50 |   void move(std::vector<double>);
 51 | 
 52 | public:
 53 |   // data collection vars, for testing
 54 |   ros::Time begin;
 55 |   ros::Time end;
 56 | 
 57 |   // Gripper
 58 |   ros::Publisher gripper_command;
 59 |   ros::Publisher grasps_visualization_pub_;
 60 | 
 61 |   // Constructor
 62 |   Manipulation(ros::NodeHandle nodeHandle, std::string planning_group);
 63 | 
 64 |   // Moveit planning variables
 65 |   moveit::core::RobotStatePtr current_state;
 66 |   std::vector<double> joint_group_positions;
 67 |   std::string PLANNING_GROUP;
 68 |   PlanningScenePtr planning_scene_ptr;
 69 |   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 70 |   MoveGroupPtr move_group_ptr;
 71 |   moveit::planning_interface::MoveGroupInterface::Plan my_plan;
 72 | 
 73 |   // Hard coded arm goTo functions
 74 |   void goTop();
 75 |   void goRight();
 76 |   void goLeft();
 77 |   void goVertical();
 78 |   void goWait();
 79 |   void goPlace();
 80 |   void goPostGrasp();
 81 | 
 82 |   // Collision Objects
 83 |   void set_objects();
 84 |   void remove_objects();
 85 | 
 86 |   // Gripper commands, pick pipeline
 87 |   void closedGripper(trajectory_msgs::JointTrajectory &);
 88 |   void openGripper(trajectory_msgs::JointTrajectory &);
 89 | 
 90 |   // Gripper Action commands
 91 |   void close_gripper();
 92 |   void open_gripper();
 93 |   control_msgs::GripperCommandActionGoal gripper_cmd;
 94 | 
 95 |   /* GPD Functions */
 96 |   void callback(const gpd_ros::GraspConfigList msg);
 97 |   void path_planning();
 98 |   void set_target_pose();
 99 |   void plan_pose_goal();
100 |   void pick_and_place();
101 |   void pickup();
102 |   void place(float);
103 | 
104 |   // GPD pre/post grasp transform function  
105 |   std::vector<geometry_msgs::Pose> gpd_grasp_to_pose(gpd_ros::GraspConfig &);
106 | 
107 |   // GPD grasp atributes
108 |   bool getting_grasps = true; // Flag, used for multiple runs
109 |   bool pose_success;
110 |   gpd_ros::GraspConfigList grasp_candidates;
111 |   gpd_ros::GraspConfig grasp;
112 |   std_msgs::Float32 score;
113 | 
114 |   bool grabbed_object;  // Flags a good grasping plan
115 |   
116 |   // Planning variables
117 |   geometry_msgs::Pose target_pose;
118 |   geometry_msgs::Vector3 orientation;
119 |   geometry_msgs::Vector3 axis;
120 |   geometry_msgs::Point pose;
121 |   geometry_msgs::Point pose_sample;
122 |   geometry_msgs::Vector3 grasp_orientation;
123 |   moveit_msgs::CollisionObject collision_object;
124 |   tf2::Quaternion q;
125 | };
126 | 
127 | #endif


--------------------------------------------------------------------------------
/include/perception_class.hpp:
--------------------------------------------------------------------------------
 1 | /*****************************************************************
 2 |   Perception Class Definition
 3 | *****************************************************************/
 4 | 
 5 | #ifndef PERCEPTION_CLASS_HPP
 6 | #define PERCEPTION_CLASS_HPP
 7 | 
 8 | #include <boost/filesystem.hpp>
 9 | #include <ros/ros.h>
10 | 
11 | #include <tf/transform_listener.h>
12 | 
13 | #include <pcl_ros/point_cloud.h>
14 | #include <pcl_ros/transforms.h>
15 | 
16 | #include <pcl/point_types.h>
17 | #include <pcl/io/pcd_io.h>
18 | #include <pcl/filters/passthrough.h>
19 | #include <pcl/ModelCoefficients.h>
20 | 
21 | #include <pcl/sample_consensus/method_types.h>
22 | #include <pcl/sample_consensus/sac_model_plane.h>
23 | #include <pcl/sample_consensus/ransac.h>
24 | #include <pcl/sample_consensus/model_types.h>
25 | 
26 | #include <pcl/segmentation/sac_segmentation.h>
27 | 
28 | #include <pcl/filters/extract_indices.h>
29 | #include <pcl/filters/voxel_grid.h>
30 | 
31 | using namespace pcl;
32 | 
33 | // Smart pointer for transforms
34 | typedef boost::shared_ptr<tf::TransformListener> TransformListenerPtr;
35 | 
36 | class Perception
37 | {
38 | private:
39 |     ros::Publisher pub;
40 |     ros::Subscriber wrist_camera;
41 | 
42 | public:
43 |     Perception(ros::NodeHandle nodeHandle);
44 |     TransformListenerPtr transform_listener;
45 | 
46 |     PointCloud<PointXYZ> combined_cloud;
47 |     PointCloud<PointXYZ> left_cloud, right_cloud, top_cloud, current_cloud;
48 | 
49 |     void callback(const sensor_msgs::PointCloud2 msg);
50 |     void concatenate_clouds();
51 |     void publish();
52 | 
53 |     // Image functions
54 |     void snapshot_left();
55 |     void snapshot_right();
56 |     void snapshot_top();
57 | 
58 |     // Filter functions
59 |     void passthrough_filter(PointCloud<PointXYZ>::Ptr);
60 |     void segmentPlane(PointCloud<PointXYZ>::Ptr);
61 |     void voxelGrid(PointCloud<PointXYZ>::Ptr);
62 | };
63 | 
64 | #endif


--------------------------------------------------------------------------------
/launch/gpd_grasping_node.launch:
--------------------------------------------------------------------------------
 1 | <!--Launches GPD and the Pick/Place Node-->
 2 | 
 3 | <launch>
 4 |     <include file="$(find gpd_ros)/launch/gen3.launch"/>
 5 | 
 6 |     <group ns = "/my_gen3">
 7 |         <node pkg="uml_hri_nerve_pick_and_place" name="gpd_grasping_node" type="basic_manipulation_and_3d_perception" output="screen"/>
 8 |     </group>
 9 | 
10 | </launch>


--------------------------------------------------------------------------------
/launch/perception_test.launch:
--------------------------------------------------------------------------------
1 | <!--Launch Perception Unit Test-->
2 | <launch>
3 |     <group ns = "/my_gen3">
4 |         <node pkg="uml_hri_nerve_pick_and_place" name="pick_place_tests" type="pick_place_tests" output="screen"/>
5 |     </group>
6 | </launch>


--------------------------------------------------------------------------------
/launch/real_gen3_grasping.launch:
--------------------------------------------------------------------------------
 1 | <!--Launch file for using the acutal arm
 2 |         Launches GPD, Vision, and Pick/place node -->
 3 | <launch>
 4 | 
 5 |     <include file="$(find gpd_ros)/launch/gen3.launch"/>
 6 | 
 7 |     <include file="$(find kinova_vision)/launch/kinova_vision.launch">
 8 |         <arg name="device" value="192.168.1.10"/>
 9 |     </include>
10 | 
11 |     <group ns = "/my_gen3">
12 |         <node pkg="uml_hri_nerve_pick_and_place" name="gpd_grasping_node" type="basic_manipulation_and_3d_perception" output="screen"/>
13 |     </group>
14 | 
15 | </launch>


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>uml_hri_nerve_pick_and_place</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The uml_hri_nerve_pick_and_place package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="kyassini@example.com">Kevin Yassini</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/uml_hri_nerve_pick_and_place</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintainers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35 |   <!--   <depend>roscpp</depend> -->
36 |   <!--   Note that this is equivalent to the following: -->
37 |   <!--   <build_depend>roscpp</build_depend> -->
38 |   <!--   <exec_depend>roscpp</exec_depend> -->
39 |   <!-- Use build_depend for packages you need at compile time: -->
40 |   <!--   <build_depend>message_generation</build_depend> -->
41 |   <!-- Use build_export_depend for packages you need in order to build against this package: -->
42 |   <!--   <build_export_depend>message_generation</build_export_depend> -->
43 |   <!-- Use buildtool_depend for build tool packages: -->
44 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
45 |   <!-- Use exec_depend for packages you need at runtime: -->
46 |   <!--   <exec_depend>message_runtime</exec_depend> -->
47 |   <!-- Use test_depend for packages you need only for testing: -->
48 |   <!--   <test_depend>gtest</test_depend> -->
49 |   <!-- Use doc_depend for packages you need only for building documentation: -->
50 |   <!--   <doc_depend>doxygen</doc_depend> -->
51 |   <buildtool_depend>catkin</buildtool_depend>
52 |   <build_depend>pcl_conversions</build_depend>
53 |   <build_depend>pcl_ros</build_depend>
54 |   <build_depend>roscpp</build_depend>
55 |   <build_depend>sensor_msgs</build_depend>
56 |   <build_depend>geometry_msgs</build_depend>
57 |   <build_depend>libpcl-all-dev</build_depend>
58 |   <build_depend>moveit_core</build_depend>
59 |   <build_depend>moveit_visual_tools</build_depend>
60 |   <build_depend>moveit_ros_planning_interface</build_depend>
61 |   <build_depend>gpd_ros</build_depend>
62 |   <build_depend>control_msgs</build_depend>
63 | 
64 |   <build_export_depend>pcl_conversions</build_export_depend>
65 |   <build_export_depend>pcl_ros</build_export_depend>
66 |   <build_export_depend>roscpp</build_export_depend>
67 |   <build_export_depend>sensor_msgs</build_export_depend>
68 |   <build_export_depend>geometry_msgs</build_export_depend>
69 |   <build_export_depend>moveit_core</build_export_depend>
70 |   <build_export_depend>moveit_visual_tools</build_export_depend>
71 |   <build_export_depend>moveit_ros_planning_interface</build_export_depend>
72 | 
73 |   <exec_depend>libpcl-all</exec_depend>
74 |   <exec_depend>pcl_conversions</exec_depend>
75 |   <exec_depend>pcl_ros</exec_depend>
76 |   <exec_depend>roscpp</exec_depend>
77 |   <exec_depend>sensor_msgs</exec_depend>
78 |   <exec_depend>geometry_msgs</exec_depend>
79 |   <exec_depend>moveit_core</exec_depend>
80 |   <exec_depend>moveit_visual_tools</exec_depend>
81 |   <exec_depend>moveit_ros_planning_interface</exec_depend>
82 |   <exec_depend>gpd_ros</exec_depend>
83 |   <exec_depend>control_msgs</exec_depend>
84 | 
85 |   <!-- The export tag contains other, unspecified, tags -->
86 |   <export>
87 |     <!-- Other tools can request additional information be placed here -->
88 | 
89 |   </export>
90 | </package>


--------------------------------------------------------------------------------
/src/functions/gpd.cpp:
--------------------------------------------------------------------------------
  1 | /*****************************************************************
  2 |   GPD (Grasp Pose Detection) Specific Function Definitions
  3 | *****************************************************************/
  4 | 
  5 | #include "manipulation_class.hpp"
  6 | 
  7 | /* 
  8 |  * Get grasp msgs from gpd_ros package 
  9 |  */
 10 | void Manipulation::callback(const gpd_ros::GraspConfigList msg)
 11 | {
 12 |     ROS_WARN("Received Grasp Candidates");
 13 |     grasp_candidates = msg;
 14 |     this->getting_grasps = false;
 15 | }
 16 | 
 17 | /* 
 18 |  * Plan to each grasp candidate in order of score until a valid one is found
 19 |  * Assign grasp msg values to class variables
 20 |  */
 21 | void Manipulation::path_planning()
 22 | {
 23 |     int n = sizeof(this->grasp_candidates);
 24 |     if (n == 0)
 25 |     {
 26 |         ROS_ERROR("Grasp list is empty");
 27 |     }
 28 | 
 29 |     this->score = this->grasp_candidates.grasps[0].score;
 30 | 
 31 |     int i = 0;
 32 |     this->grabbed_object = 0;
 33 | 
 34 |     // Plan to the top grasps to see if they are possible
 35 |     for (i = 0; ((this->score.data > -150) && i < 10); i++)
 36 |     {
 37 |         this->grasp = this->grasp_candidates.grasps[i];
 38 |         this->pose = this->grasp.position;
 39 |         this->pose_sample = this->grasp.sample;
 40 |         this->orientation = this->grasp.approach;
 41 |         this->axis = this->grasp.axis;
 42 |         this->score = this->grasp.score;
 43 |         ROS_INFO("Grasp score: %f", this->score.data);
 44 | 
 45 |         set_target_pose();
 46 |         plan_pose_goal();
 47 | 
 48 |         if (this->pose_success)
 49 |         {
 50 |             this->grabbed_object = true;
 51 |             ROS_WARN("Plan success");
 52 |             break;
 53 |         }
 54 |     }
 55 | }
 56 | 
 57 | void Manipulation::set_target_pose()
 58 | {
 59 |     this->q.setRPY(this->orientation.x - pi, this->orientation.y, this->orientation.z);
 60 |     this->q.normalize();
 61 |     this->target_pose.orientation = tf2::toMsg(this->q);
 62 |     this->target_pose.position.x = this->pose_sample.x;
 63 |     this->target_pose.position.y = this->pose_sample.y;
 64 |     this->target_pose.position.z = this->pose_sample.z;
 65 | }
 66 | 
 67 | void Manipulation::plan_pose_goal()
 68 | {
 69 |     this->move_group_ptr->setPoseTarget(this->target_pose);
 70 |     this->move_group_ptr->setGoalPositionTolerance(0.01);
 71 |     this->move_group_ptr->setGoalOrientationTolerance(0.002);
 72 |     this->move_group_ptr->setPlanningTime(2);
 73 |     this->move_group_ptr->setNumPlanningAttempts(30);
 74 |     this->pose_success = (this->move_group_ptr->plan(this->my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
 75 | }
 76 | 
 77 | /* 
 78 |  * Working pick_and_place functions:
 79 |  *   Removed moveit! pick() pipeline. Instead moving directly to pre and post by
 80 |  *   calculating these positions using gpd_grasp_to_pose()
 81 |  */
 82 | void Manipulation::pickup()
 83 | {
 84 |     /* Log algorithim time and grasp score, for testing */
 85 |     //std::ofstream logfile;
 86 |     //logfile.open("/home/kyassini/Desktop/Testing_Logs/test.txt", std::ios_base::app);
 87 | 
 88 |     std::vector<geometry_msgs::Pose> poses;
 89 | 
 90 |     // Get pre, actual, and post grasps directly from GPD grasp msg
 91 |     poses = gpd_grasp_to_pose(grasp);
 92 | 
 93 |     //TODO: Add exception when a plan fails
 94 | 
 95 |     ROS_WARN_STREAM("Moving to pre-grasp...");
 96 |     this->target_pose.orientation = poses[0].orientation;
 97 |     this->target_pose.position.x = poses[0].position.x;
 98 |     this->target_pose.position.y = poses[0].position.y;
 99 |     this->target_pose.position.z = poses[0].position.z;
100 |     plan_pose_goal();
101 |     this->move_group_ptr->move();
102 | 
103 |     ROS_WARN_STREAM("Picking...");
104 |     this->target_pose.orientation = poses[0].orientation;
105 |     this->target_pose.position.x = poses[1].position.x;
106 |     this->target_pose.position.y = poses[1].position.y;
107 |     this->target_pose.position.z = poses[1].position.z;
108 |     plan_pose_goal();
109 |     this->move_group_ptr->move();
110 | 
111 |     this->end = ros::Time::now();    // Get times, for testing
112 | 
113 |     close_gripper();
114 | 
115 |     ROS_WARN_STREAM("Retreating...");
116 |     this->target_pose.orientation = poses[0].orientation;
117 |     this->target_pose.position.x = poses[0].position.x;
118 |     this->target_pose.position.y = poses[0].position.y;
119 |     this->target_pose.position.z = poses[0].position.z;
120 |     plan_pose_goal();
121 |     this->move_group_ptr->move();
122 | 
123 | 
124 |     //logfile << std::endl << this->end - this-> begin << " , " << grasp.score.data; // For testing
125 |     //logfile.close();
126 | }
127 | 
128 | void Manipulation::place(float z_dist)
129 | {
130 |     ROS_WARN_STREAM("Placing... ");
131 | 
132 |     // NOTE: All hard coded values, just places the object to the right of the arm base
133 | 
134 |     this->q.setRPY(-pi, 0, 0);
135 |     this->target_pose.orientation = tf2::toMsg(this->q);
136 |     this->target_pose.position.x = 0;
137 |     this->target_pose.position.y = -0.28;
138 |     this->target_pose.position.z = z_dist;
139 |     plan_pose_goal();
140 |     this->move_group_ptr->move();
141 | /*
142 |     this->q.setRPY(-pi, 0, 0);
143 |     this->target_pose.orientation = tf2::toMsg(this->q);
144 |     this->target_pose.position.x = 0;
145 |     this->target_pose.position.y = -0.28;
146 |     this->target_pose.position.z = z_dist - 0.15;
147 |     plan_pose_goal();
148 |     this->move_group_ptr->move();
149 | */
150 |     open_gripper();
151 | 
152 | /*
153 |     this->q.setRPY(-pi, 0, 0);
154 |     this->target_pose.orientation = tf2::toMsg(this->q);
155 |     this->target_pose.position.x = 0;
156 |     this->target_pose.position.y = -0.28;
157 |     this->target_pose.position.z = z_dist;
158 |     plan_pose_goal();
159 |     this->move_group_ptr->move();
160 |     */
161 | }
162 | 
163 | /* 
164 |  * Given GPD grasp msg, calculate the pre and post grasps along the approach vector
165 |  * Modified from: https://gist.github.com/tkelestemur/60401be131344dae98671b95d46060f8
166 |  */
167 | std::vector<geometry_msgs::Pose> Manipulation::gpd_grasp_to_pose(gpd_ros::GraspConfig &grasp)
168 | {
169 |     geometry_msgs::Pose pose;
170 |     geometry_msgs::Pose pre;
171 |     geometry_msgs::Pose post;
172 |     std::vector<geometry_msgs::Pose> poses;
173 | 
174 |     tf::StampedTransform tf_base_odom;
175 |     tf::Quaternion q_grasp_base;
176 |     tf::TransformListener listener_;
177 | 
178 |     tf::Matrix3x3 orientation(-grasp.axis.x, grasp.binormal.x, grasp.approach.x,
179 |                               -grasp.axis.y, grasp.binormal.y, grasp.approach.y,
180 |                               -grasp.axis.z, grasp.binormal.z, grasp.approach.z);
181 | 
182 |     tf::Vector3 tr_grasp_base(grasp.position.x, grasp.position.y, grasp.position.z);
183 |     tf::Transform tf_grasp_base(orientation, tr_grasp_base);
184 | 
185 |     // Useless transform?
186 |     try
187 |     {
188 |         listener_.waitForTransform("base_link", "base_link", ros::Time(0), ros::Duration(3.0));
189 |         listener_.lookupTransform("base_link", "base_link", ros::Time(0), tf_base_odom);
190 |     }
191 |     catch (tf::TransformException ex)
192 |     {
193 |         ROS_ERROR("%s", ex.what());
194 |     }
195 | 
196 |     // Find grasp pose
197 |     tf::Transform tf_grasp_odom = tf_base_odom * tf_grasp_base;
198 |     tf::poseTFToMsg(tf_grasp_odom, pose);
199 | 
200 |     // Find pre-grasp pose
201 |     //                                      CHANGE THIS VALUE TO EDIT PREGRASP HEIGHT |
202 |     //                                                                                V              
203 |     tf::Transform tf_pregrasp_odom_(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, -0.13));
204 |     tf::Transform tf_pregrasp_odom = tf_grasp_odom * tf_pregrasp_odom_;
205 |     tf::poseTFToMsg(tf_pregrasp_odom, pre);
206 | 
207 |     tf::StampedTransform tf_hand_odom;
208 |     try
209 |     {
210 |         listener_.waitForTransform("base_link", "end_effector_link", ros::Time(0), ros::Duration(3.0));
211 |         listener_.lookupTransform("base_link", "end_effector_link", ros::Time(0), tf_hand_odom);
212 |     }
213 |     catch (tf::TransformException ex)
214 |     {
215 |         ROS_ERROR("%s", ex.what());
216 |     }
217 | 
218 |     Eigen::VectorXf grasp_position(2);
219 |     Eigen::VectorXf hand_position(2);
220 | 
221 |     grasp_position << grasp.position.x, grasp.position.y;
222 |     hand_position << tf_hand_odom.getOrigin().getX(), tf_hand_odom.getOrigin().getY();
223 |     float distance = (grasp_position - hand_position).squaredNorm();
224 | 
225 |     //tf::Quaternion orientation_quat;
226 |     //orientation.getRotation(orientation_quat);
227 |     //tf::quaternionTFToMsg(orientation_quat, pose.orientation);
228 | 
229 |     //pose.position = grasp.position;
230 | 
231 |     poses.push_back(pre);
232 |     poses.push_back(pose);
233 |     poses.push_back(post);
234 | 
235 |     return poses;
236 | }


--------------------------------------------------------------------------------
/src/functions/manipulation_class.cpp:
--------------------------------------------------------------------------------
  1 | /*****************************************************************
  2 |   Manipulation Function Definitions
  3 | *****************************************************************/
  4 | 
  5 | #include "manipulation_class.hpp"
  6 | 
  7 | /* 
  8 |  * Manipulation constructor, init planning_group 
  9 |  */
 10 | Manipulation::Manipulation(ros::NodeHandle nodeHandle, std::string planning_group)
 11 | {
 12 |     PLANNING_GROUP = planning_group;
 13 |     grasp_config = nodeHandle.subscribe("/detect_grasps/clustered_grasps", 1, &Manipulation::callback, this); // Subscribe to GPD output
 14 |     this->gripper_command = nodeHandle.advertise<control_msgs::GripperCommandActionGoal>("robotiq_2f_85_gripper_controller/gripper_cmd/goal", 10);
 15 |     grasps_visualization_pub_ = nodeHandle.advertise<geometry_msgs::PoseArray>("grasps_visualization", 10);
 16 | }
 17 | 
 18 | /* 
 19 |  * Get robot state and move to specified joint_group_position: 
 20 |  */
 21 | void Manipulation::getCurrentState()
 22 | {
 23 | 
 24 |     const robot_state::JointModelGroup *joint_model_group =
 25 |         move_group_ptr->getCurrentState()->getJointModelGroup(PLANNING_GROUP);
 26 |     current_state = move_group_ptr->getCurrentState();
 27 |     current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
 28 | }
 29 | 
 30 | /* 
 31 |  * Move to specigied joint values: 
 32 |  */
 33 | void Manipulation::move(std::vector<double>)
 34 | {
 35 |     move_group_ptr->setJointValueTarget(joint_group_positions);
 36 |     move_group_ptr->move();
 37 | }
 38 | 
 39 | /* 
 40 |  * Set joint values: 
 41 |  */
 42 | void Manipulation::setJointGroup(double j0, double j1, double j2, double j3, double j4, double j5, double j6)
 43 | {
 44 |     joint_group_positions[0] = j0; // base
 45 |     joint_group_positions[1] = j1;
 46 |     joint_group_positions[2] = j2;
 47 |     joint_group_positions[3] = j3;
 48 |     joint_group_positions[4] = j4;
 49 |     joint_group_positions[5] = j5;
 50 |     joint_group_positions[6] = j6; // gripper
 51 | }
 52 | 
 53 | /* 
 54 |  * Preset arm positions: 
 55 |  */
 56 | void Manipulation::goRight()
 57 | {
 58 |     getCurrentState();
 59 |     ROS_INFO("Moving to Right Position");
 60 |     setJointGroup(pi / 1.8, pi / 3, -pi / 3, pi / 2, 0, pi / 3, pi / 7);
 61 |     move(joint_group_positions);
 62 | }
 63 | 
 64 | void Manipulation::goLeft()
 65 | {
 66 |     getCurrentState();
 67 |     ROS_INFO("Moving to Left Position");
 68 |     setJointGroup(-pi / 1.8, pi / 3, pi / 3, pi / 2, 0, pi / 3, 2.8);
 69 |     move(joint_group_positions);
 70 | }
 71 | 
 72 | void Manipulation::goVertical()
 73 | {
 74 |     getCurrentState();
 75 |     ROS_INFO("Moving to Vertical Position");
 76 |     setJointGroup(0, 0, 0, 0, 0, 0, 0);
 77 |     move(joint_group_positions);
 78 | }
 79 | 
 80 | void Manipulation::goTop()
 81 | {
 82 |     getCurrentState();
 83 |     ROS_INFO("Moving to Top Position");
 84 |     setJointGroup(0.043, -0.1824, -0.0133, 2.208, -0.0188, 0.7828, -1.524);
 85 |     move(joint_group_positions);
 86 | }
 87 | 
 88 | void Manipulation::goPostGrasp()
 89 | {
 90 |     getCurrentState();
 91 |     ROS_INFO("Moving to Post Grasp Position");
 92 |     setJointGroup(-0.06, 0.4309, 0.0458, 2.1915, -0.0325, -1.043, -1.567);
 93 |     move(joint_group_positions);
 94 | }
 95 | 
 96 | void Manipulation::goPlace()
 97 | {
 98 |     getCurrentState();
 99 |     ROS_INFO("Moving to Place Position");
100 |     setJointGroup(pi, pi / 4, 0, pi / 4, 0, pi / 2, -pi / 2);
101 |     move(joint_group_positions);
102 | }
103 | 
104 | void Manipulation::goWait()
105 | {
106 |     getCurrentState();
107 |     ROS_INFO("Moving to Waiting Position");
108 |     setJointGroup(0, pi / 5, 0, pi / 2, 0, -pi / 5, -pi / 2);
109 |     move(joint_group_positions);
110 | }
111 | 
112 | /* 
113 |  * Set objects for collision detection: 
114 |  */
115 | void Manipulation::set_objects()
116 | {
117 |     std::vector<moveit_msgs::CollisionObject> collision_objects;
118 |     collision_objects.resize(1);
119 | 
120 |     collision_objects[0].id = "object";
121 |     collision_objects[0].header.frame_id = "base_link"; //world for NERVE workstation
122 | 
123 |     // Define the primitive and its dimensions
124 |     collision_objects[0].primitives.resize(1);
125 |     collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].CYLINDER;
126 |     collision_objects[0].primitives[0].dimensions.resize(3);
127 |     collision_objects[0].primitives[0].dimensions[0] = 0.01; //h
128 |     collision_objects[0].primitives[0].dimensions[1] = 0.01; //r
129 | 
130 |     // Define the pose of the object
131 |     collision_objects[0].primitive_poses.resize(1);
132 |     collision_objects[0].primitive_poses[0].position.x = this->pose.x;
133 |     collision_objects[0].primitive_poses[0].position.y = this->pose.y;
134 |     collision_objects[0].primitive_poses[0].position.z = this->pose.z;
135 |     //collision_objects[0].operation = collision_objects[0].ADD;
136 | 
137 |     // Define the primitive and its dimensions.
138 |     collision_objects[0].id = "table";
139 |     collision_objects[0].header.frame_id = "base_link"; //world for NERVE workstation
140 |     collision_objects[0].primitives.resize(1);
141 |     collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
142 |     collision_objects[0].primitives[0].dimensions.resize(3);
143 |     collision_objects[0].primitives[0].dimensions[0] = 1;
144 |     collision_objects[0].primitives[0].dimensions[1] = 1;
145 |     collision_objects[0].primitives[0].dimensions[2] = 0;
146 | 
147 |     // Define the pose of the table.
148 |     collision_objects[0].primitive_poses.resize(1);
149 |     collision_objects[0].primitive_poses[0].position.x = 0.4;
150 |     collision_objects[0].primitive_poses[0].position.y = 0;
151 |     collision_objects[0].primitive_poses[0].position.z = -0.04;
152 | 
153 |     collision_objects[0].operation = collision_objects[0].ADD;
154 | 
155 |     this->planning_scene_ptr->applyCollisionObjects(collision_objects);
156 | }
157 | 
158 | /* 
159 |  * Remove objects from collision detection: 
160 |  */
161 | void Manipulation::remove_objects()
162 | {
163 |     std::vector<std::string> object_ids;
164 |     object_ids.push_back("object");
165 |     object_ids.push_back("table");
166 |     this->planning_scene_ptr->removeCollisionObjects(object_ids);
167 | }
168 | 
169 | /* 
170 |  * Gripper Action Commands, TODO: replace with gripper planning group! 
171 |  */
172 | 
173 | void Manipulation::close_gripper()
174 | {
175 |     this->gripper_cmd.goal.command.position = 0.70;
176 |     gripper_command.publish(gripper_cmd);
177 |     ROS_WARN("Closing gripper...");
178 | }
179 | 
180 | void Manipulation::open_gripper()
181 | {
182 |     this->gripper_cmd.goal.command.position = 0;
183 |     gripper_command.publish(gripper_cmd);
184 |     ROS_WARN("Opening gripper...");
185 | }
186 | 
187 | /* 
188 |  * Working Gripper Commands for the pick() pipeline, not currently used
189 |  */
190 | 
191 | /*
192 | void Manipulation::closedGripper(trajectory_msgs::JointTrajectory &posture)
193 | {
194 |     //Add finger joints
195 |     posture.joint_names.resize(6);
196 |     posture.joint_names[0] = "left_inner_finger_joint";
197 |     posture.joint_names[1] = "right_inner_knuckle_joint";
198 |     posture.joint_names[2] = "finger_joint";
199 |     posture.joint_names[3] = "right_inner_finger_joint";
200 |     posture.joint_names[4] = "left_inner_knuckle_joint";
201 |     posture.joint_names[5] = "right_outer_knuckle_joint";
202 | 
203 |     // Set them as closed.
204 |     posture.points.resize(1);
205 |     posture.points[0].positions.resize(6);
206 |     posture.points[0].positions[0] = 0.8;
207 |     posture.points[0].positions[1] = 0.8;
208 |     posture.points[0].positions[2] = 0.8;
209 |     posture.points[0].positions[3] = -0.8;
210 |     posture.points[0].positions[4] = 0.8;
211 |     posture.points[0].positions[5] = 0.8;
212 |     posture.points[0].time_from_start = ros::Duration(0.5);
213 | }
214 | 
215 | void Manipulation::openGripper(trajectory_msgs::JointTrajectory &posture)
216 | {
217 | 
218 |     posture.joint_names.resize(6);
219 |     posture.joint_names[0] = "left_inner_finger_joint";
220 |     posture.joint_names[1] = "right_inner_knuckle_joint";
221 |     posture.joint_names[2] = "finger_joint";
222 |     posture.joint_names[3] = "right_inner_finger_joint";
223 |     posture.joint_names[4] = "left_inner_knuckle_joint";
224 |     posture.joint_names[5] = "right_outer_knuckle_joint";
225 | 
226 |     posture.points.resize(1);
227 |     posture.points[0].positions.resize(6);
228 |     posture.points[0].positions[0] = 0;
229 |     posture.points[0].positions[1] = 0;
230 |     posture.points[0].positions[2] = 0;
231 |     posture.points[0].positions[3] = 0;
232 |     posture.points[0].positions[4] = 0;
233 |     posture.points[0].positions[5] = 0;
234 | 
235 |     posture.points[0].time_from_start = ros::Duration(0.5);
236 | }
237 | */


--------------------------------------------------------------------------------
/src/functions/perception_class.cpp:
--------------------------------------------------------------------------------
  1 | /*****************************************************************
  2 |   Perception Function Definitions
  3 | *****************************************************************/
  4 | 
  5 | #include "perception_class.hpp"
  6 | 
  7 | /* 
  8 |  * Perception constructor, init cloud publisher and camera subscriber 
  9 |  */
 10 | Perception::Perception(ros::NodeHandle nh)
 11 | {
 12 |     pub = nh.advertise<sensor_msgs::PointCloud2>("/cloud_combined", 1);
 13 |     wrist_camera = nh.subscribe("/camera/depth/points", 1, &Perception::callback, this);
 14 | }
 15 | 
 16 | /* 
 17 |  * Cloud callback, apply transform
 18 |  */
 19 | void Perception::callback(const sensor_msgs::PointCloud2 msg)
 20 | {
 21 |     pcl::fromROSMsg(msg, this->current_cloud);
 22 | 
 23 |     ros::Time stamp = ros::Time(0);
 24 |     tf::StampedTransform transform;
 25 | 
 26 |     /* 
 27 |      * NOTE: base_link transform for sim and basic workstation testing,
 28 |      * world transform for NERVE workstation 
 29 |      */
 30 | 
 31 |     pcl_conversions::toPCL(stamp, this->current_cloud.header.stamp);
 32 |     try
 33 |     {
 34 |         this->transform_listener->waitForTransform("base_link", this->current_cloud.header.frame_id, stamp, ros::Duration(10.0));
 35 |         this->transform_listener->lookupTransform("base_link", this->current_cloud.header.frame_id, stamp, transform);
 36 |     }
 37 |     catch (tf::TransformException ex)
 38 |     {
 39 |         ROS_ERROR("%s", ex.what());
 40 |     }
 41 |     pcl_ros::transformPointCloud("base_link", this->current_cloud, this->current_cloud, *this->transform_listener);
 42 | }
 43 | 
 44 | /* 
 45 |  * Publish final cloud as ROS msg for grasp detection with GPD
 46 |  */
 47 | void Perception::publish()
 48 | {
 49 |     ROS_WARN("Publishing combined cloud...");
 50 |     sensor_msgs::PointCloud2 cloud;
 51 |     toROSMsg(this->combined_cloud, cloud);
 52 |     pub.publish(cloud);
 53 | }
 54 | 
 55 | /* 
 56 |  * Combine clouds and filter, after being transfromed
 57 |  */
 58 | void Perception::concatenate_clouds()
 59 | {
 60 |     //TODO: add exception when pointcloud is empty...
 61 | 
 62 |     PointCloud<PointXYZ>::Ptr temp_cloud(new PointCloud<PointXYZ>);
 63 | 
 64 |     // Combine clouds, currently only the top view is used so concatenating is not necessary!
 65 |     *temp_cloud = this->top_cloud;
 66 |     //*temp_cloud += this->right_cloud;
 67 |     //*temp_cloud += this->left_cloud;
 68 |     //*temp_cloud += this->top_cloud;
 69 | 
 70 |     // Code for loading saved pointcloud, for testing purposes
 71 |     /*
 72 |     if (pcl::io::loadPCDFile("/home/kyassini/Desktop/test_cloud.pcd", *temp_cloud) == -1)
 73 |     {
 74 |         ROS_ERROR("Failed to open");
 75 |     }
 76 |     ROS_INFO_STREAM("Loaded Point Clound");
 77 |     */
 78 | 
 79 |     // Apply series of filtering steps to reduce GPD grasp candidate time and isolate object
 80 |     passthrough_filter(temp_cloud);
 81 |     segmentPlane(temp_cloud);
 82 |     voxelGrid(temp_cloud);
 83 | 
 84 |     this->combined_cloud = *temp_cloud;
 85 | 
 86 |     // Optional: save pointcloud to a file
 87 |     //pcl::io::savePCDFileASCII("test_cloud.pcd", this->combined_cloud);
 88 | 
 89 |     publish();
 90 | }
 91 | 
 92 | /*
 93 |  * Snapshot functions for each angle, takes image from wrist camera
 94 |  */
 95 | void Perception::snapshot_left()
 96 | {
 97 |     this->left_cloud = this->current_cloud;
 98 |     ROS_WARN("Left snapshot complete");
 99 | }
100 | void Perception::snapshot_right()
101 | {
102 |     this->right_cloud = this->current_cloud;
103 |     ROS_WARN("Right snapshot complete");
104 | }
105 | void Perception::snapshot_top()
106 | {
107 |     this->top_cloud = this->current_cloud;
108 |     ROS_WARN("Top snapshot complete");
109 | }
110 | 
111 | /*
112 |  * Filtering steps:
113 |  *      Passthrough: isolates defined region (https://pcl.readthedocs.io/en/latest/passthrough.html#passthrough)
114 |  *      segmentPlane: removes table from cloud (https://pcl.readthedocs.io/en/latest/planar_segmentation.html#planar-segmentation)
115 |  *      voxelGrid: down sample cloud (https://pcl.readthedocs.io/en/latest/voxel_grid.html#voxelgrid)
116 |  */
117 | 
118 | void Perception::passthrough_filter(PointCloud<PointXYZ>::Ptr cloud)
119 | {
120 |     
121 |     /*
122 |     // WORKSTATION PassThrough VALUES:
123 |     PassThrough<PointXYZ> pass_x;
124 |     pass_x.setInputCloud(cloud);
125 |     pass_x.setFilterFieldName("x");
126 |     pass_x.setFilterLimits(-0.3, 0.3);
127 |     pass_x.filter(*cloud);
128 | 
129 |     PassThrough<PointXYZ> pass_y;
130 |     pass_y.setInputCloud(cloud);
131 |     pass_y.setFilterFieldName("y");
132 |     pass_y.setFilterLimits(-0.15, 0.2);
133 |     pass_y.filter(*cloud);
134 | 
135 |     PassThrough<PointXYZ> pass_z;
136 |     pass_z.setInputCloud(cloud);
137 |     pass_z.setFilterFieldName("z");
138 |     pass_z.setFilterLimits(0.2, 0.5);
139 |     pass_z.filter(*cloud);
140 |     */
141 |     
142 |     
143 |     // Kev's workstation/No sim workstation PassThrough values:
144 |     PassThrough<PointXYZ> pass_z;
145 |     pass_z.setInputCloud(cloud);
146 |     pass_z.setFilterFieldName("z");
147 |     pass_z.setFilterLimits(-0.1, 0.2); //0, 0.2 //0.03, 0.2
148 |     pass_z.filter(*cloud);
149 |     
150 |     
151 | }
152 | 
153 | void Perception::segmentPlane(PointCloud<PointXYZ>::Ptr cloud)
154 | {
155 |     pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
156 |     pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
157 |     pcl::SACSegmentation<pcl::PointXYZ> seg;
158 | 
159 |     seg.setOptimizeCoefficients(true);
160 |     seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
161 |     seg.setMaxIterations(1000);
162 | 
163 |     Eigen::Vector3f axis = Eigen::Vector3f(0.0, 0.0, 1.0);
164 |     seg.setAxis(axis);
165 | 
166 |     seg.setMethodType(pcl::SAC_RANSAC);
167 |     seg.setMaxIterations(1000);
168 |     seg.setDistanceThreshold(0.01);
169 |     seg.setEpsAngle(30.0f * (3.14159 / 180.0f)); // 30 degree tolerance
170 | 
171 |     seg.setInputCloud(cloud);
172 |     seg.segment(*inliers, *coefficients);
173 | 
174 |     pcl::ExtractIndices<PointXYZ> extract_indices;
175 |     extract_indices.setInputCloud(cloud);
176 |     extract_indices.setIndices(inliers);
177 |     extract_indices.setNegative(true);
178 |     extract_indices.filter(*cloud);
179 | }
180 | 
181 | void Perception::voxelGrid(PointCloud<PointXYZ>::Ptr cloud)
182 | {
183 |     pcl::VoxelGrid<pcl::PointXYZ> sor;
184 |     sor.setInputCloud(cloud);
185 |     sor.setLeafSize(0.01f, 0.01f, 0.01f);
186 |     sor.filter(*cloud);
187 | }


--------------------------------------------------------------------------------
/src/nodes/gpd_grasping_node.cpp:
--------------------------------------------------------------------------------
 1 | /*****************************************************************
 2 |   Main Node for Gen3 GPD grasping
 3 | *****************************************************************/
 4 | 
 5 | #include "manipulation_class.hpp"
 6 | #include "perception_class.hpp"
 7 | 
 8 | int main(int argc, char **argv)
 9 | {
10 |   ros::init(argc, argv, "gpd_grasping_node");
11 | 
12 |   ros::NodeHandle nh;
13 |   ros::AsyncSpinner spinner(4);
14 |   spinner.start();
15 |   ros::Duration(1.0).sleep();
16 | 
17 |   std::string planning_group = "arm"; // Gen3 planning group
18 | 
19 |   Manipulation manipulation(nh, planning_group);
20 |   Perception perception(nh);
21 | 
22 |   // Point cloud transform listener
23 |   perception.transform_listener = TransformListenerPtr(
24 |       new tf::TransformListener());
25 | 
26 |   // Manipulation variables/setup
27 |   manipulation.planning_scene_ptr = PlanningScenePtr(
28 |       new moveit::planning_interface::PlanningSceneInterface());
29 |   manipulation.move_group_ptr = MoveGroupPtr(
30 |       new moveit::planning_interface::MoveGroupInterface(manipulation.PLANNING_GROUP));
31 | 
32 |   manipulation.move_group_ptr->setPlanningTime(45.0);
33 |   //manipulation.move_group_ptr->setMaxVelocityScalingFactor(0.5); // Changes arm speed
34 |   manipulation.move_group_ptr->setPlannerId("RRT");
35 | 
36 |   while (ros::ok())
37 |   {
38 |     // Detach objects potentially left over from previous run
39 |     manipulation.move_group_ptr->detachObject("object");
40 |     manipulation.remove_objects();
41 | 
42 |     // Ensure gripper is open
43 |     manipulation.open_gripper();
44 | 
45 |     // Take top snapshot, left and right angles also possible but not currently used
46 |     manipulation.goTop();
47 |     ros::Duration(5).sleep();
48 |     perception.snapshot_top();
49 |     ros::Duration(1).sleep();
50 |     /*
51 |     manipulation.goRight();
52 |     ros::Duration(3).sleep();
53 |     perception.snapshot_right();
54 |     ros::Duration(1).sleep();
55 | 
56 |     manipulation.goLeft();
57 |     ros::Duration(3).sleep();
58 |     perception.snapshot_left();
59 |     ros::Duration(1).sleep();
60 |     */
61 | 
62 |     // Combine clouds and publish, for now only one cloud is used (top)
63 |     perception.concatenate_clouds();
64 |     manipulation.begin = ros::Time::now();
65 | 
66 |     // Set collision objects (grasping object, table)
67 |     //manipulation.set_objects();
68 | 
69 |     // Wait to get grasps grom GPD
70 |     while (manipulation.getting_grasps)
71 |     {
72 |     }
73 | 
74 |     // Main grasping ppanning code
75 |     manipulation.path_planning();
76 | 
77 |     // Pickup the object
78 |     if (manipulation.grabbed_object)
79 |     {
80 |       manipulation.pickup();
81 |       //manipulation.place(0.25); // Place at inputed height offset
82 |     }
83 |     ros::Duration(1.0).sleep();
84 | 
85 |     // Test how well the arm has grasped the object
86 |     manipulation.goPostGrasp();
87 |     ros::Duration(3.0).sleep();
88 | 
89 |     // Go back to the top position for repeating, get a new grasp
90 |     manipulation.goTop();
91 |     manipulation.getting_grasps = true;
92 |   }
93 | 
94 |   ros::waitForShutdown();
95 | 
96 |   return 0;
97 | }


--------------------------------------------------------------------------------
/src/nodes/perception_test.cpp:
--------------------------------------------------------------------------------
 1 | /*****************************************************************
 2 |   Unit Test to make sure perception code & filters are working
 3 | *****************************************************************/
 4 | 
 5 | #include "manipulation_class.hpp"
 6 | #include "perception_class.hpp"
 7 | 
 8 | int main(int argc, char **argv)
 9 | {
10 |     ros::init(argc, argv, "pick_place_test");
11 | 
12 |     ros::NodeHandle nh;
13 |     ros::AsyncSpinner spinner(4);
14 |     spinner.start();
15 |     ros::Duration(1.0).sleep();
16 | 
17 |     std::string planning_group = "arm"; // Gen3 planning group
18 | 
19 |     Perception perception(nh);
20 | 
21 |     perception.transform_listener = TransformListenerPtr(
22 |         new tf::TransformListener());
23 | 
24 |     // Take a snapshot, concatenate, filter, and then publish
25 |     while (ros::ok())
26 |     {
27 |         perception.snapshot_top();
28 |         ros::Duration(1).sleep();
29 |         perception.concatenate_clouds();
30 |         ros::Duration(2).sleep();
31 |     }
32 | 
33 |     ros::waitForShutdown();
34 | 
35 |     return 0;
36 | }


--------------------------------------------------------------------------------