Merge pull request #1307 from erwincoumans/master

fixes in libdl/DL, add testMJCF.py script, second try

18 files changed, 150 insertions, 46 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9e04fe3e7..8eb1c0b44 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,7 +39,7 @@ ENDIF (BULLET2_USE_THREAD_LOCKS)
 
 
 IF(NOT WIN32)
-	SET(DL dl)
+	SET(DL ${CMAKE_DL_LIBS})
 	IF(CMAKE_SYSTEM_NAME MATCHES "Linux")
 		MESSAGE("Linux")
 		SET(OSDEF -D_LINUX)
@@ -50,7 +50,6 @@ IF(NOT WIN32)
 		ELSE(APPLE)
 			MESSAGE("BSD?")
 			SET(OSDEF -D_BSD)
-			SET(DL "")
 		ENDIF(APPLE)
 	ENDIF(CMAKE_SYSTEM_NAME MATCHES "Linux")
 ENDIF(NOT WIN32)
diff --git a/build_visual_studio_vr_pybullet_double_cmake.bat b/build_visual_studio_vr_pybullet_double_cmake.bat
index ea53e6c6a..72158083c 100644
--- a/build_visual_studio_vr_pybullet_double_cmake.bat
+++ b/build_visual_studio_vr_pybullet_double_cmake.bat
@@ -1,4 +1,4 @@
 mkdir build_cmake
 cd build_cmake
 cmake -DBUILD_PYBULLET=ON -DUSE_DOUBLE_PRECISION=ON -DCMAKE_BUILD_TYPE=Release -DPYTHON_INCLUDE_DIR=c:\python-3.5.3.amd64\include -DPYTHON_LIBRARY=c:\python-3.5.3.amd64\libs\python35.lib -DPYTHON_DEBUG_LIBRARY=c:\python-3.5.3.amd64\libs\python35_d.lib -G "Visual Studio 14 2015 Win64" ..
