Merge pull request #1308 from benelot/fix-gyms

Fix gyms to make them work just like robotschool

examples/pybullet/gym/pybullet_envs/env_bases.py

@@ -46,7 +46,7 @@ class MJCFBaseBulletEnv(gym.Env):
 				else:
 					self.physicsClientId = p.connect(p.DIRECT)
 		p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
-  
+
 		if self.scene is None:
 			self.scene = self.create_single_player_scene()
 		if not self.scene.multiplayer:

examples/pybullet/gym/pybullet_envs/robot_locomotors.py

@@ -152,23 +152,25 @@ class Humanoid(WalkerBase):
 		self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
 		self.motor_power += [75, 75, 75]
 		self.motors = [self.jdict[n] for n in self.motor_names]
-		# if self.random_yaw: # TODO: Make leaning work as soon as the rest works
-		# 	cpose = cpp_household.Pose()
-		# 	yaw = self.np_random.uniform(low=-3.14, high=3.14)
-		# 	if self.random_lean and self.np_random.randint(2)==0:
-		# 		cpose.set_xyz(0, 0, 1.4)
-		# 		if self.np_random.randint(2)==0:
-		# 			pitch = np.pi/2
-		# 			cpose.set_xyz(0, 0, 0.45)
-		# 		else:
-		# 			pitch = np.pi*3/2
-		# 			cpose.set_xyz(0, 0, 0.25)
-		# 		roll = 0
-		# 		cpose.set_rpy(roll, pitch, yaw)
-		# 	else:
-		# 		cpose.set_xyz(0, 0, 1.4)
-		# 		cpose.set_rpy(0, 0, yaw)  # just face random direction, but stay straight otherwise
-		# 	self.cpp_robot.set_pose_and_speed(cpose, 0,0,0)
+		if self.random_yaw:
+			position = [0,0,0]
+			orientation = [0,0,0]
+			yaw = self.np_random.uniform(low=-3.14, high=3.14)
+			if self.random_lean and self.np_random.randint(2)==0:
+				cpose.set_xyz(0, 0, 1.4)
+				if self.np_random.randint(2)==0:
+					pitch = np.pi/2
+					position = [0, 0, 0.45]
+				else:
+					pitch = np.pi*3/2
+					position = [0, 0, 0.25]
+				roll = 0
+				orientation = [roll, pitch, yaw]
+			else:
+				position = [0, 0, 1.4]
+				orientation = [0, 0, yaw]  # just face random direction, but stay straight otherwise
+			self.robot_body.reset_position(position)
+			self.robot_body.reset_orientation(orientation)
 		self.initial_z = 0.8
 
 	random_yaw = False

examples/pybullet/gym/pybullet_envs/robot_pendula.py

@@ -3,7 +3,6 @@ import numpy as np
 
 class InvertedPendulum(MJCFBasedRobot):
 	swingup = False
-	force_gain = 12 # TODO: Try to find out why we need to scale the force
 	def __init__(self):
 		MJCFBasedRobot.__init__(self, 'inverted_pendulum.xml', 'cart', action_dim=1, obs_dim=5)
 
@@ -16,16 +15,16 @@ class InvertedPendulum(MJCFBasedRobot):
 		self.j1.set_motor_torque(0)
 
 	def apply_action(self, a):
-		#assert( np.isfinite(a).all() )
+		assert( np.isfinite(a).all() )
 		if not np.isfinite(a).all():
 			print("a is inf")
 			a[0] = 0
-		self.slider.set_motor_torque( self.force_gain * 100*float(np.clip(a[0], -1, +1)) )
+		self.slider.set_motor_torque( 100*float(np.clip(a[0], -1, +1)) )
 
 	def calc_state(self):
 		self.theta, theta_dot = self.j1.current_position()
 		x, vx = self.slider.current_position()
-		#assert( np.isfinite(x) )
+		assert( np.isfinite(x) )
 
 		if not np.isfinite(x):
 			print("x is inf")
@@ -50,7 +49,6 @@ class InvertedPendulum(MJCFBasedRobot):
 
 class InvertedPendulumSwingup(InvertedPendulum):
 	swingup = True
-	force_gain = 2.2  # TODO: Try to find out why we need to scale the force
 
 
 class InvertedDoublePendulum(MJCFBasedRobot):
@@ -70,8 +68,7 @@ class InvertedDoublePendulum(MJCFBasedRobot):
 
 	def apply_action(self, a):
 		assert( np.isfinite(a).all() )
-		force_gain = 0.78  # TODO: Try to find out why we need to scale the force
-		self.slider.set_motor_torque( force_gain *200*float(np.clip(a[0], -1, +1)) )
+		self.slider.set_motor_torque( 200*float(np.clip(a[0], -1, +1)) )
 
 	def calc_state(self):
 		theta, theta_dot = self.j1.current_position()

examples/pybullet/gym/pybullet_envs/scene_abstract.py

@@ -15,10 +15,6 @@ class Scene:
 
         self.dt = self.timestep * self.frame_skip
         self.cpp_world = World(gravity, timestep, frame_skip)
-        #self.cpp_world.set_glsl_path(os.path.join(os.path.dirname(__file__), "cpp-household/glsl"))
-
-        #self.big_caption = self.cpp_world.test_window_big_caption  # that's a function you can call
-        #self.console_print = self.cpp_world.test_window_print      # this too
 
         self.test_window_still_open = True  # or never opened
         self.human_render_detected = False  # if user wants render("human"), we open test window
@@ -52,8 +48,6 @@ class Scene:
         The idea is: apply motor torques for all robots, then call global_step(), then collect
         observations from robots using step() with the same action.
         """
-        #if self.human_render_detected:
-        #    self.test_window_still_open = self.cpp_world.test_window()
         self.cpp_world.step(self.frame_skip)
 
 class SingleRobotEmptyScene(Scene):