fix pybullet_envs locomotion camera issues: camera lagging behind over time, also follow cam in 'GUI' mode, allow user to change disance/yaw/pitch

examples/pybullet/gym/pybullet_envs/env_bases.py

@@ -88,16 +88,23 @@ class MJCFBaseBulletEnv(gym.Env):
     self.potential = self.robot.calc_potential()
     return s
 
+  def camera_adjust(self):
+    pass
+
   def render(self, mode='human', close=False):
+  
     if mode == "human":
       self.isRender = True
+    if self.physicsClientId>=0:
+      self.camera_adjust()
+
     if mode != "rgb_array":
       return np.array([])
 
     base_pos = [0, 0, 0]
     if (hasattr(self, 'robot')):
-      if (hasattr(self.robot, 'body_xyz')):
-        base_pos = self.robot.body_xyz
+      if (hasattr(self.robot, 'body_real_xyz')):
+        base_pos = self.robot.body_real_xyz
     if (self.physicsClientId>=0):
       view_matrix = self._p.computeViewMatrixFromYawPitchRoll(cameraTargetPosition=base_pos,
                                                             distance=self._cam_dist,
@@ -115,15 +122,8 @@ class MJCFBaseBulletEnv(gym.Env):
                                               viewMatrix=view_matrix,
                                               projectionMatrix=proj_matrix,
                                               renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
-      try:
-        # Keep the previous orientation of the camera set by the user.
-        con_mode = self._p.getConnectionInfo()['connectionMethod']
-        if con_mode==self._p.SHARED_MEMORY or con_mode == self._p.GUI:
-          [yaw, pitch, dist] = self._p.getDebugVisualizerCamera()[8:11]
-          self._p.resetDebugVisualizerCamera(dist, yaw, pitch, base_pos)
-      except:
-        pass
 
+      self._p.configureDebugVisualizer(self._p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
     else:
       px = np.array([[[255,255,255,255]]*self._render_width]*self._render_height, dtype=np.uint8)
     rgb_array = np.array(px, dtype=np.uint8)
@@ -166,6 +166,9 @@ class Camera:
 
   def move_and_look_at(self, i, j, k, x, y, z):
     lookat = [x, y, z]
-    distance = 10
-    yaw = 10
-    self.env._p.resetDebugVisualizerCamera(distance, yaw, -20, lookat)
+    camInfo = self.env._p.getDebugVisualizerCamera()
+    
+    distance = camInfo[10]
+    pitch = camInfo[9]
+    yaw = camInfo[8]
+    self.env._p.resetDebugVisualizerCamera(distance, yaw, pitch, lookat)

examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py

@@ -4,7 +4,6 @@ import inspect
 currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
 parentdir = os.path.dirname(os.path.dirname(currentdir))
 os.sys.path.insert(0, parentdir)
-import pybullet
 import gym
 import numpy as np
 import pybullet_envs
@@ -32,8 +31,8 @@ class SmallReactivePolicy:
 
 
 def main():
-  pybullet.connect(pybullet.DIRECT)
   env = gym.make("AntBulletEnv-v0")
+
   env.render(mode="human")
 
   pi = SmallReactivePolicy(env.observation_space, env.action_space)
@@ -55,7 +54,6 @@ def main():
       frame += 1
       distance = 5
       yaw = 0
-
       still_open = env.render("human")
       if still_open == False:
         return

examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py

@@ -49,7 +49,6 @@ def main():
       obs, r, done, _ = env.step(a)
       score += r
       frame += 1
-
       still_open = env.render("human")
       if still_open == False:
         return

examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py

@@ -51,7 +51,6 @@ def main():
       obs, r, done, _ = env.step(a)
       score += r
       frame += 1
-
       still_open = env.render("human")
       if still_open == False:
         return

examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py

@@ -43,16 +43,15 @@ def main():
     obs = env.reset()
 
     while 1:
+      time.sleep(1. / 60.)
       a = pi.act(obs)
       obs, r, done, _ = env.step(a)
       score += r
       frame += 1
-      time.sleep(1. / 60.)
-
       still_open = env.render("human")
       if still_open == False:
         return
-      if not done: continue
+      continue
       if restart_delay == 0:
         print("score=%0.2f in %i frames" % (score, frame))
         restart_delay = 60 * 2  # 2 sec at 60 fps

examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py

@@ -45,12 +45,13 @@ def demo_run():
     obs = env.reset()
 
     while 1:
+      if (gui):
+        time.sleep(1. / 60)
+
       a = pi.act(obs)
       obs, r, done, _ = env.step(a)
       score += r
       frame += 1
-      if (gui):
-        time.sleep(1. / 60)
 
       still_open = env.render("human")
 

examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py

@@ -23,6 +23,7 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv):
                                                   frame_skip=4)
     return self.stadium_scene
 
+
   def reset(self):
     if (self.stateId >= 0):
       #print("restoreState self.stateId:",self.stateId)
@@ -125,9 +126,10 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv):
     return state, sum(self.rewards), bool(done), {}
 
   def camera_adjust(self):
-    x, y, z = self.robot.body_xyz
-    self.camera_x = 0.98 * self.camera_x + (1 - 0.98) * x
-    self.camera.move_and_look_at(self.camera_x, y - 2.0, 1.4, x, y, 1.0)
+    x, y, z = self.robot.body_real_xyz
+
+    self.camera_x = x
+    self.camera.move_and_look_at(self.camera_x, y , 1.4, x, y, 1.0)
 
 
 class HopperBulletEnv(WalkerBaseBulletEnv):

examples/pybullet/gym/pybullet_envs/robot_locomotors.py

@@ -44,6 +44,7 @@ class WalkerBase(MJCFBasedRobot):
     parts_xyz = np.array([p.pose().xyz() for p in self.parts.values()]).flatten()
     self.body_xyz = (parts_xyz[0::3].mean(), parts_xyz[1::3].mean(), body_pose.xyz()[2]
                     )  # torso z is more informative than mean z
+    self.body_real_xyz = body_pose.xyz()
     self.body_rpy = body_pose.rpy()
     z = self.body_xyz[2]
     if self.initial_z == None: