Merge pull request bulletphysics#1302 from erwincoumans/master

test travis2

.travis.yml

@@ -13,7 +13,7 @@ addons:
 script:
   - echo "CXX="$CXX
   - echo "CC="$CC
-  - cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror
+  - cmake . -DBUILD_PYBULLET=OFF -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror
   - make -j8
   - ctest -j8 --output-on-failure
   # Build again with double precision

CMakeLists.txt

@@ -15,7 +15,6 @@ IF(COMMAND cmake_policy)
    endif(POLICY CMP0042)
 ENDIF(COMMAND cmake_policy)
 
-
 IF (NOT CMAKE_BUILD_TYPE)
 # SET(CMAKE_BUILD_TYPE "Debug")
  SET(CMAKE_BUILD_TYPE "Release")
@@ -38,14 +37,18 @@ IF (BULLET2_USE_THREAD_LOCKS)
     ENDIF (MSVC)
 ENDIF (BULLET2_USE_THREAD_LOCKS)
 
+
 IF(NOT WIN32)
 	SET(DL dl)
 	IF(CMAKE_SYSTEM_NAME MATCHES "Linux")
-	    SET(OSDEF -D_LINUX)
+		MESSAGE("Linux")
+		SET(OSDEF -D_LINUX)
 	ELSE(CMAKE_SYSTEM_NAME MATCHES "Linux")
 		IF(APPLE)
+			MESSAGE("Apple")
 			SET(OSDEF -D_DARWIN)
 		ELSE(APPLE)
+			MESSAGE("BSD?")
 			SET(OSDEF -D_BSD)
 			SET(DL "")
 		ENDIF(APPLE)

examples/CMakeLists.txt

@@ -2,6 +2,7 @@ SUBDIRS( HelloWorld BasicDemo )
 IF(BUILD_BULLET3)
 	SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow )
 ENDIF()
+
 IF(BUILD_PYBULLET)
 	SUBDIRS(pybullet)
 ENDIF(BUILD_PYBULLET)

examples/OpenGLWindow/CMakeLists.txt

@@ -50,7 +50,7 @@ ENDIF()
 
 ADD_LIBRARY(OpenGLWindow ${OpenGLWindow_SRCS} ${OpenGLWindow_HDRS})
 if (UNIX AND NOT APPLE)
-  target_link_libraries(OpenGLWindow )
+  target_link_libraries(OpenGLWindow ${DL})
 elseif (APPLE)
   target_link_libraries(OpenGLWindow ${COCOA_LIBRARY})
 endif ()

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -199,29 +199,53 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
 			btTransform tr;
 			tr.setIdentity();
 			btScalar rad, len;
-			if (visual->m_geometry.m_hasFromTo) {
+			btVector3 center(0,0,0);
+			btVector3 axis(0,0,1);
+			btAlignedObjectArray<btVector3> vertices;
+			int numSteps = 32;
+
+			if (visual->m_geometry.m_hasFromTo) 
+			{
 				btVector3 v      =  p2 - p1;
-				btVector3 center = (p2 + p1) * 0.5;
-				btVector3 up_vector(0,0,1);
 				btVector3 dir  = v.normalized();
-				btVector3 axis = dir.cross(up_vector);
-				if (axis.fuzzyZero())
-				{
-					axis = btVector3(0,0,1);
-				}
-				else
-				{
-					axis.normalize();
-				}
-				btQuaternion q(axis, -acos(dir.dot(up_vector)));
-				btTransform capsule_orient(q, center);
-				tr = visual->m_linkLocalFrame * capsule_orient;
+				tr = visual->m_linkLocalFrame;
 				len = v.length();
 				rad = visual->m_geometry.m_capsuleRadius;
+				btVector3 ax1,ax2;
+				btPlaneSpace1(dir,ax1,ax2);
+
+				for (int i = 0; i<numSteps; i++)
+				{
+					{
+						btVector3 vert = p1 + ax1*rad*btSin(SIMD_2_PI*(float(i) / numSteps))+ax2*rad*btCos(SIMD_2_PI*(float(i) / numSteps));
+						vertices.push_back(vert);
+					}
+					{
+						btVector3 vert = p2 + ax1*rad*btSin(SIMD_2_PI*(float(i) / numSteps))+ax2*rad*btCos(SIMD_2_PI*(float(i) / numSteps));
+						vertices.push_back(vert);
+					}
+				}
+				btVector3 pole1 = p1 - dir * rad;
+				btVector3 pole2 = p2 + dir * rad;
+				vertices.push_back(pole1);
+				vertices.push_back(pole2);
+
 			} else {
+				//assume a capsule along the Z-axis, centered at the origin
 				tr = visual->m_linkLocalFrame;
 				len = visual->m_geometry.m_capsuleHeight;
 				rad = visual->m_geometry.m_capsuleRadius;
+				for (int i = 0; i<numSteps; i++)
+				{
+					btVector3 vert(rad*btSin(SIMD_2_PI*(float(i) / numSteps)), rad*btCos(SIMD_2_PI*(float(i) / numSteps)), len / 2.);
+					vertices.push_back(vert);
+					vert[2] = -len / 2.;
+					vertices.push_back(vert);
+				}
+				btVector3 pole1(0, 0, + len / 2. + rad);
+				btVector3 pole2(0, 0, - len / 2. - rad);
+				vertices.push_back(pole1);
+				vertices.push_back(pole2);
 			}
 			visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0];
 			visualShapeOut.m_localVisualFrame[1] = tr.getOrigin()[1];
@@ -233,24 +257,9 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
 			visualShapeOut.m_dimensions[0] = len;
 			visualShapeOut.m_dimensions[1] = rad;
 
-			btAlignedObjectArray<btVector3> vertices;
-			int numSteps = 32;
-			for (int i = 0; i<numSteps; i++)
-			{
-				btVector3 vert(rad*btSin(SIMD_2_PI*(float(i) / numSteps)), rad*btCos(SIMD_2_PI*(float(i) / numSteps)), len / 2.);
-				vertices.push_back(vert);
-				vert[2] = -len / 2.;
-				vertices.push_back(vert);
-			}
-			if (visual->m_geometry.m_type==URDF_GEOM_CAPSULE) {
-				// TODO: check if tiny renderer works with that, didn't check -- Oleg
-				btVector3 pole1(0, 0, + len / 2. + rad);
-				btVector3 pole2(0, 0, - len / 2. - rad);
-				vertices.push_back(pole1);
-				vertices.push_back(pole2);
-			}
-
 			btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+			//btCapsuleShape* cylZShape = new btCapsuleShape(rad,len);//btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+
 			cylZShape->setMargin(0.001);
 			convexColShape = cylZShape;
 			break;

examples/pybullet/gym/pybullet_envs/env_bases.py

@@ -21,20 +21,23 @@ def __init__(self, robot, render=False):
 		self.camera = Camera()
 		self.isRender = render
 		self.robot = robot
-
 		self._seed()
+		self._cam_dist = 3
+		self._cam_yaw = 0
+		self._cam_pitch = -30
+		self._render_width =320
+		self._render_height = 240
 
 		self.action_space = robot.action_space
 		self.observation_space = robot.observation_space
-
+	def configure(self, args):
+		self.robot.args = args
 	def _seed(self, seed=None):
 		self.np_random, seed = gym.utils.seeding.np_random(seed)
 		self.robot.np_random = self.np_random # use the same np_randomizer for robot as for env
 		return [seed]
 
 	def _reset(self):
-		print("self.isRender=")
-		print(self.isRender)
 		if (self.physicsClientId<0):
 			if (self.isRender):
 				self.physicsClientId = p.connect(p.GUI)
@@ -60,6 +63,28 @@ def _reset(self):
 	def _render(self, mode, close):
 		if (mode=="human"):
 			self.isRender = True
+		if mode != "rgb_array":
+			return np.array([])
+		base_pos = self.robot.body_xyz
+
+		view_matrix = p.computeViewMatrixFromYawPitchRoll(
+			cameraTargetPosition=base_pos,
+			distance=self._cam_dist,
+			yaw=self._cam_yaw,
+			pitch=self._cam_pitch,
+			roll=0,
+			upAxisIndex=2)
+		proj_matrix = p.computeProjectionMatrixFOV(
+			fov=60, aspect=float(self._render_width)/self._render_height,
+			nearVal=0.1, farVal=100.0)
+		(_, _, px, _, _) = p.getCameraImage(
+		width=self._render_width, height=self._render_height, viewMatrix=view_matrix,
+			projectionMatrix=proj_matrix,
+			#renderer=p.ER_BULLET_HARDWARE_OPENGL
+			)
+		rgb_array = np.array(px)
+		rgb_array = rgb_array[:, :, :3]
+		return rgb_array
 
 	def _close(self):
 		if (self.physicsClientId>=0):

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

@@ -51,6 +51,8 @@ def main():
             time.sleep(0.01)
             a = pi.act(obs)
             obs, r, done, _ = env.step(a)
+            #print("reward")
+            #print(r)
             score += r
             frame += 1
             distance=5

examples/pybullet/gym/pybullet_envs/examples/testEnv.py

@@ -0,0 +1,48 @@
+import os
+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_envs
+import gym
+import argparse
+
+
+def test(args):
+
+	env = gym.make(args.env)
+	env.env.configure(args)
+	if (args.render):
+		env.render(mode="human")
+	env.reset()
+	print("action space:")
+	sample = env.action_space.sample()
+	action = sample*0.0
+	print("action=")
+	print(action)
+	for i in range(args.steps):
+		obs,rewards,done,_ =env.step(action)
+		if (args.rgb):
+			print(env.render(mode="rgb_array"))
+		print("obs=")
+		print(obs)
+		print("rewards")
+		print (rewards)
+		print ("done")
+		print(done)
+
+
+def main():
+    import argparse
+    parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    parser.add_argument('--env', help='environment ID', default='AntBulletEnv-v0')
+    parser.add_argument('--seed', help='RNG seed', type=int, default=0)
+    parser.add_argument('--render', help='OpenGL Visualizer', type=int, default=0)
+    parser.add_argument('--rgb',help='rgb_array gym rendering',type=int, default=0)
+    parser.add_argument('--steps', help='Number of steps', type=int, default=1)
+    parser.add_argument('--bla',help='bla',type=int, default=42)
+    args = parser.parse_args()
+    test(args)
+
+if __name__ == '__main__':
+    main()

examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py

@@ -15,7 +15,7 @@ def __init__(self, robot, render=False):
 		self.walk_target_y = 0
 
 	def create_single_player_scene(self):
-		self.stadium_scene = SinglePlayerStadiumScene(gravity=9.8, timestep=0.0165, frame_skip=4)
+		self.stadium_scene = SinglePlayerStadiumScene(gravity=9.8, timestep=0.0165/4, frame_skip=4)
 		return self.stadium_scene
 
 	def _reset(self):
@@ -63,15 +63,31 @@ def _step(self, a):
 		feet_collision_cost = 0.0
 		for i,f in enumerate(self.robot.feet): # TODO: Maybe calculating feet contacts could be done within the robot code
 			contact_ids = set((x[2], x[4]) for x in f.contact_list())
-			#print("CONTACT OF '%s' WITH %s" % (f.name, ",".join(contact_names)) )
-			self.robot.feet_contact[i] = 1.0 if (self.ground_ids & contact_ids) else 0.0
-			if contact_ids - self.ground_ids:
-				feet_collision_cost += self.foot_collision_cost
+			#print("CONTACT OF '%d' WITH %d" % (contact_ids, ",".join(contact_names)) )
+			if (self.ground_ids & contact_ids):
+                        	#see Issue 63: https://github.com/openai/roboschool/issues/63
+				#feet_collision_cost += self.foot_collision_cost
+				self.robot.feet_contact[i] = 1.0
+			else:
+				self.robot.feet_contact[i] = 0.0
+
 
 		electricity_cost  = self.electricity_cost  * float(np.abs(a*self.robot.joint_speeds).mean())  # let's assume we have DC motor with controller, and reverse current braking
 		electricity_cost += self.stall_torque_cost * float(np.square(a).mean())
 
 		joints_at_limit_cost = float(self.joints_at_limit_cost * self.robot.joints_at_limit)