-start .
+start .
\ No newline at end of file
diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp
index 7672e1962..0fe118331 100644
--- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp
+++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp
@@ -1204,10 +1204,13 @@ void OpenGLExampleBrowser::updateGraphics()
 
 void OpenGLExampleBrowser::update(float deltaTime)
 {
-	if (!gEnableRenderLoop)
-		return;
+    b3ChromeUtilsEnableProfiling();
 
-	b3ChromeUtilsEnableProfiling();
+    if (!gEnableRenderLoop)
+    {
+        sCurrentDemo->updateGraphics();
+		return;
+    }
 	
 		B3_PROFILE("OpenGLExampleBrowser::update");
 		assert(glGetError()==GL_NO_ERROR);
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
index c4a370c0d..e4e7f68ba 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
@@ -380,29 +380,59 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
 	else if (type_name == "cylinder")
 	{
 		geom.m_type = URDF_GEOM_CYLINDER;
-		if (!shape->Attribute("length") ||
-			!shape->Attribute("radius"))
-	  {
-		  logger->reportError("Cylinder shape must have both length and radius attributes");
-		  return false;
-	  }
 		geom.m_hasFromTo = false;
-		geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
-		geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
+		geom.m_capsuleRadius = 0.1;
+		geom.m_capsuleHeight = 0.1;
 		
+		if (m_parseSDF)
+		{
+			if (TiXmlElement* scale = shape->FirstChildElement("radius"))
+			{
+				parseVector3(geom.m_meshScale,scale->GetText(),logger);
+				geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(scale->GetText());	
+			}
+			if (TiXmlElement* scale = shape->FirstChildElement("length"))
+			{
+				parseVector3(geom.m_meshScale,scale->GetText(),logger);
+				geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(scale->GetText());	
+			}
+		} else
+		{
+			if (!shape->Attribute("length") ||!shape->Attribute("radius"))
+			{
+				logger->reportError("Cylinder shape must have both length and radius attributes");
+				return false;
+			}
+			geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
+			geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
+		}
 	}
 	else if (type_name == "capsule")
 	{
 		geom.m_type = URDF_GEOM_CAPSULE;
-		if (!shape->Attribute("length") ||
-			!shape->Attribute("radius"))
+		geom.m_hasFromTo = false;
+		if (m_parseSDF)
 		{
-			logger->reportError("Capsule shape must have both length and radius attributes");
-			return false;
+			if (TiXmlElement* scale = shape->FirstChildElement("radius"))
+			{
+				parseVector3(geom.m_meshScale,scale->GetText(),logger);
+				geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(scale->GetText());	
+			}
+			if (TiXmlElement* scale = shape->FirstChildElement("length"))
+			{
+				parseVector3(geom.m_meshScale,scale->GetText(),logger);
+				geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(scale->GetText());	
+			}
+		} else
+		{
+			if (!shape->Attribute("length") || !shape->Attribute("radius"))
+			{
+				logger->reportError("Capsule shape must have both length and radius attributes");
+				return false;
+			}
+			geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
+			geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
 		}
-		geom.m_hasFromTo = false;
-		geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
-		geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
 	}
 	else if (type_name == "mesh")
 	{
diff --git a/examples/OpenGLWindow/CMakeLists.txt b/examples/OpenGLWindow/CMakeLists.txt
index 385b32216..78e2c2d9e 100644
--- a/examples/OpenGLWindow/CMakeLists.txt
+++ b/examples/OpenGLWindow/CMakeLists.txt
@@ -62,7 +62,7 @@ if (BUILD_SHARED_LIBS)
   else()
 	set (CMAKE_THREAD_PREFER_PTHREAD TRUE)
 	FIND_PACKAGE(Threads)
-	target_link_libraries(OpenGLWindow ${DL} ${CMAKE_THREAD_LIBS_INIT})
+	target_link_libraries(OpenGLWindow ${CMAKE_THREAD_LIBS_INIT})
   endif()
 endif()
 
diff --git a/examples/SharedMemory/PosixSharedMemory.cpp b/examples/SharedMemory/PosixSharedMemory.cpp
index 9c16b3fe7..43dcc86eb 100644
--- a/examples/SharedMemory/PosixSharedMemory.cpp
+++ b/examples/SharedMemory/PosixSharedMemory.cpp
@@ -91,7 +91,7 @@ void*   PosixSharedMemory::allocateSharedMemory(int key, int size,  bool allowCr
     int id = shmget((key_t) key, (size_t) size,flags);
     if (id < 0)
     {
-        b3Error("shmget error");
+        b3Warning("shmget error");
     } else
     {
         btPointerCaster result;
diff --git a/examples/ThirdPartyLibs/Gwen/Macros.h b/examples/ThirdPartyLibs/Gwen/Macros.h
index 26585b21d..71d878c12 100644
--- a/examples/ThirdPartyLibs/Gwen/Macros.h
+++ b/examples/ThirdPartyLibs/Gwen/Macros.h
@@ -4,8 +4,10 @@
 #define GWEN_MACROS_H
 #include <stdlib.h>
 #include <stdarg.h>
-#if !defined(__APPLE__) && !defined(__OpenBSD__)
-#include <malloc.h>
+#if !defined(__APPLE__) && !defined(__OpenBSD__) && !defined(__FreeBSD__)
+	#include <malloc.h>
+#else
+	#include <stdlib.h>
 #endif //__APPLE__
 #include <memory.h>
 #include <algorithm>
diff --git a/examples/pybullet/examples/getAABB.py b/examples/pybullet/examples/getAABB.py
index ef1d3ee3f..a43c144e1 100644
--- a/examples/pybullet/examples/getAABB.py
+++ b/examples/pybullet/examples/getAABB.py
@@ -74,4 +74,5 @@ for i in range (p.getNumJoints(r2d2)):
 
 
 while(1):
-	a=0
\ No newline at end of file
+	a=0
+	p.stepSimulation()
diff --git a/examples/pybullet/examples/humanoid_benchmark.py b/examples/pybullet/examples/humanoid_benchmark.py
index 30d479443..5c056cdaa 100644
--- a/examples/pybullet/examples/humanoid_benchmark.py
+++ b/examples/pybullet/examples/humanoid_benchmark.py
@@ -6,10 +6,10 @@ p.setPhysicsEngineParameter(numSolverIterations=5)
 p.setPhysicsEngineParameter(fixedTimeStep=1./240.)
 p.setPhysicsEngineParameter(numSubSteps=1)
 
+p.loadURDF("plane.urdf")
+
 objects = p.loadMJCF("mjcf/humanoid_symmetric.xml")
 ob = objects[0]
-p.resetBasePositionAndOrientation(ob,[0.000000,0.000000,0.000000],[0.000000,0.000000,0.000000,1.000000])
-ob = objects[1]
 p.resetBasePositionAndOrientation(ob,[0.789351,0.962124,0.113124],[0.710965,0.218117,0.519402,-0.420923])
 jointPositions=[ -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374, 0.000000 ]
 for jointIndex in range (p.getNumJoints(ob)):
@@ -24,9 +24,13 @@ p.setRealTimeSimulation(0)
 
 #now do a benchmark
 print("Starting benchmark")
-logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"pybullet_humanoid_timings.json")
+fileName = "pybullet_humanoid_timings.json"
+
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,fileName)
 for i in range(1000):
 	p.stepSimulation()
 p.stopStateLogging(logId)
 
-print("ended benchmark")
\ No newline at end of file
+print("ended benchmark")
+print("Use Chrome browser, visit about://tracing, and load the %s file" % fileName)
+
diff --git a/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py b/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py
index 6d4749eb9..fcdedf10c 100644
--- a/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py
+++ b/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py
@@ -8,7 +8,9 @@ class BulletClient(object):
 
   def __init__(self, connection_mode=pybullet.DIRECT):
     """Create a simulation and connect to it."""
-    self._client = pybullet.connect(connection_mode)
+    self._client = -1 #pybullet.connect(pybullet.SHARED_MEMORY)
+    if(self._client<0):
+      self._client = pybullet.connect(connection_mode)
     self._shapes = {}
 
   def __del__(self):
diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py
index 56c9fb864..903727b07 100644
--- a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py
+++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py
@@ -37,7 +37,9 @@ class KukaGymEnv(gym.Env):
     self.terminated = 0
     self._p = p
     if self._renders:
-      p.connect(p.GUI)
+      cid = -1 #p.connect(p.SHARED_MEMORY)
+      if (cid<0):
+         cid = p.connect(p.GUI)
       p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
     else:
       p.connect(p.DIRECT)
diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py
index 0822255cb..d74906408 100644
--- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py
+++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py
@@ -166,6 +166,9 @@ class MinitaurBulletEnv(gym.Env):
   def set_env_randomizer(self, env_randomizer):
     self._env_randomizer = env_randomizer
 
+  def configure(self, args):
+    self._args = args
+
   def _reset(self):
     if self._hard_reset:
       self._pybullet_client.resetSimulation()
diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py
index 5fcbd75f7..507f6cb1a 100644
--- a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py
+++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py
@@ -25,7 +25,7 @@ class RacecarZEDGymEnv(gym.Env):
                urdfRoot=pybullet_data.getDataPath(),
                actionRepeat=10,
                isEnableSelfCollision=True,
-               isDiscrete=True,
+               isDiscrete=False,
                renders=True):
     print("init")
     self._timeStep = 0.01
@@ -52,7 +52,13 @@ class RacecarZEDGymEnv(gym.Env):
     #print(observationDim)
     
     observation_high = np.array([np.finfo(np.float32).max] * observationDim)    
-    self.action_space = spaces.Discrete(9)
+    if (isDiscrete):
+      self.action_space = spaces.Discrete(9)
+    else:
+       action_dim = 2
+       self._action_bound = 1
+       action_high = np.array([self._action_bound] * action_dim)
+       self.action_space = spaces.Box(-action_high, action_high)
     self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4))
 
     self.viewer = None
