Merge branch 'master' of https://github.com/erwincoumans/bullet3

MANIFEST.in

@@ -5,7 +5,6 @@ recursive-include Extras *.h
 recursive-include Extras *.hpp
 recursive-include src *.h
 recursive-include src *.hpp
-recursive-include data *.*
 recursive-include examples/pybullet/gym *.*
 include examples/ThirdPartyLibs/enet/unix.c
 include examples/OpenGLWindow/X11OpenGLWindow.cpp

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp

@@ -187,6 +187,7 @@ struct BulletMJCFImporterInternalData
 
 	//those collision shapes are deleted by caller (todo: make sure this happens!)
 	btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
+	mutable btAlignedObjectArray<btTriangleMesh*> m_allocatedMeshInterfaces;
 
 	BulletMJCFImporterInternalData()
 		:m_activeModel(-1),
@@ -1855,6 +1856,8 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes( int linkI
 					{
 
 						btTriangleMesh* meshInterface = new btTriangleMesh();
+						m_data->m_allocatedMeshInterfaces.push_back(meshInterface);
+
 						for (int i=0;i<glmesh->m_numIndices/3;i++)
 						{
 							float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
@@ -1926,6 +1929,15 @@ class btCollisionShape* BulletMJCFImporter::getAllocatedCollisionShape(int index
 	return m_data->m_allocatedCollisionShapes[index];
 }
 
+int BulletMJCFImporter::getNumAllocatedMeshInterfaces() const
+{
+    return m_data->m_allocatedMeshInterfaces.size();
+}
+
+btStridingMeshInterface* BulletMJCFImporter::getAllocatedMeshInterface(int index)
+{
+    return m_data->m_allocatedMeshInterfaces[index];
+}
 
 int BulletMJCFImporter::getNumModels() const
 {

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h

@@ -78,6 +78,9 @@ public:
 	virtual int getNumModels() const;
     virtual void activateModel(int modelIndex);
 
+	virtual int getNumAllocatedMeshInterfaces() const;
+	virtual btStridingMeshInterface* getAllocatedMeshInterface(int index);
+
 
 };
 

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -54,6 +54,7 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
 	int m_bodyId;
 	btHashMap<btHashInt,UrdfMaterialColor> m_linkColors;
     btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
+	mutable btAlignedObjectArray<btTriangleMesh*> m_allocatedMeshInterfaces;
 	
 	LinkVisualShapesConverter* m_customVisualShapesConverter;
 
@@ -572,7 +573,7 @@ bool findExistingMeshFile(
 	}
 }
 
-btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
+btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix) const
 {
 	BT_PROFILE("convertURDFToCollisionShape");
 
@@ -775,6 +776,7 @@ upAxisMat.setIdentity();
 		{
 			BT_PROFILE("convert trimesh");
 			btTriangleMesh* meshInterface = new btTriangleMesh();
+			m_data->m_allocatedMeshInterfaces.push_back(meshInterface);
 			{
 				BT_PROFILE("convert vertices");
 
@@ -1229,6 +1231,19 @@ btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index)
     return m_data->m_allocatedCollisionShapes[index];
 }
 
+int BulletURDFImporter::getNumAllocatedMeshInterfaces() const
+{
+    return m_data->m_allocatedMeshInterfaces.size();
+}
+
+
+btStridingMeshInterface* BulletURDFImporter::getAllocatedMeshInterface(int index)
+{
+    return m_data->m_allocatedMeshInterfaces[index];
+}
+
+
+
  class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
 {
         

examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -62,6 +62,8 @@ public:
 
     virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
 
+	class btCollisionShape* convertURDFToCollisionShape(const struct UrdfCollision* collision, const char* urdfPathPrefix) const;
+
     ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
     
 	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
@@ -69,6 +71,8 @@ public:
     virtual int getNumAllocatedCollisionShapes() const;
     virtual class btCollisionShape* getAllocatedCollisionShape(int index);
 
+	virtual int getNumAllocatedMeshInterfaces() const;
+	virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index);
 
 };
 

examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -80,6 +80,8 @@ public:
     virtual class btCollisionShape* getAllocatedCollisionShape(int /*index*/ ) {return 0;}
 	virtual int getNumModels() const {return 0;}
     virtual void activateModel(int /*modelIndex*/) { }
+	virtual int getNumAllocatedMeshInterfaces() const { return 0;}
+	virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index) {return 0;}
 
 };
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1191,6 +1191,7 @@ struct PhysicsServerCommandProcessorInternalData
 	btAlignedObjectArray<std::string*> m_strings;
 
 	btAlignedObjectArray<btCollisionShape*>	m_collisionShapes;
