Option to add random noise to object locations

cfg/active_grasp.yaml

@@ -2,7 +2,7 @@ bt_sim:
   gui: False
   cam_noise: False
   gripper_force: 10
-  scene: mustard1.yaml
+  scene: mustard.yaml
 
 grasp_controller:
   base_frame_id: panda_link0

cfg/scenes/kitchen2.yaml

@@ -1,13 +0,0 @@
-center: [0.5, 0.05, 0.2]
-q:  [0.0, -1.39, 0.0, -2.36, 0.0, 1.57, 0.79]
-objects:
-- object_id: 004_sugar_box
-  xyz: [-0.04, -0.03, 0.0]
-  rpy: [0, 0, 30]
-- object_id: 008_pudding_box
-  xyz: [-0.02, -0.06, 0.19]
-  rpy: [0, 0, 45]
-- object_id: 010_potted_meat_can
-  xyz: [0.04, 0.06, 0.0]
-  rpy: [0, 0, 60]
-  scale: 0.9

cfg/scenes/mustard.yaml

@@ -4,3 +4,4 @@ objects:
 - object_id: 006_mustard_bottle
   xyz: [0.0, 0.1, 0.0]
   rpy: [0, 0, -50]
+  randomize: True

cfg/scenes/mustard2.yaml

@@ -4,3 +4,5 @@ objects:
 - object_id: 006_mustard_bottle
   xyz: [0.0, 0.1, 0.0]
   rpy: [0, 0, 130]
+  randomize: True
+

src/active_grasp/simulation.py

@@ -178,12 +178,14 @@ class CustomScene(Scene):
         self.load_config()
         self.load_support(self.center)
         for object in self.scene["objects"]:
-            self.load_object(
+            urdf = self.get_ycb_urdf_path(object["object_id"])
-                self.get_ycb_urdf_path(object["object_id"]),
+            ori = Rotation.from_euler("xyz", object["rpy"], degrees=True)
-                Rotation.from_euler("xyz", object["rpy"], degrees=True),
+            pos = self.center + np.asarray(object["xyz"])
-                self.center + np.asarray(object["xyz"]),
+            scale = object.get("scale", 1)
-                object.get("scale", 1),
+            if object.get("randomize", False):
-            )
+                ori = Rotation.from_euler("z", rng.uniform(0, 2 * np.pi))
+                pos[:2] += rng.uniform(-0.05, 0.05, 2)
+            self.load_object(urdf, ori, pos, scale)
         for _ in range(60):
             p.stepSimulation()