diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml index 3d1742e..742c3cd 100644 --- a/cfg/active_grasp.yaml +++ b/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 diff --git a/cfg/scenes/kitchen1.yaml b/cfg/scenes/kitchen.yaml similarity index 100% rename from cfg/scenes/kitchen1.yaml rename to cfg/scenes/kitchen.yaml diff --git a/cfg/scenes/kitchen2.yaml b/cfg/scenes/kitchen2.yaml deleted file mode 100644 index 5b53174..0000000 --- a/cfg/scenes/kitchen2.yaml +++ /dev/null @@ -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 diff --git a/cfg/scenes/mustard1.yaml b/cfg/scenes/mustard.yaml similarity index 89% rename from cfg/scenes/mustard1.yaml rename to cfg/scenes/mustard.yaml index 1a986ce..c1a69a9 100644 --- a/cfg/scenes/mustard1.yaml +++ b/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 diff --git a/cfg/scenes/mustard2.yaml b/cfg/scenes/mustard2.yaml index 9a97dbc..0f5cf26 100644 --- a/cfg/scenes/mustard2.yaml +++ b/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 + diff --git a/src/active_grasp/simulation.py b/src/active_grasp/simulation.py index 41c129f..8b6f597 100644 --- a/src/active_grasp/simulation.py +++ b/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( - self.get_ycb_urdf_path(object["object_id"]), - Rotation.from_euler("xyz", object["rpy"], degrees=True), - self.center + np.asarray(object["xyz"]), - object.get("scale", 1), - ) + urdf = self.get_ycb_urdf_path(object["object_id"]) + ori = Rotation.from_euler("xyz", object["rpy"], degrees=True) + pos = self.center + np.asarray(object["xyz"]) + scale = 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()