separate openrave independent part query_grasp

cob_grasp_generation/CMakeLists.txt

@@ -26,6 +26,14 @@ install(DIRECTORY files launch
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 
-install(PROGRAMS scripts/cob_grasp_generation_action_server.py scripts/generate_grasps_client.py scripts/query_grasps_client.py scripts/show_grasps_client.py
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts
+install(PROGRAMS
+        scripts/generate_grasps_client.py
+        scripts/generate_grasps_server.py
+        scripts/query_grasps_client.py
+        scripts/query_grasps_server.py
+        scripts/show_grasps_or_client.py
+        scripts/show_grasps_or_server.py
+        scripts/show_grasps_rviz_client.py
+        scripts/show_grasps_rviz_server.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

cob_grasp_generation/action/QueryGrasps.action

@@ -3,6 +3,7 @@ string object_name
 
 #gripper used on the robot
 string gripper_type
+string gripper_side
 
 #only return grasp with given id
 #note: cannot be used to return first grasp (id = 0)

cob_grasp_generation/action/ShowGrasps.action

@@ -3,6 +3,7 @@ string object_name
 
 #gripper used on the robot
 string gripper_type
+string gripper_side
 
 int32 grasp_id
 bool sort_by_quality

cob_grasp_generation/files/database/corn_flakes_package/sdhx_corn_flakes_package.csv

@@ -0,0 +1,6 @@
+id,object,gripper_finger_1_joint,gripper_finger_2_joint,direction,qw,qx,qy,qz,pos-x,pos-y,pos-z,eps_l1,vol_l1,
+0,corn_flakes_package,-0.7,0.1,SIDE,0.5,0.5,-0.5,0.5,10,206,-10,0.01,0.0005,0
+1,corn_flakes_package,-0.7,0.1,SIDE,0.5,0.5,-0.5,0.5,10,206,-5,0.01,0.0005,1
+2,corn_flakes_package,-0.7,0.1,SIDE,0.5,0.5,-0.5,0.5,10,206,0,0.01,0.0005,2
+3,corn_flakes_package,-0.7,0.1,SIDE,0.5,0.5,-0.5,0.5,10,206,5,0.01,0.0005,3
+4,corn_flakes_package,-0.7,0.1,SIDE,0.5,0.5,-0.5,0.5,10,206,10,0.01,0.0005,4

cob_grasp_generation/files/database/pringles/sdhx_pringles.csv

@@ -1,25 +1,9 @@
-id,object,gripper_left_finger_1_joint,gripper_left_finger_2_joint,direction,qw,qx,qy,qz,pos-x,pos-y,pos-z,eps_l1,vol_l1,
-0,pringles,0.1,-0.9,SIDE,0.5,-0.5,-0.5,-0.5,0,-14,6,0.01,0.0005,0
-1,pringles,0.1,-0.9,SIDE,0.653,-0.271,-0.653,-0.271,9.956,-9.842,6,0.01,0.0005,1
-2,pringles,0.1,-0.9,SIDE,0.707,0,-0.707,0,14,0,6,0.01,0.0005,2
-3,pringles,0.1,-0.9,SIDE,0.653,0.271,-0.653,0.271,9.63,10.162,6,0.01,0.0005,3
-4,pringles,0.1,-0.9,SIDE,0.5,0.5,-0.5,0.5,0,14,6,0.01,0.0005,4
-5,pringles,0.1,-0.9,SIDE,0.271,0.653,-0.271,0.653,-10.106,9.688,6,0.01,0.0005,5
-6,pringles,0.1,-0.9,SIDE,0,0.707,0,0.707,-14,0,6,0.01,0.0005,6
-7,pringles,0.1,-0.9,SIDE,-0.271,0.653,0.271,0.653,-9.463,-10.317,6,0.01,0.0005,7
-8,pringles,0.1,-0.9,SIDE,0.5,-0.5,-0.5,-0.5,0,-14,0,0.01,0.0005,8
-9,pringles,0.1,-0.9,SIDE,0.653,-0.271,-0.653,-0.271,9.956,-9.842,0,0.01,0.0005,9
-10,pringles,0.1,-0.9,SIDE,0.707,0,-0.707,0,14,0,0,0.01,0.0005,10
-11,pringles,0.1,-0.9,SIDE,0.653,0.271,-0.653,0.271,9.63,10.162,0,0.01,0.0005,11
-12,pringles,0.1,-0.9,SIDE,0.5,0.5,-0.5,0.5,0,14,0,0.01,0.0005,12
-13,pringles,0.1,-0.9,SIDE,0.271,0.653,-0.271,0.653,-10.106,9.688,0,0.01,0.0005,13
-14,pringles,0.1,-0.9,SIDE,0,0.707,0,0.707,-14,0,0,0.01,0.0005,14
-15,pringles,0.1,-0.9,SIDE,-0.271,0.653,0.271,0.653,-9.463,-10.317,0,0.01,0.0005,15
-16,pringles,0.1,-0.9,SIDE,0.5,-0.5,-0.5,-0.5,0,-14,-6,0.01,0.0005,16
-17,pringles,0.1,-0.9,SIDE,0.653,-0.271,-0.653,-0.271,9.956,-9.842,-6,0.01,0.0005,17
-18,pringles,0.1,-0.9,SIDE,0.707,0,-0.707,0,14,0,-6,0.01,0.0005,18
-19,pringles,0.1,-0.9,SIDE,0.653,0.271,-0.653,0.271,9.63,10.162,-6,0.01,0.0005,19
-20,pringles,0.1,-0.9,SIDE,0.5,0.5,-0.5,0.5,0,14,-6,0.01,0.0005,20
-21,pringles,0.1,-0.9,SIDE,0.271,0.653,-0.271,0.653,-10.106,9.688,-6,0.01,0.0005,21
-22,pringles,0.1,-0.9,SIDE,0,0.707,0,0.707,-14,0,-6,0.01,0.0005,22
-23,pringles,0.1,-0.9,SIDE,-0.271,0.653,0.271,0.653,-9.463,-10.317,-6,0.01,0.0005,23
+id,object,gripper_finger_1_joint,gripper_finger_2_joint,direction,qw,qx,qy,qz,pos-x,pos-y,pos-z,eps_l1,vol_l1,
+0,pringles,-0.3,0.1,SIDE,0.5,-0.5,-0.5,-0.5,0,-140,50,0.01,0.0005,0
+1,pringles,-0.3,0.1,SIDE,0.65328,-0.2706,-0.65328,-0.2706,100,-100,50,0.01,0.0005,1
+2,pringles,-0.3,0.1,SIDE,0.70711,0,-0.70711,0,140,0,50,0.01,0.0005,2
+3,pringles,-0.3,0.1,SIDE,0.65328,0.2706,-0.65328,0.2706,100,100,50,0.01,0.0005,3
+4,pringles,-0.3,0.1,SIDE,0.5,0.5,-0.5,0.5,0,140,50,0.01,0.0005,4
+5,pringles,-0.3,0.1,SIDE,0.2706,0.65328,-0.2706,0.65328,-100,100,50,0.01,0.0005,5
+6,pringles,-0.3,0.1,SIDE,0,0.70711,0,0.70711,-140,0,50,0.01,0.0005,6
+7,pringles,-0.3,0.1,SIDE,-0.2706,0.65328,0.2706,0.65328,-90,-100,50,0.01,0.0005,7

cob_grasp_generation/files/database/yellowsaltcube/sdhx_yellowsaltcube.csv

@@ -1,9 +1,9 @@
-id,object,gripper_left_finger_1_joint,gripper_left_finger_2_joint,direction,qw,qx,qy,qz,pos-x,pos-y,pos-z,eps_l1,vol_l1,
-0,yellowsaltcube,0.1,-0.9,SIDE,0.707,0,0.707,0,-22,0,10,0.01,0.0005,0
-1,yellowsaltcube,0.1,-0.9,SIDE,0.653,-0.271,0.653,0.271,-14,-17,10,0.01,0.0005,1
-2,yellowsaltcube,0.1,-0.9,SIDE,0.5,-0.5,0.5,0.5,3.5,-21,10,0.01,0.0005,2
-3,yellowsaltcube,0.1,-0.9,SIDE,0.271,-0.653,0.271,0.653,19,-11,10,0.01,0.0005,3
-4,yellowsaltcube,0.1,-0.9,SIDE,0,-0.707,0,0.707,21.2,5.6,10,0.01,0.0005,4
-5,yellowsaltcube,0.1,-0.9,SIDE,0.271,0.653,0.271,-0.653,11,19,10,0.01,0.0005,5
-6,yellowsaltcube,0.1,-0.9,SIDE,0.5,0.5,0.5,-0.5,-5.6,21,10,0.01,0.0005,6
-7,yellowsaltcube,0.1,-0.9,SIDE,0.653,0.271,0.653,-0.271,-18.4,12,10,0.01,0.0005,7
+id,object,gripper_finger_1_joint,gripper_finger_2_joint,direction,qw,qx,qy,qz,pos-x,pos-y,pos-z,eps_l1,vol_l1,
+0,yellowsaltcube,0.1,-0.9,SIDE,0.70711,0.00000,0.70711,0.00000,-22,0,10,0.01,0.0005,0
+1,yellowsaltcube,0.1,-0.9,SIDE,0.65328,-0.27060,0.65328,0.27060,-14,-17,10,0.01,0.0005,1
+2,yellowsaltcube,0.1,-0.9,SIDE,0.50000,-0.50000,0.50000,0.50000,3.5,-21,10,0.01,0.0005,2
+3,yellowsaltcube,0.1,-0.9,SIDE,0.27060,-0.65328,0.27060,0.65328,19,-11,10,0.01,0.0005,3
+4,yellowsaltcube,0.1,-0.9,SIDE,0.00000,-0.70711,0.00000,0.70711,21.2,5.6,10,0.01,0.0005,4
+5,yellowsaltcube,0.1,-0.9,SIDE,0.27060,0.65328,0.27060,-0.65328,11,19,10,0.01,0.0005,5
+6,yellowsaltcube,0.1,-0.9,SIDE,0.50000,0.50000,0.50000,-0.50000,-5.6,21,10,0.01,0.0005,6
+7,yellowsaltcube,0.1,-0.9,SIDE,0.65328,0.27060,0.65328,-0.27060,-18.4,12,10,0.01,0.0005,7

cob_grasp_generation/files/sdh_grasptable_kit.txt

@@ -0,0 +1,24 @@
+23
+11, /grasptables_kit/Sauerkraut_SDH.xml
+13, /grasptables_kit/FruitTea_SDH.xml
+16, /grasptables_kit/OrangeMarmelade_SDH.xml
+18, /grasptables_kit/YellowSaltCube_SDH.xml
+27, /grasptables_kit/HotPot_SDH.xml
+30, /grasptables_kit/BlueSaltCube_SDH.xml
+31, /grasptables_kit/YellowSaltCylinder_SDH.xml
+44, /grasptables_kit/Knaeckebrot_SDH.xml
+46, /grasptables_kit/LivioSunflowerOil_SDH.xml
+50, /grasptables_kit/InstantSoup_SDH.xml
+52, /grasptables_kit/HotPot2_SDH.xml
+57, /grasptables_kit/MuesliBars_SDH.xml
+65, /grasptables_kit/FruitDrink_SDH.xml
+99, /grasptables_kit/RuskWholemeal_SDH.xml
+101, /grasptables_kit/KoalaCandy_SDH.xml
+103, /grasptables_kit/InstantTomatoSoup_SDH.xml
+106, /grasptables_kit/BakingSoda_SDH.xml
+107, /grasptables_kit/Sweetener_SDH.xml
+109, /grasptables_kit/ChocoIcing_SDH.xml
+119, /grasptables_kit/TomatoHerbSauce_SDH.xml
+122, /grasptables_kit/Peanuts_SDH.xml
+126, /grasptables_kit/HerbSalt_SDH.xml
+128, /grasptables_kit/BathDetergent_SDH.xml

cob_grasp_generation/files/urdf/sdh.urdf.xacro

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
+       name="sdh" >
+
+  <xacro:include filename="$(find schunk_description)/urdf/sdh/sdh.urdf.xacro" />
+
+  <link name="grasp_link"/>
+
+  <xacro:schunk_sdh name="sdh" parent="grasp_link">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </xacro:schunk_sdh>
+
+</robot>

cob_grasp_generation/files/urdf/sdhx.urdf.xacro

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
+       name="sdhx" >
+
+  <xacro:include filename="$(find cob_description)/urdf/cob4_gripper/gripper.urdf.xacro" />
+
+  <link name="grasp_link"/>
+
+  <xacro:gripper name="gripper" parent="grasp_link">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </xacro:gripper>
+
+</robot>

cob_grasp_generation/launch/grasp_generation.launch

@@ -4,6 +4,8 @@
   <!-- OPENRAVE ENV VARIABLE -->
   <env name="OPENRAVE_DATA" value="$(find cob_grasp_generation)/files:$(optenv OPENRAVE_DATA)"/>
 
-  <node name="cob_grasp_generation_OR" pkg="cob_grasp_generation" type="cob_grasp_generation_action_server.py" respawn="false" output="screen"/>
+  <node name="generate_grasps_server" pkg="cob_grasp_generation" type="generate_grasps_server.py" respawn="false" output="screen"/>
+  <node name="show_grasps_or_server" pkg="cob_grasp_generation" type="show_grasps_or_server.py" respawn="false" output="screen"/>
+  <node name="query_grasps_server" pkg="cob_grasp_generation" type="query_grasps_server.py" respawn="false" output="screen"/>
 
 </launch>

cob_grasp_generation/launch/show_grasp.rviz

@@ -0,0 +1,180 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+      Splitter Ratio: 0.5
+    Tree Height: 437
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        grasp_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        gripper_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper_camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper_finger_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper_finger_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper_grasp_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        gripper_palm_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /object_marker
+      Name: Marker
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Axes
+      Enabled: true
+      Length: 0.5
+      Name: object_link
+      Radius: 0.05
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/Axes
+      Enabled: true
+      Length: 0.5
+      Name: grasp_link
+      Radius: 0.05
+      Reference Frame: grasp_link
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: object_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 0.910843
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.0172839
+        Y: 0.0316376
+        Z: 0.0972207
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.204797
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.16858
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 521
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000017ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000017f000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ba0000003efc0100000002fb0000000800540069006d00650000000000000003ba000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002c1000001c300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 705
+  X: 386
+  Y: 292

cob_grasp_generation/launch/show_grasp_rviz.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+
+  <arg name="gripper"/>
+
+  <!-- upload robot description -->
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find cob_grasp_generation)/files/urdf/$(arg gripper).urdf.xacro'"/>
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/>
+
+  <node pkg="cob_grasp_generation" type="show_grasps_rviz_server.py" name="show_grasps_rviz_server" output="screen"/>
+
+  <!-- rviz visualization -->
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find cob_grasp_generation)/launch/show_grasp.rviz" required="true" />
+
+</launch>

cob_grasp_generation/package.xml

@@ -24,12 +24,21 @@
   <depend>moveit_msgs</depend>
 
   <exec_depend>actionlib</exec_depend>
+  <exec_depend>cob_description</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>python-scipy</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>roslib</exec_depend>
   <exec_depend>rospy</exec_depend>
+  <exec_depend>rviz</exec_depend>
+  <exec_depend>schunk_description</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
   <exec_depend>tf</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
   <exec_depend>trajectory_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>xacro</exec_depend>
   <!--<exec_depend>openrave</exec_depend>-->
 
 </package>