@@ -119,11 +125,14 @@ class RacecarZEDGymEnv(gym.Env):
       basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
       #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos)
     
-    fwd = [-1,-1,-1,0,0,0,1,1,1]
-    steerings = [-0.6,0,0.6,-0.6,0,0.6,-0.6,0,0.6]
-    forward = fwd[action]
-    steer = steerings[action]
-    realaction = [forward,steer]
+    if (self._isDiscrete):
+            fwd = [-1,-1,-1,0,0,0,1,1,1]
+            steerings = [-0.6,0,0.6,-0.6,0,0.6,-0.6,0,0.6]
+            forward = fwd[action]
+            steer = steerings[action]
+            realaction = [forward,steer]
+    else:
+      realaction = action
 
     self._racecar.applyAction(realaction)
     for i in range(self._actionRepeat):
diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py
index 8b16cd7be..3379ae4b4 100644
--- a/examples/pybullet/gym/pybullet_envs/env_bases.py
+++ b/examples/pybullet/gym/pybullet_envs/env_bases.py
@@ -39,10 +39,12 @@ class MJCFBaseBulletEnv(gym.Env):
 
 	def _reset(self):
 		if (self.physicsClientId<0):
-			if (self.isRender):
-				self.physicsClientId = p.connect(p.GUI)
-			else:
-				self.physicsClientId = p.connect(p.DIRECT)
+			self.physicsClientId = -1 #p.connect(p.SHARED_MEMORY)
+			if (self.physicsClientId<0):
+				if (self.isRender):
+					self.physicsClientId = p.connect(p.GUI)
+				else:
+					self.physicsClientId = p.connect(p.DIRECT)
 		p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
   
 		if self.scene is None:
