switch scene pipeline passed

cfg/active_grasp.rviz

@@ -256,10 +256,7 @@ Visualization Manager:
       Name: Markers
       Namespaces:
         bbox: true
-        grasp: true
-        grasps: true
         path: true
-        roi: true
         views: true
       Queue Size: 100
       Value: true
@@ -427,7 +424,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.261960744857788
+      Distance: 1.094533920288086
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -435,17 +432,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.5695413947105408
-        Y: -0.03970015048980713
-        Z: 0.45675671100616455
+        X: 0.32280832529067993
+        Y: 0.16504701972007751
+        Z: 0.43913549184799194
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.295397013425827
+      Pitch: 0.2053970992565155
       Target Frame: <Fixed Frame>
-      Yaw: 5.118584632873535
+      Yaw: 0.8653952479362488
     Saved:
       - Class: rviz/Orbit
         Distance: 1.2000000476837158
@@ -486,4 +483,4 @@ Window Geometry:
     collapsed: true
   Width: 1095
   X: 1260
-  Y: 86
+  Y: 27

cfg/active_grasp.yaml

@@ -2,7 +2,8 @@ bt_sim:
   gui: True
   gripper_force: 10
   # scene: random
-  scene: $(find active_grasp)/cfg/sim/challenging_scene_2.yaml
+  scene: manual
+  # scene: $(find active_grasp)/cfg/sim/challenging_scene_2.yaml
 
 hw:
   roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt
@@ -13,7 +14,7 @@ grasp_controller:
   ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065]  # offset to panda_link8
   control_rate: 100
   linear_vel: 0.05
-  move_to_target_threshold: 0.05 # meter
+  move_to_target_threshold: 0.01 # meter
   camera:
     frame_id: camera_depth_optical_frame
     info_topic: /camera/depth/camera_info

cfg/sim/mustard.yaml

@@ -1,7 +0,0 @@
-center: [0.5, 0.2, 0.25]
-q:  [0.0, -1.39, 0.0, -2.36, 0.0, 1.57, 0.79]
-objects:
-- object_id: ycb/006_mustard_bottle
-  xyz: [0.0, 0.0, 0.0]
-  rpy: [0, 0, -50]
-  scale: 0.8

scripts/run.py

@@ -46,7 +46,7 @@ def main():
 def create_parser():
     parser = argparse.ArgumentParser()
     parser.add_argument("policy", type=str, choices=registry.keys())
-    parser.add_argument("--runs", type=int, default=5)
+    parser.add_argument("--runs", type=int, default=20)
     parser.add_argument("--wait-for-input", action="store_true")
     parser.add_argument("--logdir", type=Path, default="logs")
     parser.add_argument("--seed", type=int, default=1)

src/active_grasp/active_perception_policy.py

@@ -269,7 +269,11 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
             # gsnet_input_points = target_points_list
             # gsnet_input_points = merged_points_list
             self.publish_pointcloud(gsnet_input_points)
-            gsnet_grasping_poses = np.asarray(self.request_grasping_pose(gsnet_input_points))
+            received_points = False
+            while(received_points == False): 
+                gsnet_grasping_poses = np.asarray(self.request_grasping_pose(gsnet_input_points))
+                received_points = True
+                print(gsnet_grasping_poses[0].keys())
 
             # DEBUG: publish grasps
             # self.publish_grasps(gsnet_grasping_poses)

src/active_grasp/simulation.py

@@ -232,11 +232,57 @@ class RandomScene(Scene):
         return q
 
 
+class ManualScene(Scene):
+    def __init__(self):
+        super().__init__()
+        self.config_path = pkg_root / "cfg/sim"
+        self.scene_index = 0
+        # Visit the directory and read all the yaml files
+        self.scenes = []
+        for file in self.config_path.iterdir():
+            if file.suffix == ".yaml":
+                self.scenes.append(file)
+        self.num_scenes = len(self.scenes)
+    
+    def load_config(self):
+        self.scene = load_yaml(self.scenes[self.scene_index])
+        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 generate(self, rng):
+        self.load_config()
+        self.add_support(self.center)
+
+        for object in self.scene["objects"]:
+            urdf = urdfs_dir / object["object_id"] / "model.urdf"
+            ori = Rotation.from_euler("xyz", object["rpy"], degrees=True)
+            pos = self.center + np.asarray(object["xyz"])
+            scale = object.get("scale", 1)
+            if randomize := object.get("randomize", False):
+                angle = rng.uniform(-randomize["rot"], randomize["rot"])
+                ori = Rotation.from_euler("z", angle, degrees=True) * ori
+                b = np.asarray(randomize["pos"])
+                pos += rng.uniform(-b, b)
+            self.add_object(urdf, ori, pos, scale)
+        for _ in range(60):
+            p.stepSimulation()
+        
+        self.scene_index += 1
+        if(self.scene_index >= self.num_scenes):
+            self.scene_index = 0
+
+        return self.scene["q"]
+
+
 def get_scene(scene_id):
     if scene_id.endswith(".yaml"):
         return YamlScene(scene_id)
     elif scene_id == "random":
         return RandomScene()
+    elif scene_id == "manual":
+        return ManualScene()
 
     else:
         raise ValueError("Unknown scene {}.".format(scene_id))