Minor

cfg/active_grasp.yaml

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

scripts/reset.py

@@ -2,10 +2,13 @@ import rospy
 
 from active_grasp.srv import *
 
-rospy.init_node("test")
+rospy.init_node("reset")
 
 seed = rospy.ServiceProxy("seed", Seed)
 reset = rospy.ServiceProxy("reset", Reset)
 
-# seed(SeedRequest(1))
-reset(ResetRequest())
+seed(SeedRequest(1))
+
+while True:
+    reset(ResetRequest())
+    rospy.sleep(1.0)

src/active_grasp/simulation.py

@@ -153,18 +153,18 @@ class RandomScene(Scene):
         self.origin = self.center - np.r_[0.5 * self.length, 0.5 * self.length, 0.0]
         self.object_urdfs = find_urdfs(assets_dir / "test")
 
-    def generate(self, rng, attempts=10):
+    def generate(self, rng, object_count=4, attempts=10):
         self.add_support(self.center)
-        urdfs = rng.choice(self.object_urdfs, 4)
-        scale = 1.0
+        urdfs = rng.choice(self.object_urdfs, object_count)
         for urdf in urdfs:
+            scale = rng.uniform(0.8, 1.0)
             uid = self.add_object(urdf, Rotation.identity(), np.zeros(3), scale)
             lower, upper = p.getAABB(uid)
             z_offset = 0.5 * (upper[2] - lower[2]) + 0.002
             state_id = p.saveState()
             for _ in range(attempts):
                 # Try to place and check for collisions
-                ori = Rotation.from_rotvec([0.0, 0.0, rng.uniform(0, 2 * np.pi)])
+                ori = Rotation.from_euler("z", rng.uniform(0, 2 * np.pi))
                 pos = np.r_[rng.uniform(0.2, 0.8, 2) * self.length, z_offset]
                 p.resetBasePositionAndOrientation(uid, self.origin + pos, ori.as_quat())
                 p.stepSimulation()