diff --git a/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py
index 176846651..64521b121 100644
--- a/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py
+++ b/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py
@@ -8,7 +8,7 @@ from pybullet_envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv
 
 def main():
 	
-	environment = RacecarZEDGymEnv(renders=True)
+	environment = RacecarZEDGymEnv(renders=True, isDiscrete=True)
 	
 	targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
 	steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
@@ -37,4 +37,4 @@ def main():
 	  print(obs)
 
 if __name__=="__main__":
-    main()
\ No newline at end of file
+    main()
diff --git a/examples/pybullet/gym/pybullet_envs/examples/runServer.py b/examples/pybullet/gym/pybullet_envs/examples/runServer.py
new file mode 100644
index 000000000..8c37caa9d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/examples/runServer.py
@@ -0,0 +1,18 @@
+#add parent dir to find package. Only needed for source code build, pip install doesn't need it.
+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_data
+import pybullet as p
+import time
+
+p.connect(p.GUI_SERVER)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+while(1):
+	time.sleep(0.01)
+	p.getNumBodies()
+	
diff --git a/examples/pybullet/gym/pybullet_envs/examples/testMJCF.py b/examples/pybullet/gym/pybullet_envs/examples/testMJCF.py
new file mode 100644
index 000000000..ebdef3b38
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/examples/testMJCF.py
@@ -0,0 +1,28 @@
+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 as p
+import pybullet_data
+import time
+
+def test(args):	
+	p.connect(p.GUI)
+	p.setAdditionalSearchPath(pybullet_data.getDataPath())
+	fileName = os.path.join("mjcf", args.mjcf)
+	print("fileName")
+	print(fileName)
+	p.loadMJCF(fileName)
+	while (1):
+		p.stepSimulation()
+		p.getCameraImage(320,240)
+		time.sleep(0.01)
+	
+if __name__ == '__main__':
+	import argparse
+	parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+	parser.add_argument('--mjcf', help='MJCF filename', default="humanoid.xml")
+	args = parser.parse_args()
+	test(args)
\ No newline at end of file
diff --git a/setup.py b/setup.py
index 2ef9bd97d..395bd22b9 100644
--- a/setup.py
+++ b/setup.py
@@ -440,7 +440,7 @@ print("-----")
 
 setup(
 	name = 'pybullet',
-	version='1.3.5',
+	version='1.3.8',
 	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',
@@ -467,6 +467,7 @@ setup(
                    'Programming Language :: Python :: 3.5',
                    'Programming Language :: Python :: 3.6',
                    'Topic :: Games/Entertainment :: Simulation',
+                   'Topic :: Scientific/Engineering :: Artificial Intelligence',
                    'Framework :: Robot Framework'],
     package_dir = { '': 'examples/pybullet/gym'},
     packages=[x for x in find_packages('examples/pybullet/gym')],