diff --git a/cfg/scenes/scene1.yaml b/cfg/scenes/scene1.yaml index fd94019..3cc6ad6 100644 --- a/cfg/scenes/scene1.yaml +++ b/cfg/scenes/scene1.yaml @@ -3,5 +3,5 @@ objects: - object_id: ycb/006_mustard_bottle xyz: [0.0, 0.0, 0.0] rpy: [0, 0, -50] - randomize: {rot: 5, pos: 0.02} + randomize: {rot: 5, pos: [0.02, 0.02, 0]} q: [0.0, -1.39, 0.0, -2.36, 0.0, 1.57, 0.79] diff --git a/src/active_grasp/simulation.py b/src/active_grasp/simulation.py index d99a217..48a9e0c 100644 --- a/src/active_grasp/simulation.py +++ b/src/active_grasp/simulation.py @@ -137,7 +137,8 @@ class YamlScene(Scene): if randomize := object.get("randomize", False): angle = rng.uniform(-randomize["rot"], randomize["rot"]) ori *= Rotation.from_euler("z", angle, degrees=True) - pos[:2] += rng.uniform(-randomize["pos"], randomize["pos"], 2) + b = np.asarray(randomize["pos"]) + pos += rng.uniform(-b, b) self.add_object(urdf, ori, pos, scale) for _ in range(60): p.stepSimulation()