Merge pull request #1962 from erwincoumans/master

eglPlugin: remove visual shape for removeBody.

Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp

@@ -274,6 +274,7 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
 		//equal number of objects, # links etc
 		if ((bulletFile2->m_multiBodies.size() != m_data->m_mbDynamicsWorld->getNumMultibodies()))
 		{
+			printf("btMultiBodyWorldImporter::convertAllObjects error: expected %d multibodies, got %d.\n", m_data->m_mbDynamicsWorld->getNumMultibodies(), bulletFile2->m_multiBodies.size());
 			result = false;
 			return result;
 		}
@@ -287,7 +288,15 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
 			{
 				btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
 				btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
-				syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
+				if (mbd->m_numLinks != mb->getNumLinks())
+				{
+					printf("btMultiBodyWorldImporter::convertAllObjects error: mismatch in number of links in a body (expected %d, found %d).\n", mbd->m_numLinks, mb->getNumLinks() );
+					result = false;
+					return result;
+				} else
+				{
+					syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
+				}
 			}
 
 			for (int i = bulletFile2->m_rigidBodies.size() - 1; i >= 0; i--)
@@ -319,11 +328,13 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
 					}
 					else
 					{
+						printf("btMultiBodyWorldImporter::convertAllObjects error: cannot find btRigidBody with bodyUniqueId %d\n", uid);
 						result = false;
 					}
 				}
 				else
 				{
+					printf("Error in btMultiBodyWorldImporter::convertAllObjects: didn't find bodyUniqueId: %d\n", uid);
 					result = false;
 				}
 			}
@@ -362,7 +373,55 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
 			{
 				btMultiBodyFloatData* mbd = (btMultiBodyFloatData*)bulletFile2->m_multiBodies[i];
 				btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
-				syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
+				if (mbd->m_numLinks != mb->getNumLinks())
+				{
+					printf("btMultiBodyWorldImporter::convertAllObjects error: mismatch in number of links in a body (expected %d, found %d).\n", mbd->m_numLinks, mb->getNumLinks() );
+					result = false;
+					return result;
+				} else
+				{
+					syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
+				}
+			}
+
+			for (int i = bulletFile2->m_rigidBodies.size() - 1; i >= 0; i--)
+			{
+				btRigidBodyFloatData* rbd = (btRigidBodyFloatData*)bulletFile2->m_rigidBodies[i];
+				int foundRb = -1;
+				int uid = rbd->m_collisionObjectData.m_uniqueId;
+				for (int i = 0; i < m_data->m_mbDynamicsWorld->getNumCollisionObjects(); i++)
+				{
+					if (uid == m_data->m_mbDynamicsWorld->getCollisionObjectArray()[i]->getBroadphaseHandle()->m_uniqueId)
+					{
+						foundRb = i;
+						break;
+					}
+				}
+				if (foundRb >= 0)
+				{
+					btRigidBody* rb = btRigidBody::upcast(m_data->m_mbDynamicsWorld->getCollisionObjectArray()[foundRb]);
+					if (rb)
+					{
+						btTransform tr;
+						tr.deSerializeFloat(rbd->m_collisionObjectData.m_worldTransform);
+						rb->setWorldTransform(tr);
+						btVector3 linVel, angVel;
+						linVel.deSerializeFloat(rbd->m_linearVelocity);
+						angVel.deSerializeFloat(rbd->m_angularVelocity);
+						rb->setLinearVelocity(linVel);
+						rb->setAngularVelocity(angVel);
+					}
+					else
+					{
+						printf("btMultiBodyWorldImporter::convertAllObjects error: cannot find btRigidBody with bodyUniqueId %d\n", uid);
+						result = false;
+					}
+				}
+				else
+				{
+					printf("Error in btMultiBodyWorldImporter::convertAllObjects: didn't find bodyUniqueId: %d\n", uid);
+					result = false;
+				}
 			}
 
 			//todo: check why body1 pointer is not properly deserialized

examples/CommonInterfaces/CommonFileIOInterface.h

@@ -3,6 +3,15 @@
 
 struct CommonFileIOInterface
 {
+	int m_fileIOType;
+	const char* m_pathPrefix;
+
+	CommonFileIOInterface(int fileIOType, const char* pathPrefix)
+		:m_fileIOType(fileIOType),
+		m_pathPrefix(pathPrefix)
+	{
+	}
+
 	virtual ~CommonFileIOInterface()
 	{
 	}

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -143,7 +143,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
 	b3FileUtils fu;
 
 	//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
-	bool fileFound = m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024) > 0;
+	bool fileFound = m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024);
 
 	std::string xml_string;
 