+	btAlignedObjectArray<btStridingMeshInterface*> m_meshInterfaces;
 
 	MyOverlapFilterCallback* m_broadphaseCollisionFilterCallback;
 	btHashedOverlappingPairCache* m_pairCache;
@@ -2027,6 +2028,11 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
 		btCollisionShape* shape = m_data->m_collisionShapes[j];
 		delete shape;
 	}
+	for (int j=0;j<m_data->m_meshInterfaces.size();j++)
+	{
+		delete m_data->m_meshInterfaces[j];
+	}
+	m_data->m_meshInterfaces.clear();
 	m_data->m_collisionShapes.clear();
 
 	delete m_data->m_dynamicsWorld;
@@ -2233,6 +2239,11 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
 
     }
 
+	for (int i=0;i<u2b.getNumAllocatedMeshInterfaces();i++)
+	{
+		m_data->m_meshInterfaces.push_back(u2b.getAllocatedMeshInterface(i));
+	}
+
 	for (int i=0;i<u2b.getNumAllocatedCollisionShapes();i++)
     {
         btCollisionShape* shape =u2b.getAllocatedCollisionShape(i);
@@ -2278,7 +2289,6 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS
     bool loadOk =  u2b.loadMJCF(fileName, &logger, useFixedBase);
     if (loadOk)
 	{
-		
 		processImportedObjects(fileName,bufferServerToClient,bufferSizeInBytes,useMultiBody,flags, u2b);
 	}
 	return loadOk;
@@ -3787,6 +3797,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 
 											BT_PROFILE("convert trimesh");
 											btTriangleMesh* meshInterface = new btTriangleMesh();
+											this->m_data->m_meshInterfaces.push_back(meshInterface);
 											{
 												BT_PROFILE("convert vertices");
 
@@ -3801,6 +3812,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 											{
 												BT_PROFILE("create btBvhTriangleMeshShape");
 												btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
+												m_data->m_collisionShapes.push_back(trimesh);
 												//trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
 												shape = trimesh;
 												if (compound)
@@ -3808,6 +3820,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 													compound->addChildShape(childTransform,shape);
 												}
 											}
+											delete glmesh;
 										} else
 										{
 

examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py

@@ -22,7 +22,7 @@ class BulletClient(object):
     """Inject the client id into Bullet functions."""
     attribute = getattr(pybullet, name)
     if inspect.isbuiltin(attribute):
-      if name not in ["invertTransform", "multiplyTransforms",
+      if name not in ["invertTransform", "computeViewMatrix","multiplyTransforms",
                       "getMatrixFromQuaternion"]:  # A temporary hack for now.
         attribute = functools.partial(attribute, physicsClientId=self._client)
     return attribute

examples/pybullet/gym/pybullet_envs/bullet/minitaur.py

@@ -4,7 +4,7 @@
 import copy
 import math
 import numpy as np
-import motor
+from . import motor
 import os
 
 INIT_POSITION = [0, 0, .2]

examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py

@@ -8,8 +8,8 @@ from gym import spaces
 from gym.utils import seeding
 import numpy as np
 import pybullet
-import bullet_client
-import minitaur
+from . import bullet_client
+from . import minitaur
 import os
 
 NUM_SUBSTEPS = 5

examples/pybullet/gym/pybullet_envs/bullet/racecar.py

@@ -12,18 +12,44 @@ class Racecar:
 		self.reset()
 
 	def reset(self):  	
-    self.racecarUniqueId = self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","racecar/racecar.urdf"), [0,0,.2])
+		car = self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","racecar/racecar_differential.urdf"), [0,0,.2],useFixedBase=False)
+		self.racecarUniqueId = car
+		#for i in range (self._p.getNumJoints(car)):
+		#	print (self._p.getJointInfo(car,i))
+		for wheel in range(self._p.getNumJoints(car)):
+				self._p.setJointMotorControl2(car,wheel,self._p.VELOCITY_CONTROL,targetVelocity=0,force=0)
+				self._p.getJointInfo(car,wheel)	
+
+		#self._p.setJointMotorControl2(car,10,self._p.VELOCITY_CONTROL,targetVelocity=1,force=10)
+		c = self._p.createConstraint(car,9,car,11,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+		self._p.changeConstraint(c,gearRatio=1, maxForce=10000)
+
+		c = self._p.createConstraint(car,10,car,13,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+		self._p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+		c = self._p.createConstraint(car,9,car,13,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+		self._p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+		c = self._p.createConstraint(car,16,car,18,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+		self._p.changeConstraint(c,gearRatio=1, maxForce=10000)
+
+		c = self._p.createConstraint(car,16,car,19,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+		self._p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+		c = self._p.createConstraint(car,17,car,19,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+		self._p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+		c = self._p.createConstraint(car,1,car,18,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+		self._p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000)
+		c = self._p.createConstraint(car,3,car,19,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+		self._p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000)
+
+		self.steeringLinks = [0,2]
 		self.maxForce = 20
 		self.nMotors = 2
-    self.motorizedwheels=[2]
-    self.inactiveWheels = [3,5,7]
-    for wheel in self.inactiveWheels:
-      self._p.setJointMotorControl2(self.racecarUniqueId,wheel,self._p.VELOCITY_CONTROL,targetVelocity=0,force=0)
-
-    self.motorizedWheels = [2]
-    self.steeringLinks=[4,6]
-    self.speedMultiplier = 4.
-    
+		self.motorizedwheels=[8,15]
+		self.speedMultiplier = 20.
+		self.steeringMultiplier = 0.5
 
 	def getActionDimension(self):
 		return self.nMotors
@@ -44,13 +70,12 @@ class Racecar:
 		targetVelocity=motorCommands[0]*self.speedMultiplier
 		#print("targetVelocity")
 		#print(targetVelocity)
-    steeringAngle = motorCommands[1]
+		steeringAngle = motorCommands[1]*self.steeringMultiplier
 		#print("steeringAngle")
 		#print(steeringAngle)
 		#print("maxForce")
 		#print(self.maxForce)
 
-    
 		for motor in self.motorizedwheels:
 			self._p.setJointMotorControl2(self.racecarUniqueId,motor,self._p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=self.maxForce)
 		for steer in self.steeringLinks:

examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py

@@ -8,7 +8,7 @@ import time
 import pybullet
 from . import racecar
 import random
-import bullet_client
+from . import bullet_client
 
 class RacecarGymEnv(gym.Env):
   metadata = {
@@ -20,6 +20,7 @@ class RacecarGymEnv(gym.Env):
                urdfRoot="",
                actionRepeat=50,
                isEnableSelfCollision=True,
+               isDiscrete=False,
                renders=False):
     print("init")
     self._timeStep = 0.01
@@ -30,6 +31,7 @@ class RacecarGymEnv(gym.Env):
     self._ballUniqueId = -1
     self._envStepCounter = 0
     self._renders = renders
+    self._isDiscrete = isDiscrete
     if self._renders:
       self._p = bullet_client.BulletClient(
           connection_mode=pybullet.GUI)
@@ -42,7 +44,13 @@ class RacecarGymEnv(gym.Env):
     #print("observationDim")
     #print(observationDim) 
     observation_high = np.array([np.finfo(np.float32).max] * observationDim)    
+    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(-observation_high, observation_high)
     self.viewer = None
 
@@ -53,10 +61,10 @@ class RacecarGymEnv(gym.Env):
     #self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf"))
     stadiumobjects = self._p.loadSDF(os.path.join(os.path.dirname(__file__),"../data","stadium.sdf"))
     #move the stadium objects slightly above 0
-    for i in stadiumobjects:
-    	pos,orn = self._p.getBasePositionAndOrientation(i)
-    	newpos = [pos[0],pos[1],pos[2]+0.1]
-    	self._p.resetBasePositionAndOrientation(i,newpos,orn)
+    #for i in stadiumobjects:
+    #	pos,orn = self._p.getBasePositionAndOrientation(i)
+    #	newpos = [pos[0],pos[1],pos[2]-0.1]
+    #	self._p.resetBasePositionAndOrientation(i,newpos,orn)
     
     dist = 5 +2.*random.random()
     ang = 2.*3.1415925438*random.random()
@@ -96,11 +104,15 @@ class RacecarGymEnv(gym.Env):
       basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
       #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos)
     
-    fwd = [-5,-5,-5,0,0,0,5,5,5]
-    steerings = [-0.3,0,0.3,-0.3,0,0.3,-0.3,0,0.3]
+    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):
       self._p.stepSimulation()

examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py

@@ -5,7 +5,8 @@ from gym import spaces
 from gym.utils import seeding
 import numpy as np
 import time
-import pybullet as p
+import pybullet
+from . import bullet_client
 from . import racecar
 import random
 
@@ -17,8 +18,9 @@ class RacecarZEDGymEnv(gym.Env):
 
   def __init__(self,
                urdfRoot="",
-               actionRepeat=100,
+               actionRepeat=10,
                isEnableSelfCollision=True,
+               isDiscrete=True,
                renders=True):
     print("init")
     self._timeStep = 0.01
@@ -30,11 +32,14 @@ class RacecarZEDGymEnv(gym.Env):
     self._renders = renders
     self._width = 100
     self._height = 10
-    self._p = p
+    
+    self._isDiscrete = isDiscrete
     if self._renders:
-      p.connect(p.GUI)
+      self._p = bullet_client.BulletClient(
+          connection_mode=pybullet.GUI)
     else:
-      p.connect(p.DIRECT)
+      self._p = bullet_client.BulletClient()
+
     self._seed()
     self.reset()
     observationDim = len(self.getExtendedObservation())
@@ -42,22 +47,22 @@ class RacecarZEDGymEnv(gym.Env):
     #print(observationDim)
     
     observation_high = np.array([np.finfo(np.float32).max] * observationDim)    
-    self.action_space = spaces.Discrete(6)
+    self.action_space = spaces.Discrete(9)
     self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4))
 
     self.viewer = None
 
   def _reset(self):
-    p.resetSimulation()
+    self._p.resetSimulation()
     #p.setPhysicsEngineParameter(numSolverIterations=300)
-    p.setTimeStep(self._timeStep)
-    #p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf"))
-    stadiumobjects = p.loadSDF(os.path.join(os.path.dirname(__file__),"../data","stadium.sdf"))
+    self._p.setTimeStep(self._timeStep)
+    #self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf"))
+    stadiumobjects = self._p.loadSDF(os.path.join(os.path.dirname(__file__),"../data","stadium.sdf"))
     #move the stadium objects slightly above 0
     for i in stadiumobjects:
-      pos,orn = p.getBasePositionAndOrientation(i)
+      pos,orn = self._p.getBasePositionAndOrientation(i)
       newpos = [pos[0],pos[1],pos[2]+0.1]
-      p.resetBasePositionAndOrientation(i,newpos,orn)
+      self._p.resetBasePositionAndOrientation(i,newpos,orn)
     
     dist = 5 +2.*random.random()
     ang = 2.*3.1415925438*random.random()
@@ -66,39 +71,39 @@ class RacecarZEDGymEnv(gym.Env):
     bally = dist * math.cos(ang)
     ballz = 1
     
-    self._ballUniqueId = p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","sphere2red.urdf"),[ballx,bally,ballz])
-    p.setGravity(0,0,-10)
-    self._racecar = racecar.Racecar(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
+    self._ballUniqueId = self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","sphere2red.urdf"),[ballx,bally,ballz])
+    self._p.setGravity(0,0,-10)
+    self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
     self._envStepCounter = 0
     for i in range(100):
-      p.stepSimulation()
+      self._p.stepSimulation()
     self._observation = self.getExtendedObservation()
     return np.array(self._observation)
 
   def __del__(self):
-    p.disconnect()
+    self._p = 0
 
   def _seed(self, seed=None):
     self.np_random, seed = seeding.np_random(seed)
     return [seed]
 
   def getExtendedObservation(self):
-     carpos,carorn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
-     carmat = p.getMatrixFromQuaternion(carorn)
-     ballpos,ballorn = p.getBasePositionAndOrientation(self._ballUniqueId)      
-     invCarPos,invCarOrn = p.invertTransform(carpos,carorn)
-     ballPosInCar,ballOrnInCar = p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn)
+     carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
+     carmat = self._p.getMatrixFromQuaternion(carorn)
+     ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId)      
+     invCarPos,invCarOrn = self._p.invertTransform(carpos,carorn)
+     ballPosInCar,ballOrnInCar = self._p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn)
      dist0 = 0.3
      dist1 = 1.
      eyePos = [carpos[0]+dist0*carmat[0],carpos[1]+dist0*carmat[3],carpos[2]+dist0*carmat[6]+0.3]
      targetPos = [carpos[0]+dist1*carmat[0],carpos[1]+dist1*carmat[3],carpos[2]+dist1*carmat[6]+0.3]      
      up = [carmat[2],carmat[5],carmat[8]]
-     viewMat = p.computeViewMatrix(eyePos,targetPos,up)
-     #viewMat = p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2)
+     viewMat = self._p.computeViewMatrix(eyePos,targetPos,up)
+     #viewMat = self._p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2)
      #print("projectionMatrix:")
-     #print(p.getDebugVisualizerCamera()[3])
+     #print(self._p.getDebugVisualizerCamera()[3])
      projMatrix = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
-     img_arr = p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix)
+     img_arr = self._p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix)
      rgb=img_arr[2]
      np_img_arr = np.reshape(rgb, (self._height, self._width, 4))
      self._observation = np_img_arr
@@ -106,17 +111,18 @@ class RacecarZEDGymEnv(gym.Env):
      
   def _step(self, action):
     if (self._renders):
-      basePos,orn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
-      #p.resetDebugVisualizerCamera(1, 30, -40, basePos)
+      basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
+      #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos)
     
-    fwd = [5,0,5,10,10,10]
-    steerings = [-0.5,0,0.5,-0.3,0,0.3]
+    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]
+
     self._racecar.applyAction(realaction)
     for i in range(self._actionRepeat):
