code state for kPAM experiments

models/iiwa/director/stored_poses.json

@@ -708,6 +708,21 @@
       },
       "name": "home",
       "nominal_handedness": "left"
+    },
+    {
+      "allow_mirror": true,
+      "description": "",
+      "joints": {
+        "iiwa_joint_1": -0.26647037267684937,
+        "iiwa_joint_2": 0.7008078694343567,
+        "iiwa_joint_3": 0.6157979965209961,
+        "iiwa_joint_4": -1.017812967300415,
+        "iiwa_joint_5": 0.7048412561416626,
+        "iiwa_joint_6": 1.7503107786178589,
+        "iiwa_joint_7": -1.3295161724090576
+      },
+      "name": "mug_rack_evaluation",
+      "nominal_handedness": "left"
     }
   ],
   "Grasping": [
@@ -1526,6 +1541,96 @@
       },
       "name": "look_at_rack_2",
       "nominal_handedness": "left"
+    },
+    {
+      "allow_mirror": true,
+      "description": "",
+      "joints": {
+        "iiwa_joint_1": 0.42992353439331055,
+        "iiwa_joint_2": -0.34932446479797363,
+        "iiwa_joint_3": 0.6271766424179077,
+        "iiwa_joint_4": -1.1395694017410278,
+        "iiwa_joint_5": 0.3954339921474457,
+        "iiwa_joint_6": 1.733742356300354,
+        "iiwa_joint_7": 0.0733514279127121
+      },
+      "name": "shoe_evaluation",
+      "nominal_handedness": "left"
+    },
+    {
+      "allow_mirror": true,
+      "description": "",
+      "joints": {
+        "iiwa_joint_1": 0.5119408369064331,
+        "iiwa_joint_2": 0.6319459676742554,
+        "iiwa_joint_3": 0.7225427031517029,
+        "iiwa_joint_4": -0.8657618165016174,
+        "iiwa_joint_5": 0.3663095235824585,
+        "iiwa_joint_6": 1.9009556770324707,
+        "iiwa_joint_7": -1.3394646644592285
+      },
+      "name": "shoe_evaluation_side_2",
+      "nominal_handedness": "left"
+    },
+    {
+      "allow_mirror": true,
+      "description": "",
+      "joints": {
+        "iiwa_joint_1": 0.11202535778284073,
+        "iiwa_joint_2": 0.48418450355529785,
+        "iiwa_joint_3": 0.763058602809906,
+        "iiwa_joint_4": -1.034080147743225,
+        "iiwa_joint_5": 0.6769240498542786,
+        "iiwa_joint_6": 1.8925440311431885,
+        "iiwa_joint_7": -1.3538767099380493
+      },
+      "name": "shoe_evaluation_side",
+      "nominal_handedness": "left"
+    },
+    {
+      "allow_mirror": true,
+      "description": "",
+      "joints": {
+        "iiwa_joint_1": 0.029475560411810875,
+        "iiwa_joint_2": 0.6006894707679749,
+        "iiwa_joint_3": 0.3461817800998688,
+        "iiwa_joint_4": -0.760356605052948,
+        "iiwa_joint_5": 0.2649575173854828,
+        "iiwa_joint_6": 1.657822608947754,
+        "iiwa_joint_7": -0.8735575675964355
+      },
+      "name": "look_at_mug_shelf",
+      "nominal_handedness": "left"
+    },
+    {
+      "allow_mirror": true,
+      "description": "",
+      "joints": {
+        "iiwa_joint_1": -0.3235970735549927,
+        "iiwa_joint_2": 0.5881968140602112,
+        "iiwa_joint_3": 0.47376877069473267,
+        "iiwa_joint_4": -1.0043022632598877,
+        "iiwa_joint_5": 0.5601233243942261,
+        "iiwa_joint_6": 1.5545154809951782,
+        "iiwa_joint_7": -1.1482882499694824
+      },
+      "name": "look_at_mug_shelf_2",
+      "nominal_handedness": "left"
+    },
+    {
+      "allow_mirror": true,
+      "description": "",
+      "joints": {
+        "iiwa_joint_1": -0.2651101052761078,
+        "iiwa_joint_2": 0.6987987160682678,
+        "iiwa_joint_3": 0.6174938082695007,
+        "iiwa_joint_4": -1.0204466581344604,
+        "iiwa_joint_5": 0.7046487331390381,
+        "iiwa_joint_6": 1.7471362352371216,
+        "iiwa_joint_7": -1.325123906135559
+      },
+      "name": "mug_rack_evaluation",
+      "nominal_handedness": "left"
     }
   ],
   "mug": [
@@ -1693,6 +1798,36 @@
       },
       "name": "horizontal_grasp_approach",
       "nominal_handedness": "left"