+		debugmode=0
+		if(debugmode):
+			print("alive=")
+			print(alive)
+			print("progress")
+			print(progress)
+			print("electricity_cost")
+			print(electricity_cost)
+			print("joints_at_limit_cost")
+			print(joints_at_limit_cost)
+			print("feet_collision_cost")
+			print(feet_collision_cost)
 
 		self.rewards = [
 			alive,
@@ -80,8 +96,14 @@ def _step(self, a):
 			joints_at_limit_cost,
 			feet_collision_cost
 			]
-
+		if (debugmode):
+			print("rewards=")
+			print(self.rewards)
+			print("sum rewards")
+			print(sum(self.rewards))
 		self.HUD(state, a, done)
+		self.reward += sum(self.rewards)
+
 		return state, sum(self.rewards), bool(done), {}
 
 	def camera_adjust(self):

examples/pybullet/gym/pybullet_envs/robot_locomotors.py

@@ -9,7 +9,8 @@ def __init__(self, fn, robot_name, action_dim, obs_dim, power):
 		self.camera_x = 0
 		self.walk_target_x = 1e3  # kilometer away
 		self.walk_target_y = 0
-
+		self.body_xyz=[0,0,0]
+
 	def robot_specific_reset(self):
 		for j in self.ordered_joints:
 			j.reset_current_position(self.np_random.uniform(low=-0.1, high=0.1), 0)
@@ -62,6 +63,16 @@ def calc_state(self):
 	def calc_potential(self):
 		# progress in potential field is speed*dt, typical speed is about 2-3 meter per second, this potential will change 2-3 per frame (not per second),
 		# all rewards have rew/frame units and close to 1.0
+		debugmode=0
+		if (debugmode):
+			print("calc_potential: self.walk_target_dist")
+			print(self.walk_target_dist)
+			print("self.scene.dt")
+			print(self.scene.dt)
+			print("self.scene.frame_skip")
+			print(self.scene.frame_skip)
+			print("self.scene.timestep")
+			print(self.scene.timestep)
 		return - self.walk_target_dist / self.scene.dt
 
 

examples/pybullet/gym/pybullet_envs/scene_abstract.py

@@ -12,8 +12,9 @@ def __init__(self, gravity, timestep, frame_skip):
         self.np_random, seed = gym.utils.seeding.np_random(None)
         self.timestep = timestep
         self.frame_skip = frame_skip
+
         self.dt = self.timestep * self.frame_skip
-        self.cpp_world = World(gravity, timestep)
+        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
@@ -60,16 +61,17 @@ class SingleRobotEmptyScene(Scene):
 
 class World:
 
-	def __init__(self, gravity, timestep):
+	def __init__(self, gravity, timestep, frame_skip):
 		self.gravity = gravity
 		self.timestep = timestep
+		self.frame_skip = frame_skip
 		self.clean_everything()
 
 	def clean_everything(self):
 		p.resetSimulation()
 		p.setGravity(0, 0, -self.gravity)
 		p.setDefaultContactERP(0.9)
-		p.setPhysicsEngineParameter(fixedTimeStep=self.timestep, numSolverIterations=5, numSubSteps=4)
+		p.setPhysicsEngineParameter(fixedTimeStep=self.timestep*self.frame_skip, numSolverIterations=5, numSubSteps=self.frame_skip)
 
 	def step(self, frame_skip):
 		p.stepSimulation()

setup.py

@@ -440,7 +440,7 @@
 
 setup(
 	name = 'pybullet',
-	version='1.3.3',
+	version='1.3.5',
 	description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
 	long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
 	url='https://github.com/bulletphysics/bullet3',