-      p.stepSimulation()
+      self._p.stepSimulation()
       if self._renders:
         time.sleep(self._timeStep)
       self._observation = self.getExtendedObservation()
@@ -137,7 +143,7 @@ class RacecarZEDGymEnv(gym.Env):
     return self._envStepCounter>1000
     
   def _reward(self):
-    closestPoints = p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) 
+    closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) 
     
     numPt = len(closestPoints)
     reward=-1000

examples/pybullet/gym/pybullet_envs/data/differential/diff_ring.urdf

@@ -0,0 +1,198 @@
+
+<?xml version="1.0" ?>
+<robot name="husky_robot" xmlns:xacro="http://ros.org/wiki/xacro">
+  <material name="White">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  <link name="world"/>
+	<link name="diff_ring">
+  	<contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0"/>
+      <stiffness value="30000"/>
+      <damping value="1000"/>
+    </contact>
+    
+    <inertial>
+      <mass value="2.637"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
+    </inertial>
+    <visual>
+      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh scale="0.001 0.001 0.001" filename="differential/diff_ring.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+     <visual>
+      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh scale="0.001 0.001 0.001" filename="differential/diff_carrier.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="1.570795 0 0" xyz="0 0.025 0"/>
+      <geometry>
+        <cylinder length="0.01" radius="0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="diff_ring_world" type="continuous">
+    <parent link="world"/>
+    <child link="diff_ring"/>
+    <origin rpy="0 0 0" xyz="0.256 0.2854 0.03282"/>
+    <axis rpy="0 0 0" xyz="0 1 0"/>
+  </joint>
+  <link name="diff_spiderA">
+  	<contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0"/>
+      <stiffness value="30000"/>
+      <damping value="1000"/>
+    </contact>
+    
+    <inertial>
+      <mass value="2.637"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
+    </inertial>
+    <visual>
+      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh scale="0.001 0.001 0.001" filename="differential/diff_spider.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+   
+    
+    
+    
+    <collision>
+      <origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
+      <geometry>
+        <cylinder length="0.01" radius="0.015"/>
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint name="diff_spiderA_ring" type="continuous">
+    <parent link="diff_ring"/>
+    <child link="diff_spiderA"/>
+    <origin rpy="0 0 1.570795" xyz="0.0 0.0 0.0"/>
+    <axis rpy="0 0 0" xyz="0 1 0"/>
+  </joint>
+ 
+ <link name="diff_spiderB">
+  	<contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0"/>
+      <stiffness value="30000"/>
+      <damping value="1000"/>
+    </contact>
+    
+    <inertial>
+      <mass value="2.637"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
+    </inertial>
+    <visual>
+      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh scale="0.001 0.001 0.001" filename="differential/diff_spider.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+   
+    
+    
+    
+    <collision>
+      <origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
+      <geometry>
+        <cylinder length="0.01" radius="0.015"/>
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint name="diff_spiderB_ring" type="continuous">
+    <parent link="diff_ring"/>
+    <child link="diff_spiderB"/>
+    <origin rpy="0 0 -1.570795" xyz="0.0 0.0 0.0"/>
+    <axis rpy="0 0 0" xyz="0 1 0"/>
+  </joint>
+  
+  
+  <link name="diff_sideA">
+  	<contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0"/>
+      <stiffness value="30000"/>
+      <damping value="1000"/>
+    </contact>
+    
+    <inertial>
+      <mass value="2.637"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
+    </inertial>
+    <visual>
+      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh scale="0.001 0.001 0.001" filename="differential/diff_side.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    
+    <collision>
+      <origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
+      <geometry>
+        <cylinder length="0.01" radius="0.015"/>
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint name="diff_sideA_ring" type="continuous">
+    <parent link="diff_ring"/>
+    <child link="diff_sideA"/>
+    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+    <axis rpy="0 0 0" xyz="0 1 0"/>
+  </joint>
+  
+   <link name="diff_sideB">
+  	<contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0"/>
+      <stiffness value="30000"/>
+      <damping value="1000"/>
+    </contact>
+    
+    <inertial>
+      <mass value="2.637"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
+    </inertial>
+    <visual>
+      <origin rpy="-1.570795 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh scale="0.001 0.001 0.001" filename="differential/diff_side.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    
+    <collision>
+      <origin rpy="-1.570795 0 0" xyz="0 -0.01 0"/>
+      <geometry>
+        <cylinder length="0.01" radius="0.015"/>
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint name="diff_sideB_ring" type="continuous">
+    <parent link="diff_ring"/>
+    <child link="diff_sideB"/>
+    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+    <axis rpy="0 0 0" xyz="0 1 0"/>
+  </joint>
+</robot>

