sample target pose around object pose in pushing

Author: erampone

pybullet_robot_envs/envs/icub_envs/icub_push_gym_env.py

@@ -34,7 +34,7 @@ def __init__(self,
                  control_orientation=0,
                  obj_name=get_objects_list()[1],
                  obj_pose_rnd_std=0,
-                 tg_pose_rnd_std=0,
+                 tg_pose_rnd_std=0.2,
                  renders=False,
                  max_steps=2000,
                  reward_type=1):
@@ -134,17 +134,17 @@ def reset(self):
         for _ in range(100):
             p.stepSimulation()
 
-        self._tg_pose = self._sample_pose()
+        world_obs, _ = self._world.get_observation()
+        self._tg_pose = self._sample_pose(world_obs[:3])
 
+        self._robot.debug_gui()
+        self._world.debug_gui()
+        self.debug_gui()
 
         # Let the world run for a bit
         for _ in range(100):
             p.stepSimulation()
 
-        self._robot.debug_gui()
-        self._world.debug_gui()
-        self.debug_gui()
-
         robot_obs, _ = self._robot.get_observation()
         world_obs, _ = self._world.get_observation()
 
@@ -332,32 +332,35 @@ def _compute_reward(self):
 
         return reward
 
-    def _sample_pose(self):
+    def _sample_pose(self, obj_pos):
 
         # ws_lim = self._ws_lim
         x_min = self._world._ws_lim[0][0] + 0.064668
         x_max = self._world._ws_lim[0][1] - 0.05
 
-        px = x_min + 0.2 * (x_max - x_min)
-        py = self._world._ws_lim[1][0] + 0.5 * (self._world._ws_lim[1][1] - self._world._ws_lim[1][0])
-        pz = self._world.get_table_height()
+        px = obj_pos[0] + 0.05
+        py = obj_pos[1] + 0.05
+        pz = obj_pos[2]
+
+        px = np.clip(px, x_min, x_max)
+        py = np.clip(py, self._world._ws_lim[1][0], self._world._ws_lim[1][1])
 
         if self._tg_pose_rnd_std > 0:
             # Add a Gaussian noise to position
             mu, sigma = 0, self._tg_pose_rnd_std
             noise = np.random.normal(mu, sigma, 2)
 
-            px = px + noise[0]
+            px = obj_pos[0] + noise[0]
             px = np.clip(px, x_min, x_max)
 
-            py = py + noise[1]
+            py = obj_pos[1] + noise[1]
             py = np.clip(py, self._world._ws_lim[1][0], self._world._ws_lim[1][1])
 
         pose = (px, py, pz)
 
         return pose
 
-    def debug_gui(self):
+    def _debug_gui(self):
         p.addUserDebugLine(self._tg_pose, [self._tg_pose[0] + 0.1, self._tg_pose[1], self._tg_pose[2]], [1, 0, 0])
         p.addUserDebugLine(self._tg_pose, [self._tg_pose[0], self._tg_pose[1] + 0.1, self._tg_pose[2]], [0, 1, 0])
         p.addUserDebugLine(self._tg_pose, [self._tg_pose[0], self._tg_pose[1], self._tg_pose[2] + 0.1], [0, 0, 1])

pybullet_robot_envs/envs/panda_envs/panda_push_gym_env.py

@@ -41,7 +41,7 @@ def __init__(self,
                  renders=False,
                  max_steps=1000,
                  dist_delta=0.03, joint_action_space=7,
-                 obj_pose_rnd_std=0, tg_pose_rnd_std=0,
+                 obj_pose_rnd_std=0, tg_pose_rnd_std=0.2,
                  includeVelObs=True):
 
         self._timeStep = 1. / 240.
@@ -123,20 +123,29 @@ def reset(self):
         p.setTimeStep(self._timeStep)
         self._env_step_counter = 0
 
-        self._robot.reset()
-        self._world.reset()
-
-        self._target_pose = self._sample_pose()
-
         p.setGravity(0, 0, -9.8)
 
+        self._robot.reset()
+        # Let the world run for a bit
+        for _ in range(100):
+            p.stepSimulation()
+
+        self._world.reset()
         # Let the world run for a bit
-        for _ in range(500):
+        for _ in range(100):
             p.stepSimulation()
 
+        world_obs, _ = self._world.get_observation()
+
+        self._target_pose = self._sample_pose(world_obs[:3])
+
         self._robot.debug_gui()
         self._world.debug_gui()
-        self._debug_gui()
+        self.debug_gui()
+
+        # Let the world run for a bit
+        for _ in range(100):
+            p.stepSimulation()
 
         self._observation = self.get_extended_observation()
         return np.array(self._observation)
@@ -258,31 +267,35 @@ def _compute_reward(self):
             reward = np.float32(1000.0) + (100 - d2*80)
         return reward
 
-    def _sample_pose(self):
+    def _sample_pose(self, obj_pos):
+
         # ws_lim = self._ws_lim
         x_min = self._world._ws_lim[0][0] + 0.064668
         x_max = self._world._ws_lim[0][1] - 0.05
 
-        px = x_min + 0.2 * (x_max - x_min)
-        py = self._world._ws_lim[1][0] + 0.5 * (self._world._ws_lim[1][1] - self._world._ws_lim[1][0])
-        pz = self._world.get_table_height()
+        px = obj_pos[0] + 0.05
+        py = obj_pos[1] + 0.05
+        pz = obj_pos[2]
+
+        px = np.clip(px, x_min, x_max)
+        py = np.clip(py, self._world._ws_lim[1][0], self._world._ws_lim[1][1])
 
         if self._tg_pose_rnd_std > 0:
             # Add a Gaussian noise to position
             mu, sigma = 0, self._tg_pose_rnd_std
             noise = np.random.normal(mu, sigma, 2)
 
-            px = px + noise[0]
+            px = obj_pos[0] + noise[0]
             px = np.clip(px, x_min, x_max)
 
-            py = py + noise[1]
-            py = np.clip(py, self._ws_lim[1][0], self._ws_lim[1][1])
+            py = obj_pos[1] + noise[1]
+            py = np.clip(py, self._world._ws_lim[1][0], self._world._ws_lim[1][1])
 
         pose = (px, py, pz)
 
         return pose
 
-    def _debug_gui(self):
+    def debug_gui(self):
         p.addUserDebugLine(self._target_pose, [self._target_pose[0] + 0.1, self._target_pose[1], self._target_pose[2]], [1, 0, 0])
         p.addUserDebugLine(self._target_pose, [self._target_pose[0], self._target_pose[1] + 0.1, self._target_pose[2]], [0, 1, 0])
         p.addUserDebugLine(self._target_pose, [self._target_pose[0], self._target_pose[1], self._target_pose[2] + 0.1], [0, 0, 1])

pybullet_robot_envs/examples/test_envs/test_panda_push_gym_env.py

@@ -9,7 +9,6 @@
 os.sys.path.insert(0, parentdir)
 
 from pybullet_robot_envs.envs.panda_envs.panda_push_gym_env import pandaPushGymEnv
-from pybullet_robot_envs import robot_data
 import pybullet_data