+    },
+    {
+      "allow_mirror": true,
+      "description": "",
+      "joints": {
+        "iiwa_joint_1": 0.043940939009189606,
+        "iiwa_joint_2": 0.1439794898033142,
+        "iiwa_joint_3": -0.017146429046988487,
+        "iiwa_joint_4": -1.3585749864578247,
+        "iiwa_joint_5": 0.0077800629660487175,
+        "iiwa_joint_6": 1.161775827407837,
+        "iiwa_joint_7": 0.42396053671836853
+      },
+      "name": "mug_shelf_image_capture",
+      "nominal_handedness": "left"
+    },
+    {
+      "allow_mirror": true,
+      "description": "",
+      "joints": {
+        "iiwa_joint_1": 0.0439445935189724,
+        "iiwa_joint_2": 0.14397314190864563,
+        "iiwa_joint_3": -0.017145171761512756,
+        "iiwa_joint_4": -1.3585752248764038,
+        "iiwa_joint_5": 0.007774789817631245,
+        "iiwa_joint_6": 1.1617718935012817,
+        "iiwa_joint_7": 0.4239634573459625
+      },
+      "name": "image_capture_for_mug_shelf",
+      "nominal_handedness": "left"
     }
   ],
   "object_interaction": [

modules/spartan/director/iiwamanipdev.py

@@ -19,6 +19,7 @@
 from spartan.manipulation.category_manipulation import CategoryManipulation
 from spartan.utils.director_ros_visualizer import DirectorROSVisualizer
 
+
 # ros
 import tf2_ros
 
@@ -69,13 +70,12 @@ def setupRLGDirector(globalsDict=None):
     # fix for https://github.com/RobotLocomotion/spartan/issues/244
     globalsDict['treeViewer'].subscriber.setSpeedLimit(5)
 
-
+    #
     ros_visualizer = DirectorROSVisualizer(tf_buffer=tfBuffer)
     topic = "/camera_carmine_1/depth/points"
     ros_visualizer.add_subscriber(topic, name="Carmine", visualize=True)
     globalsDict['ros_visualizer'] = ros_visualizer
-    # ros_visualizer.start()
-
+    ros_visualizer.start()
 
 
     # load background scene if it exists
@@ -86,12 +86,17 @@ def setupRLGDirector(globalsDict=None):
     robotSystem = globalsDict['robotSystem']
     robotStateModel = robotSystem.robotStateModel
 
+
     category_manip = CategoryManipulation(robotStateModel)
     category_manip.load_side_table()
-    # category_manip.load_mug_rack()
+    category_manip.load_mug_rack()
     # category_manip.setup_horizontal_mug_grasp()
-    category_manip.load_mug_platform()
-    category_manip.load_shoe_rack()
+    # category_manip.load_mug_platform()
+    # category_manip.load_shoe_rack()
+
+
+    graspSupervisor._category_manip = category_manip
+
 
     def visualize_background():
         if not os.path.exists(background_ply_file):

modules/spartan/manipulation/category_manipulation.py

@@ -204,3 +204,8 @@ def get_object_pose_relative_to_gripper(self):
 
         print(transformUtils.poseFromTransform(T_gripper_object))
         return T_gripper_object
+
+
+    @property
+    def mug_rack_vis_obj(self):
+        return self._mug_rack

modules/spartan/utils/director_ros_visualizer.py

@@ -8,6 +8,7 @@
 import tf2_ros
 import sensor_msgs
 
+
 # director
 import director.objectmodel as om
 import director.vtkNumpy as vnp
@@ -26,7 +27,12 @@ class DirectorROSVisualizer(object):
 
     def __init__(self, tf_buffer=None):
         self.taskRunner = TaskRunner()
-        self._tf_buffer = tf_buffer
+
+        if tf_buffer is None:
+            self.setup_TF()
+        else:
+            self._tf_buffer = tf_buffer
+
         self.clear_visualization()
         self._subscribers = dict()
         self._expressed_in_frame = "base"
@@ -177,6 +183,27 @@ def numpy_from_pointcloud2_msg(msg):
 
         return points
 
+    @staticmethod
+    def pointcloud2_msg_from_numpy(pc_numpy):
+        """
+
+        :param pc_numpy: N x 3
+        :type pc_numpy:
+        :return:
+        :rtype:
+        """
+
+        N = pc_numpy.shape[0]
+
+        pc = np.zeros(N, dtype=[('x', np.float64), ('y', np.float64), ('z', np.float64)])
+        pc['x'] = pc_numpy[:, 0]
+        pc['y'] = pc_numpy[:, 1]
+        pc['z'] = pc_numpy[:, 2]
+
+        msg = ros_numpy.msgify(sensor_msgs.msg.PointCloud2, pc)
+
+        return msg
+
 
 
 

src/catkin_projects/station_config/RLG_iiwa_1/manipulation/mug_rack.yaml

@@ -12,7 +12,7 @@ poses:
       x: 0.08307750740841482
       y: 0.12323850875823555
       z: -0.03754592630709378
-  left_side_table:
+  left_side_table_v2:
     quaternion:
       w: 0.6784430069620968
       x: 0.0
@@ -23,6 +23,18 @@ poses:
       y: 0.010488924015345145
       z: -0.04068805659371864
 
+  left_side_table:
+    quaternion:
+      w: 0.6784430069620968
+      x: 0.0
+      y: 0.0
+      z: 0.7346530380419237
+    translation:
+      x: 0.3285152706168804
+      y: 0.007816573364084667
+      z: -0.045688056593718634
+
+
 
 keypoints:
   top_bar_tip:

src/catkin_projects/station_config/RLG_iiwa_1/manipulation/params.yaml

@@ -160,6 +160,7 @@ speed:
   grasp: 30
   nominal: 30
   fast: 45
+  super_fast: 60
   slow: 20
   stow: 30
   cartesian_grasp: 0.05 # meters/second
@@ -188,7 +189,7 @@ gripper_palm_to_ee:
     z: -0.00107904
 
 
-pre_grasp_distance: 0.10 # distance (in meters) to push back grasp for pre-grasp
+pre_grasp_distance: 0.15 # distance (in meters) to push back grasp for pre-grasp
 force_threshold_magnitude: 30 # Newtons, force threshold at which to stop and execute grasp
 grasp_push_in_distance: 0.02 # distance (in meters) to push "past" the planned grasp
 gripper_width_offset: 0.015 # how much narrower the fingers are due to the rubber pads

src/catkin_projects/station_config/RLG_iiwa_1/manipulation/shoe_rack.yaml

@@ -22,7 +22,7 @@ poses:
       y: -0.10488875332467817
       z: -0.024193096290937022
 
-  left_side_table:
+  left_side_table_v2:
     quaternion:
       w: 0.7056393004795904
       x: -0.002677959473069953
@@ -32,6 +32,16 @@ poses:
       x: 0.2106936579319592
       y: -0.23106176379997845
       z: -0.0262905078264197
+  left_side_table:
+    quaternion:
+      w: 0.7056393004795904
+      x: -0.002677959473069953
+      y: 0.00266691656368899
+      z: 0.7085611432387308
+    translation:
+      x: 0.21035342533257284
+      y: -0.14872416407500277
+      z: -0.026912900937897694
 
 
 plane: # these are in shoe rack frame

src/catkin_projects/station_config/RLG_iiwa_1/stored_poses.yaml

@@ -313,6 +313,14 @@ General:
   - 1.3406859636306763
   - 1.0746363401412964
   - 0.9941601753234863
+  mug_rack_evaluation:
+  - -0.26647037267684937
+  - 0.7008078694343567
+  - 0.6157979965209961
+  - -1.017812967300415
+  - 0.7048412561416626
+  - 1.7503107786178589
+  - -1.3295161724090576
   q_nom:
   - 0.0
   - -0.68
@@ -760,6 +768,22 @@ left_table:
   - 0.03117365390062332
   - 1.01997971534729
   - 0.43648219108581543
+  look_at_mug_shelf:
+  - 0.029475560411810875
+  - 0.6006894707679749
+  - 0.3461817800998688
+  - -0.760356605052948
+  - 0.2649575173854828
+  - 1.657822608947754
+  - -0.8735575675964355
+  look_at_mug_shelf_2:
+  - -0.3235970735549927
+  - 0.5881968140602112
+  - 0.47376877069473267
+  - -1.0043022632598877
+  - 0.5601233243942261
+  - 1.5545154809951782
+  - -1.1482882499694824
   look_at_rack:
   - 0.3436020612716675
   - -0.15440696477890015
@@ -768,6 +792,14 @@ left_table:
   - 0.7468169927597046
   - 1.2323585748672485
   - -0.9038079380989075
+  look_at_rack_2:
+  - 0.4864267408847809
+  - 0.21464040875434875
+  - 0.42382150888442993
+  - -1.0233983993530273
+  - 0.21979615092277527
+  - 1.5541713237762451
+  - -0.20744794607162476
   mug_pre_place:
   - 1.2146261930465698
   - 0.42955201864242554
@@ -776,6 +808,14 @@ left_table:
   - -0.936322033405304
   - 0.9616686701774597
   - 0.8696807622909546
+  mug_rack_evaluation:
+  - -0.2651101052761078
+  - 0.6987987160682678
+  - 0.6174938082695007
+  - -1.0204466581344604
+  - 0.7046487331390381
+  - 1.7471362352371216
+  - -1.325123906135559
   scan_left:
   - 0.2685287296772003
   - -1.0680149793624878
@@ -800,6 +840,30 @@ left_table:
   - 0.15577785670757294
   - 2.0078847408294678
   - 0.04247215390205383
+  shoe_evaluation:
+  - 0.42992353439331055
+  - -0.34932446479797363
+  - 0.6271766424179077
+  - -1.1395694017410278
+  - 0.3954339921474457
+  - 1.733742356300354
+  - 0.0733514279127121
+  shoe_evaluation_side:
+  - 0.11202535778284073
+  - 0.48418450355529785
+  - 0.763058602809906
+  - -1.034080147743225
+  - 0.6769240498542786
+  - 1.8925440311431885
+  - -1.3538767099380493
+  shoe_evaluation_side_2:
+  - 0.5119408369064331
+  - 0.6319459676742554
+  - 0.7225427031517029
+  - -0.8657618165016174
+  - 0.3663095235824585
+  - 1.9009556770324707
+  - -1.3394646644592285
   top_down:
   - 1.5718567371368408
   - 0.28527259826660156
@@ -849,6 +913,14 @@ mug:
   - 1.331290602684021
   - 1.0857104063034058
   - 0.9829419851303101
+  image_capture_for_mug_shelf:
+  - 0.0439445935189724
+  - 0.14397314190864563
+  - -0.017145171761512756
+  - -1.3585752248764038
+  - 0.007774789817631245
+  - 1.1617718935012817
+  - 0.4239634573459625
   mug_place_from_base_grasp:
   - 0.2691195011138916
   - 1.1659209728240967
@@ -889,6 +961,14 @@ mug:
   - 0.24946992099285126
   - 1.0983178615570068
   - 2.6610031127929688
+  mug_shelf_image_capture:
+  - 0.043940939009189606
+  - 0.1439794898033142
+  - -0.017146429046988487
+  - -1.3585749864578247
+  - 0.0077800629660487175
+  - 1.161775827407837
+  - 0.42396053671836853
   vertical_grasp_above_table:
   - 0.5099468231201172
   - 0.7887730598449707