examples/pybullet/gym/pybullet_envs/data/differential/modelorigin.txt

@@ -0,0 +1,2 @@
+stl files were copied from http://www.otvinta.com/download09.html
+URDF file was manually created, along with mimicJointConstraint.py

examples/pybullet/gym/pybullet_envs/data/racecar/racecar_differential.urdf

@@ -35,7 +35,7 @@
     <contact>
       <lateral_friction value=".5"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     <inertial>
@@ -68,7 +68,7 @@
 <contact>
       <lateral_friction value=".5"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     <inertial>
@@ -165,7 +165,7 @@
 <contact>
       <lateral_friction value=".8"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     <inertial>
@@ -209,7 +209,7 @@
 <contact>
       <lateral_friction value="0.8"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     <inertial>
@@ -337,7 +337,7 @@
   	<contact>
       <lateral_friction value="1.0"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     
@@ -377,7 +377,7 @@
   	<contact>
       <lateral_friction value="1.0"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     
@@ -416,7 +416,7 @@
   	<contact>
       <lateral_friction value="1.0"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     
@@ -456,7 +456,7 @@
   	<contact>
       <lateral_friction value="1.0"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     
@@ -492,7 +492,7 @@
   	<contact>
       <lateral_friction value="1.0"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     
@@ -528,7 +528,7 @@
   	<contact>
       <lateral_friction value="1.0"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     
