From 518fa68252b175220a0f93fd1ccdec3b27530865 Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Mon, 6 Dec 2021 18:08:50 +0100
Subject: [PATCH] Add random offset to q0

---
 scripts/hw_node.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/scripts/hw_node.py b/scripts/hw_node.py
index dc75efd..7f6c6ef 100755
--- a/scripts/hw_node.py
+++ b/scripts/hw_node.py
@@ -41,7 +41,8 @@ class HwNode:
         rospy.Service("reset", Reset, self.reset)
 
     def seed(self, req):
-        # Nothing to do
+        self.rng = np.random.default_rng(req.seed)
+        rospy.loginfo(f"Seeded the rng with {req.seed}.")
         return SeedResponse()
 
     def reset(self, req):
@@ -49,6 +50,7 @@ class HwNode:
 
         # Move to the initial configuration
         self.switch_to_joint_trajectory_controller()
+        q0 += self.rng.uniform(-0.069, 0.069, 7)
         self.moveit.goto(q0, velocity_scaling=0.4)
         self.gripper.move(0.08)