Reload scene config

active_grasp/policy.py

@@ -78,8 +78,7 @@ class Policy:
         return grasps[indices], scores[indices]
 
     def score_fn(self, grasp):
-        # return grasp.quality
+        return grasp.quality
-        return grasp.pose.translation[2]
 
     def update(self, img, pose):
         raise NotImplementedError

active_grasp/simulation.py

@@ -40,7 +40,7 @@ class Simulation:
         self.arm = BtPandaArm(panda_urdf)
         self.gripper = BtPandaGripper(self.arm)
         self.model = Model(panda_urdf, self.arm.base_frame, self.arm.ee_frame)
-        self.camera = BtCamera(320, 240, 1.047, 0.1, 1.0, self.arm.uid, 11)
+        self.camera = BtCamera(320, 240, 0.96, 0.2, 1.0, self.arm.uid, 11)
 
     def seed(self, seed):
         self.rng = np.random.default_rng(seed)
@@ -123,7 +123,6 @@ def find_urdfs(root):
 class RandomScene(Scene):
     def __init__(self):
         super().__init__()
-        # self.center = np.r_[0.5, 0.0, 0.2]
         self.center = np.r_[0.6, 0.0, 0.1]
         self.length = 0.3
         self.origin = self.center - np.r_[0.5 * self.length, 0.5 * self.length, 0.0]
@@ -161,19 +160,24 @@ class RandomScene(Scene):
 class CustomScene(Scene):
     def __init__(self, config_name):
         super().__init__()
-        config_path = Path(rospack.get_path("active_grasp")) / "cfg" / config_name
+        self.config_path = (
-        with config_path.open("r") as f:
+            Path(rospack.get_path("active_grasp")) / "cfg" / "scenes" / config_name
+        )
+
+    def load_config(self):
+        with self.config_path.open("r") as f:
             self.scene = yaml.load(f)
         self.center = np.asarray(self.scene["center"])
         self.length = 0.3
         self.origin = self.center - np.r_[0.5 * self.length, 0.5 * self.length, 0.0]
 
     def load(self, rng):
+        self.load_config()
         self.load_support(self.center)
         for object in self.scene["objects"]:
             self.load_object(
                 self.get_ycb_urdf_path(object["object_id"]),
-                Rotation.from_euler("xyz", object["rpy"]),
+                Rotation.from_euler("xyz", object["rpy"], degrees=True),
                 self.center + np.asarray(object["xyz"]),
             )
 

launch/active_grasp.rviz

@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /TF1/Frames1
       Splitter Ratio: 0.5
-    Tree Height: 805
+    Tree Height: 338
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -25,7 +25,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: SceneCloud
+    SyncSource: DepthImage
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -51,6 +51,18 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
+    - Class: rviz/Image
+      Enabled: false
+      Image Topic: /camera/depth/image_rect_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: DepthImage
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: false
     - Alpha: 0.20000000298023224
       Autocompute Intensity Bounds: false
       Autocompute Value Bounds:
@@ -86,10 +98,7 @@ Visualization Manager:
       Marker Topic: visualization_marker_array
       Name: Markers
       Namespaces:
-        bbox: true
+        {}
-        best_grasp: true
-        grasps: true
-        path: true
       Queue Size: 100
       Value: true
     - Acceleration_Scaling_Factor: 0.1
@@ -125,10 +134,6 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          camera_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
           panda_hand:
             Alpha: 1
             Show Axes: false
@@ -211,7 +216,7 @@ Visualization Manager:
         Goal State Color: 250; 128; 0
         Interactive Marker Size: 0
         Joint Violation Color: 255; 0; 255
-        Planning Group: panda_arm
+        Planning Group: hand
         Query Goal State: true
         Query Start State: false
         Show Workspace: false
@@ -234,10 +239,6 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          camera_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
           panda_hand:
             Alpha: 1
             Show Axes: false
@@ -324,74 +325,38 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          camera_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          panda_hand:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_leftfinger:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link0:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link4:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link5:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link6:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link7:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link8:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_rightfinger:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: false
       Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.0020000000949949026
+      Style: Spheres
+      Topic: /camera/depth/color/points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: false
       Autocompute Value Bounds:
@@ -432,10 +397,6 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        camera_optical_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
         panda_hand:
           Alpha: 1
           Show Axes: false
@@ -535,14 +496,18 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        camera_optical_frame:
+        camera_depth_optical_frame:
+          Value: true
+        panda_EE:
+          Value: true
+        panda_K:
           Value: true
         panda_hand:
           Value: false
         panda_leftfinger:
           Value: false
         panda_link0:
-          Value: false
+          Value: true
         panda_link1:
           Value: false
         panda_link2:
@@ -561,8 +526,6 @@ Visualization Manager:
           Value: false
         panda_rightfinger:
           Value: false
-        task:
-          Value: true
         world:
           Value: false
       Marker Alpha: 1
@@ -582,17 +545,30 @@ Visualization Manager:
                       panda_link6:
                         panda_link7:
                           panda_link8:
-                            panda_hand:
+                            camera_depth_optical_frame:
-                              camera_optical_frame:
                               {}
+                            panda_EE:
+                              panda_K:
+                                {}
+                            panda_hand:
                               panda_leftfinger:
                                 {}
                               panda_rightfinger:
                                 {}
-            task:
-              {}
       Update Interval: 0
       Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /camera/color/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: ColorImage
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -621,7 +597,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.5005543231964111
+      Distance: 1.8815758228302002
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -629,29 +605,33 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.3386422395706177
+        X: 0.4086047410964966
-        Y: -0.016611307859420776
+        Y: 0.23318199813365936
-        Z: 0.429339200258255
+        Z: 0.28523021936416626
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.39479339122772217
+      Pitch: 0.3297969102859497
       Target Frame: <Fixed Frame>
-      Yaw: 5.172863483428955
+      Yaw: 5.052877902984619
     Saved: ~
 Window Geometry:
+  ColorImage:
+    collapsed: false
+  DepthImage:
+    collapsed: false
   Displays:
     collapsed: false
-  Height: 1019
+  Height: 960
   Hide Left Dock: false
   Hide Right Dock: true
   MotionPlanning:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -659,7 +639,7 @@ Window Geometry:
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: true
+    collapsed: false
-  Width: 1516
+  Width: 1920
-  X: 1039
+  X: 0
-  Y: 114
+  Y: 27