@@ -568,7 +568,7 @@
   	<contact>
       <lateral_friction value="1.0"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     
@@ -607,7 +607,7 @@
   	<contact>
       <lateral_friction value="1.0"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     
@@ -645,7 +645,7 @@
   	<contact>
       <lateral_friction value="1.0"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     
@@ -681,7 +681,7 @@
   	<contact>
       <lateral_friction value="1.0"/>
       <rolling_friction value="0.0"/>
-      <stiffness value="30000"/>
+      <stiffness value="300000"/>
       <damping value="1000"/>
     </contact>
     
@@ -736,8 +736,5 @@
   <material name="red">
     <color rgba="0.8 0.0 0.0 1.0"/>
   </material>
-  <material name="white">
-    <color rgba="1.0 1.0 1.0 1.0"/>
-  </material>
 </robot>
 

examples/pybullet/gym/pybullet_envs/env_bases.py

@@ -3,7 +3,7 @@ import numpy as np
 import pybullet as p
 
 
-class MujocoXmlBaseBulletEnv(gym.Env):
+class MJCFBaseBulletEnv(gym.Env):
 	"""
 	Base class for MuJoCo .xml environments in a Scene.
 	These environments create single-player scenes and behave like normal Gym environments, if

examples/pybullet/gym/pybullet_envs/examples/enjoy_pybullet_racecar.py

@@ -12,7 +12,7 @@ from baselines import deepq
 
 def main():
     
-    env = RacecarGymEnv(renders=True)
+    env = RacecarGymEnv(renders=False,isDiscrete=True)
     act = deepq.load("racecar_model.pkl")
     print(act)
     while True:

examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py

@@ -5,15 +5,18 @@ parentdir = os.path.dirname(os.path.dirname(currentdir))
 os.sys.path.insert(0,parentdir)
 
 from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv
-environment = RacecarGymEnv(renders=True)
+isDiscrete = True
+
+environment = RacecarGymEnv(renders=True, isDiscrete=isDiscrete)
 environment.reset()
 
 targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
-steeringSlider = environment._p.addUserDebugParameter("steering",-0.5,0.5,0)
+steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
 
 while (True):
   targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider)
   steeringAngle = environment._p.readUserDebugParameter(steeringSlider)
+  if (isDiscrete):
     discreteAction = 0
     if (targetVelocity<-0.33):
       discreteAction=0
@@ -27,8 +30,9 @@ while (True):
         discreteAction=discreteAction+2
       else:
         discreteAction=discreteAction+1
-    
     action=discreteAction
+  else:
+    action=[targetVelocity,steeringAngle]
   state, reward, done, info = environment.step(action)
   obs = environment.getExtendedObservation()
   print("obs")

examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py

@@ -9,7 +9,7 @@ print ("hello")
 environment = RacecarZEDGymEnv(renders=True)
 
 targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
-steeringSlider = environment._p.addUserDebugParameter("steering",-0.5,0.5,0)
+steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
 
 while (True):
   targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider)
@@ -27,8 +27,8 @@ while (True):
       discreteAction=discreteAction+2
     else:
       discreteAction=discreteAction+1
-    
   action=discreteAction
+    
   state, reward, done, info = environment.step(action)
   obs = environment.getExtendedObservation()
   print("obs")

examples/pybullet/gym/pybullet_envs/examples/train_pybullet_racecar.py

@@ -23,7 +23,7 @@ def callback(lcl, glb):
 
 def main():
   
-    env = RacecarGymEnv(renders=False)
+    env = RacecarGymEnv(renders=False,isDiscrete=True)
     model = deepq.models.mlp([64])
     act = deepq.learn(
         env,

examples/pybullet/gym/pybullet_envs/examples/train_pybullet_zed_racecar.py

@@ -22,7 +22,7 @@ def callback(lcl, glb):
 
 def main():
   
-    env = RacecarZEDGymEnv(renders=False)
+    env = RacecarZEDGymEnv(renders=False, isDiscrete=True)
     model = deepq.models.cnn_to_mlp(
         convs=[(32, 8, 4), (64, 4, 2), (64, 3, 1)],
         hiddens=[256],

examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py

@@ -1,12 +1,12 @@
 from .scene_stadium import SinglePlayerStadiumScene
-from .env_bases import MujocoXmlBaseBulletEnv
+from .env_bases import MJCFBaseBulletEnv
 import numpy as np
 from robot_locomotors import Hopper, Walker2D, HalfCheetah, Ant, Humanoid
 
 
-class WalkerBaseBulletEnv(MujocoXmlBaseBulletEnv):
+class WalkerBaseBulletEnv(MJCFBaseBulletEnv):
 	def __init__(self, robot):
-		MujocoXmlBaseBulletEnv.__init__(self, robot)
+		MJCFBaseBulletEnv.__init__(self, robot)
 		self.camera_x = 0
 		self.walk_target_x = 1e3  # kilometer away
 		self.walk_target_y = 0
@@ -16,7 +16,7 @@ class WalkerBaseBulletEnv(MujocoXmlBaseBulletEnv):
 		return self.stadium_scene
 
 	def _reset(self):
-		r = MujocoXmlBaseBulletEnv._reset(self)
+		r = MJCFBaseBulletEnv._reset(self)
 		self.parts, self.jdict, self.ordered_joints, self.robot_body = self.robot.addToScene(
 			self.stadium_scene.ground_plane_mjcf)
 		self.ground_ids = set([(self.parts[f].bodies[self.parts[f].bodyIndex], self.parts[f].bodyPartIndex) for f in

examples/pybullet/gym/pybullet_envs/gym_pendulum_envs.py

@@ -1,14 +1,14 @@
 from .scene_abstract import SingleRobotEmptyScene
-from .env_bases import MujocoXmlBaseBulletEnv
+from .env_bases import MJCFBaseBulletEnv
 from robot_pendula import InvertedPendulum, InvertedPendulumSwingup, InvertedDoublePendulum
 import gym, gym.spaces, gym.utils, gym.utils.seeding
 import numpy as np
 import os, sys
 
-class InvertedPendulumBulletEnv(MujocoXmlBaseBulletEnv):
+class InvertedPendulumBulletEnv(MJCFBaseBulletEnv):
 	def __init__(self):
 		self.robot = InvertedPendulum()
-		MujocoXmlBaseBulletEnv.__init__(self, self.robot)
+		MJCFBaseBulletEnv.__init__(self, self.robot)
 
 	def create_single_player_scene(self):
 		return SingleRobotEmptyScene(gravity=9.8, timestep=0.0165, frame_skip=1)
@@ -34,12 +34,12 @@ class InvertedPendulumBulletEnv(MujocoXmlBaseBulletEnv):
 class InvertedPendulumSwingupBulletEnv(InvertedPendulumBulletEnv):
 	def __init__(self):
 		self.robot = InvertedPendulumSwingup()
-		MujocoXmlBaseBulletEnv.__init__(self, self.robot)
+		MJCFBaseBulletEnv.__init__(self, self.robot)
 
-class InvertedDoublePendulumBulletEnv(MujocoXmlBaseBulletEnv):
+class InvertedDoublePendulumBulletEnv(MJCFBaseBulletEnv):
 	def __init__(self):
 		self.robot = InvertedDoublePendulum()
-		MujocoXmlBaseBulletEnv.__init__(self, self.robot)
+		MJCFBaseBulletEnv.__init__(self, self.robot)
 
 	def create_single_player_scene(self):
 		return SingleRobotEmptyScene(gravity=9.8, timestep=0.0165, frame_skip=1)

examples/pybullet/gym/pybullet_envs/robot_bases.py

@@ -4,7 +4,7 @@ import numpy as np
 import os
 
 
-class MujocoXmlBasedRobot:
+class MJCFBasedRobot:
 	"""
 	Base class for mujoco .xml based agents.
 	"""

examples/pybullet/gym/pybullet_envs/robot_locomotors.py

@@ -1,10 +1,10 @@
-from robot_bases import MujocoXmlBasedRobot
+from robot_bases import MJCFBasedRobot
 import numpy as np
 
 
-class WalkerBase(MujocoXmlBasedRobot):
+class WalkerBase(MJCFBasedRobot):
 	def __init__(self, fn, robot_name, action_dim, obs_dim, power):
-		MujocoXmlBasedRobot.__init__(self, fn, robot_name, action_dim, obs_dim)
+		MJCFBasedRobot.__init__(self, fn, robot_name, action_dim, obs_dim)
 		self.power = power
 		self.camera_x = 0
 		self.walk_target_x = 1e3  # kilometer away

examples/pybullet/gym/pybullet_envs/robot_pendula.py

@@ -1,11 +1,11 @@
-from robot_bases import MujocoXmlBasedRobot
+from robot_bases import MJCFBasedRobot
 import numpy as np
 
-class InvertedPendulum(MujocoXmlBasedRobot):
+class InvertedPendulum(MJCFBasedRobot):
 	swingup = False
 	force_gain = 12 # TODO: Try to find out why we need to scale the force
 	def __init__(self):
-		MujocoXmlBasedRobot.__init__(self, 'inverted_pendulum.xml', 'cart', action_dim=1, obs_dim=5)
+		MJCFBasedRobot.__init__(self, 'inverted_pendulum.xml', 'cart', action_dim=1, obs_dim=5)
 
 	def robot_specific_reset(self):
 		self.pole = self.parts["pole"]
@@ -53,9 +53,9 @@ class InvertedPendulumSwingup(InvertedPendulum):
 	force_gain = 2.2  # TODO: Try to find out why we need to scale the force
 
 
-class InvertedDoublePendulum(MujocoXmlBasedRobot):
+class InvertedDoublePendulum(MJCFBasedRobot):
 	def __init__(self):
-		MujocoXmlBasedRobot.__init__(self,  'inverted_double_pendulum.xml', 'cart', action_dim=1, obs_dim=9)
+		MJCFBasedRobot.__init__(self,  'inverted_double_pendulum.xml', 'cart', action_dim=1, obs_dim=9)
 
 	def robot_specific_reset(self):
 		self.pole2 = self.parts["pole2"]