@@ -217,7 +217,7 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
 	b3FileUtils fu;
 
 	//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
-	bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024)) > 0;
+	bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024));
 
 	std::string xml_string;
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -10152,33 +10152,50 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
 	}
 	else
 	{
-		const char* prefix[] = {"", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
-		int numPrefixes = sizeof(prefix) / sizeof(const char*);
-		char relativeFileName[1024];
-		FILE* f = 0;
 		bool found = false;
+		char fileName[1024];
+		fileName[0] = 0;
 
-		for (int i = 0; !f && i < numPrefixes; i++)
+		CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
+		b3AlignedObjectArray<char> buffer;
+		buffer.reserve(1024);
+		if (fileIO)
 		{
-			sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
+			
-			f = fopen(relativeFileName, "rb");
+			int fileId = -1;
-			if (f)
+			found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName,  fileName, 1024);
+			if (found)
 			{
-				found = true;
+				fileId = fileIO->fileOpen(fileName,"rb");
-				break;
+			}
+			if (fileId>=0)
+			{
+				int size = fileIO->getFileSize(fileId);
+				if (size>0)
+				{
+					buffer.resize(size);
+					int actual = fileIO->fileRead(fileId,&buffer[0],size);
+					if (actual != size)
+					{
+						b3Warning("image filesize mismatch!\n");
+						buffer.resize(0);
+					} else
+					{
+						found=true;
+					}
+				}
+				fileIO->fileClose(fileId);
 			}
 		}
-		if (f)
-		{
-			fclose(f);
-		}
 
-		if (found)
+		if (found && buffer.size())
 		{
-			ok = importer->loadFile(relativeFileName);
+			ok = importer->loadFileFromMemory(&buffer[0], buffer.size());
+		} else
+		{
+			b3Error("Error in restoreState: cannot load file %s\n", clientCmd.m_fileArguments.m_fileName);
 		}
 	}
