code state for kPAM experiments

RobotLocomotion · Mar 25, 2019 · afd2058 · afd2058
1 parent 7c85fad
commit afd2058
Show file tree

Hide file tree

Showing 10 changed files with 594 additions and 46 deletions.
diff --git a/models/iiwa/director/stored_poses.json b/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": [

diff --git a/modules/spartan/director/iiwamanipdev.py b/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):

diff --git a/modules/spartan/manipulation/category_manipulation.py b/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