-
 	if (ok)
 	{
 		serverCmd.m_type = CMD_RESTORE_STATE_COMPLETED;
@@ -10198,30 +10215,43 @@ bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct Shared
 	//btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld);
 	btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
 
-	const char* prefix[] = {"", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
-	int numPrefixes = sizeof(prefix) / sizeof(const char*);
-	char relativeFileName[1024];
-	FILE* f = 0;
 	bool found = false;
 
-	for (int i = 0; !f && i < numPrefixes; i++)
+	CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
+	b3AlignedObjectArray<char> buffer;
+	buffer.reserve(1024);
+	if (fileIO)
 	{
-		sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
+		char fileName[1024];
-		f = fopen(relativeFileName, "rb");
+		int fileId = -1;
-		if (f)
+		found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName,  fileName, 1024);
+		if (found)
 		{
-			found = true;
+			fileId = fileIO->fileOpen(fileName,"rb");
-			break;
+		}
+		if (fileId>=0)
+		{
+			int size = fileIO->getFileSize(fileId);
+			if (size>0)
+			{
+				buffer.resize(size);
+				int actual = fileIO->fileRead(fileId,&buffer[0],size);
+				if (actual != size)
+				{
+					b3Warning("image filesize mismatch!\n");
+					buffer.resize(0);
+				} else
+				{
+					found=true;
+				}
+			}
+			fileIO->fileClose(fileId);
 		}
 	}
-	if (f)
-	{
-		fclose(f);
-	}
 
-	if (found)
+	if (found && buffer.size())
 	{
-		bool ok = importer->loadFile(relativeFileName);
+		bool ok = importer->loadFileFromMemory(&buffer[0], buffer.size());
 		if (ok)
 		{
 			int numRb = importer->getNumRigidBodies();

examples/SharedMemory/SharedMemoryPublic.h

@@ -924,6 +924,7 @@ enum eFileIOTypes
 	ePosixFileIO = 1,
 	eZipFileIO,
 	eCNSFileIO,
+	eInMemoryFileIO,
 };
 
 //limits for vertices/indices in PyBullet::createCollisionShape

examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp

@@ -1257,6 +1257,7 @@ void EGLRendererVisualShapeConverter::removeVisualShape(int collisionObjectUniqu
 		{
 			for (int o = 0; o < ptr->m_renderObjects.size(); o++)
 			{
+				m_data->m_instancingRenderer->removeGraphicsInstance(ptr->m_graphicsInstanceId);
 				delete ptr->m_renderObjects[o];
 			}
 		}

examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp

@@ -48,6 +48,7 @@ struct InMemoryFileIO : public CommonFileIOInterface
 	int m_numFrees;
 
 	InMemoryFileIO()
+		:CommonFileIOInterface(eInMemoryFileIO,0)
 	{
 		m_numAllocs=0;
 		m_numFrees=0;
@@ -293,7 +294,8 @@ struct WrapperFileIO : public CommonFileIOInterface
 	InMemoryFileIO m_cachedFiles;
 
 	WrapperFileIO()
-		:m_numWrapperInterfaces(0)
+		:CommonFileIOInterface(0,0),
+		m_numWrapperInterfaces(0)
 	{
 		for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
 		{
@@ -327,6 +329,14 @@ struct WrapperFileIO : public CommonFileIOInterface
 		return result;
 	}
 
+	CommonFileIOInterface* getFileIOInterface(int fileIOIndex)
+	{
+		if (fileIOIndex>=0 && fileIOIndex<B3_MAX_FILEIO_INTERFACES)
+		{
+			return m_availableFileIOInterfaces[fileIOIndex];
+		}
+		return 0;
+	}
 	void removeFileIOInterface(int fileIOIndex)
 	{
 		if (fileIOIndex>=0 && fileIOIndex<B3_MAX_FILEIO_INTERFACES)
@@ -571,51 +581,68 @@ B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* cont
 		{
 			case eAddFileIOAction:
 			{
-				//create new fileIO interface
+				//if the fileIO already exists, skip this action
 				int fileIOType = arguments->m_ints[1];
-				switch (fileIOType)
+				bool alreadyExists = false;
+
+				for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
 				{
-					case ePosixFileIO:
+					CommonFileIOInterface* fileIO = obj->m_fileIO.getFileIOInterface(i);
+					if (fileIO)
 					{
-#ifdef B3_EXCLUDE_DEFAULT_FILEIO
+						if (fileIO->m_fileIOType == fileIOType)
-						printf("ePosixFileIO is not enabled in this build.\n");
-#else
-						obj->m_fileIO.addFileIOInterface(new b3BulletDefaultFileIO());
-#endif
-						break;
-					}
-					case eZipFileIO:
-					{
-#ifdef B3_USE_ZIPFILE_FILEIO
-						if (arguments->m_text)
 						{
-							obj->m_fileIO.addFileIOInterface(new ZipFileIO(arguments->m_text, &obj->m_fileIO));
+							if (fileIO->m_pathPrefix && strcmp(fileIO->m_pathPrefix,arguments->m_text)==0)
+							{
+								result = i;
+								alreadyExists = true;
+								break;
+							}
 						}
-#else
-						printf("eZipFileIO is not enabled in this build.\n");
-#endif
-						break;
-					}
-					case eCNSFileIO:
-					{
-#ifdef B3_USE_CNS_FILEIO
-						if (strlen(arguments->m_text))
-						{
-							obj->m_fileIO.addFileIOInterface(new CNSFileIO(arguments->m_text));
-						}
-						else
-						{
-							obj->m_fileIO.addFileIOInterface(new CNSFileIO(""));
-						}
-#else//B3_USE_CNS_FILEIO
-						printf("CNSFileIO is not enabled in this build.\n");
-#endif //B3_USE_CNS_FILEIO
-						break;
-					}
-					default:
-					{
 					}
 				}
+				
+
+				//create new fileIO interface
+				if (!alreadyExists)
+				{
+					switch (fileIOType)
+					{
+						case ePosixFileIO:
+						{
+	#ifdef B3_EXCLUDE_DEFAULT_FILEIO
+							printf("ePosixFileIO is not enabled in this build.\n");
+	#else
+							result = obj->m_fileIO.addFileIOInterface(new b3BulletDefaultFileIO(ePosixFileIO, arguments->m_text));
+	#endif
+							break;
+						}
+						case eZipFileIO:
+						{
+	#ifdef B3_USE_ZIPFILE_FILEIO
+							if (arguments->m_text)
+							{
+								result = obj->m_fileIO.addFileIOInterface(new ZipFileIO(eZipFileIO, arguments->m_text, &obj->m_fileIO));
+							}
+	#else
+							printf("eZipFileIO is not enabled in this build.\n");
+	#endif
+							break;
+						}
+						case eCNSFileIO:
+						{
+	#ifdef B3_USE_CNS_FILEIO
+							result = obj->m_fileIO.addFileIOInterface(new CNSFileIO(eCNSFileIO, arguments->m_text));
+	#else//B3_USE_CNS_FILEIO
+							printf("CNSFileIO is not enabled in this build.\n");
+	#endif //B3_USE_CNS_FILEIO
+							break;
+						}
+						default:
+						{
+						}
+					}//switch (fileIOType)
+				}//if (!alreadyExists)
 				break;
 			}
 			case eRemoveFileIOAction:
@@ -624,6 +651,7 @@ B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* cont
 				//remove fileIO interface
 				int fileIOIndex = arguments->m_ints[1];
 				obj->m_fileIO.removeFileIOInterface(fileIOIndex);
+				result = fileIOIndex;
 				break;
 			}
 			default:

examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h

@@ -10,10 +10,12 @@ struct ZipFileIO : public CommonFileIOInterface
 	unzFile	m_fileHandles[B3_ZIP_FILEIO_MAX_FILES ];
 	int m_numFileHandles;
 
-	ZipFileIO(const char* zipfileName, CommonFileIOInterface* wrapperFileIO)
+	ZipFileIO(int fileIOType, const char* zipfileName, CommonFileIOInterface* wrapperFileIO)
-		:m_zipfileName(zipfileName),
+		:CommonFileIOInterface(fileIOType,0),
+		m_zipfileName(zipfileName),
 		m_numFileHandles(0)
 	{
+		m_pathPrefix = m_zipfileName.c_str();
 		for (int i=0;i<B3_ZIP_FILEIO_MAX_FILES ;i++)
 		{
 			m_fileHandles[i]=0;

examples/Utils/b3BulletDefaultFileIO.h

@@ -16,12 +16,19 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface
 		return fileIo->findFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen);
 	}
 
+	char m_prefix[1024];
 	FILE* m_fileHandles[B3_FILEIO_MAX_FILES];
 	int m_numFileHandles;
 
-	b3BulletDefaultFileIO()
+	b3BulletDefaultFileIO(int fileIOType=0, const char* pathPrefix=0)
-		:m_numFileHandles(0)
+		:CommonFileIOInterface(fileIOType, m_prefix),
+		m_numFileHandles(0)
 	{
+		m_prefix[0] = 0;
+		if (pathPrefix)
+		{
+			sprintf(m_prefix,"%s", pathPrefix);
+		}
 		for (int i=0;i<B3_FILEIO_MAX_FILES;i++)
 		{
 			m_fileHandles[i]=0;
@@ -97,7 +104,7 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface
 
 	virtual bool findResourcePath(const char* fileName, char* relativeFileName, int relativeFileNameSizeInBytes)
 	{
-		return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, b3BulletDefaultFileIO::FileIOPluginFindFile, this);
+		return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, b3BulletDefaultFileIO::FileIOPluginFindFile, this)>0;
 	}
 
 
@@ -114,7 +121,7 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface
 		}
 
 		//printf("Trying various directories, relative to current working directory\n");
-		const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
+		const char* prefix[] = {m_prefix, "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
 		int numPrefixes = sizeof(prefix) / sizeof(const char*);
 
 		f = 0;

examples/pybullet/gym/pybullet_data/cartpole.urdf

@@ -59,9 +59,15 @@
     </visual>
     <inertial>
     <origin xyz="0 0 0.5"/>
-      <mass value="10"/>
+      <mass value="0.1"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
+<collision>
+      <geometry>
+        <box size="0.05 0.05 1.0"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0.5"/>
+    </collision>
   </link>
 
   <joint name="cart_to_pole" type="continuous">

examples/pybullet/gym/pybullet_envs/__init__.py

@@ -9,17 +9,17 @@ def register(id,*args,**kvargs):
 # ------------bullet-------------
 
 register(
-	id='CartPoleBulletEnv-v0',
+	id='CartPoleBulletEnv-v1',
 	entry_point='pybullet_envs.bullet:CartPoleBulletEnv',
-	timestep_limit=1000,
+        max_episode_steps=200,
-	reward_threshold=950.0,
+	reward_threshold=190.0,
 )
 
 register(
     id='MinitaurBulletEnv-v0',
     entry_point='pybullet_envs.bullet:MinitaurBulletEnv',
     timestep_limit=1000,
-    reward_threshold=5.0,
+    reward_threshold=15.0,
 )
 
 register(

examples/pybullet/gym/pybullet_envs/baselines/enjoy_pybullet_cartpole.py

@@ -5,12 +5,13 @@ parentdir = os.path.dirname(os.path.dirname(currentdir))
 os.sys.path.insert(0,parentdir)
 
 import gym
+import time
 
 from baselines import deepq
 from pybullet_envs.bullet.cartpole_bullet import CartPoleBulletEnv
 
 def main():
-    env = gym.make('CartPoleBulletEnv-v0')
+    env = gym.make('CartPoleBulletEnv-v1')
     act = deepq.load("cartpole_model.pkl")
 
     while True:
@@ -28,6 +29,7 @@ def main():
             a = aa[0]
             obs, rew, done, _ = env.step(a)
             episode_rew += rew
+            time.sleep(1./240.)
         print("Episode reward", episode_rew)
 
 

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

@@ -34,24 +34,26 @@ class CartPoleBulletEnv(gym.Env):
 	    p.connect(p.GUI)
     else:
     	p.connect(p.DIRECT)
+    self.theta_threshold_radians = 12 * 2 * math.pi / 360 
+    self.x_threshold = 0.4 #2.4   
+    high = np.array([
+         self.x_threshold * 2,
+         np.finfo(np.float32).max,
+         self.theta_threshold_radians * 2,
+         np.finfo(np.float32).max])
 
-    observation_high = np.array([
+    self.force_mag = 10
-          np.finfo(np.float32).max,
-          np.finfo(np.float32).max,
-          np.finfo(np.float32).max,
-          np.finfo(np.float32).max])
-    action_high = np.array([0.1])
 
-    self.action_space = spaces.Discrete(9)
+    self.action_space = spaces.Discrete(2)
-    self.observation_space = spaces.Box(-observation_high, observation_high)
+    self.observation_space = spaces.Box(-high, high, dtype=np.float32)
 
-    self.theta_threshold_radians = 1
-    self.x_threshold = 2.4
     self._seed()
-#    self.reset()
+    #    self.reset()
     self.viewer = None
     self._configure()
 
+
+
   def _configure(self, display=None):
     self.display = display
 
@@ -60,41 +62,43 @@ class CartPoleBulletEnv(gym.Env):
     return [seed]
 
   def _step(self, action):
+    force = self.force_mag if action==1 else -self.force_mag
+
+    p.setJointMotorControl2(self.cartpole, 0, p.TORQUE_CONTROL, force=force)
     p.stepSimulation()
-#    time.sleep(self.timeStep)
+
     self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
     theta, theta_dot, x, x_dot = self.state
 
-    dv = 0.1
-    deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]
-
-    p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3]))
-
     done =  x < -self.x_threshold \
                 or x > self.x_threshold \
                 or theta < -self.theta_threshold_radians \
                 or theta > self.theta_threshold_radians
+    done = bool(done)
     reward = 1.0
-
+    #print("state=",self.state)
     return np.array(self.state), reward, done, {}
 
   def _reset(self):
 #    print("-----------reset simulation---------------")
     p.resetSimulation()
     self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
-    self.timeStep = 0.01
+    p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
+    p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
+    p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
+    self.timeStep = 0.02
     p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
-    p.setGravity(0,0, -10)
+    p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
+    p.setGravity(0,0, -9.8)
     p.setTimeStep(self.timeStep)
     p.setRealTimeSimulation(0)
 
-    initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
+    randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
-    initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
+    p.resetJointState(self.cartpole, 1, randstate[0], randstate[1])
-    p.resetJointState(self.cartpole, 1, initialAngle)
+    p.resetJointState(self.cartpole, 0, randstate[2], randstate[3])
-    p.resetJointState(self.cartpole, 0, initialCartPos)
+    #print("randstate=",randstate)
-
     self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
-
+    #print("self.state=", self.state)
     return np.array(self.state)
 
   def _render(self, mode='human', close=False):