From ee76873689296d6ca8e9f87000112b4c68129463 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 28 Oct 2018 21:08:57 -0700 Subject: [PATCH 1/6] eglPlugin: remove visual shape for removeBody. --- .../plugins/eglPlugin/eglRendererVisualShapeConverter.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp index 3ec0d1b27..0eca8b221 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp +++ b/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]; } } From 078887c4d7ba7811615575ea70bdd16d5b10477a Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 29 Oct 2018 10:25:40 -0700 Subject: [PATCH 2/6] PyBullet fileIOPlugin: don't add equal fileIO interface twice (based on identical fileIOType and pathPrefix) loadBullet goes through fileIOPlugin --- .../CommonInterfaces/CommonFileIOInterface.h | 9 ++ .../ImportURDFDemo/BulletUrdfImporter.cpp | 4 +- .../PhysicsServerCommandProcessor.cpp | 45 +++++--- examples/SharedMemory/SharedMemoryPublic.h | 1 + .../plugins/fileIOPlugin/fileIOPlugin.cpp | 106 +++++++++++------- .../plugins/fileIOPlugin/zipFileIO.h | 6 +- examples/Utils/b3BulletDefaultFileIO.h | 11 +- 7 files changed, 119 insertions(+), 63 deletions(-) diff --git a/examples/CommonInterfaces/CommonFileIOInterface.h b/examples/CommonInterfaces/CommonFileIOInterface.h index 3e2e78f6d..e41faf78a 100644 --- a/examples/CommonInterfaces/CommonFileIOInterface.h +++ b/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() { } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index e86779174..943b26ddd 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/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; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1f4242cb9..4085a6949 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -10198,30 +10198,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 buffer; + buffer.reserve(1024); + if (fileIO) { - sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName); - f = fopen(relativeFileName, "rb"); - if (f) + char fileName[1024]; + int fileId = -1; + found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024); + if (found) { - found = true; - break; + fileId = fileIO->fileOpen(fileName,"rb"); + } + 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(); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index b4f4aa6ca..166de101c 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -924,6 +924,7 @@ enum eFileIOTypes ePosixFileIO = 1, eZipFileIO, eCNSFileIO, + eInMemoryFileIO, }; //limits for vertices/indices in PyBullet::createCollisionShape diff --git a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp index d5d063488..9e53b9f77 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp +++ b/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=0 && fileIOIndex=0 && fileIOIndexm_ints[1]; - switch (fileIOType) + bool alreadyExists = false; + + for (int i=0;im_fileIO.getFileIOInterface(i); + if (fileIO) { -#ifdef B3_EXCLUDE_DEFAULT_FILEIO - 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) + if (fileIO->m_fileIOType == fileIOType) { - 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: diff --git a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h index 7366f1e24..c3efdb0ce 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h +++ b/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) - :m_zipfileName(zipfileName), + ZipFileIO(int fileIOType, const char* zipfileName, CommonFileIOInterface* wrapperFileIO) + :CommonFileIOInterface(fileIOType,0), + m_zipfileName(zipfileName), m_numFileHandles(0) { + m_rootPath = m_zipfileName.c_str(); for (int i=0;ifindFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen); } + char m_prefix[1024]; FILE* m_fileHandles[B3_FILEIO_MAX_FILES]; int m_numFileHandles; - b3BulletDefaultFileIO() - :m_numFileHandles(0) + b3BulletDefaultFileIO(int fileIOType=0, const char* pathPrefix=0) + :CommonFileIOInterface(fileIOType, m_prefix), + m_numFileHandles(0) { + sprintf(m_prefix,"%s", pathPrefix); for (int i=0;i0; } @@ -114,7 +117,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; From 35790c36c2ed83a1563918a2c73b6f24b1e8ce19 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 29 Oct 2018 10:56:55 -0700 Subject: [PATCH 3/6] fix renamed variable m_rootPath->m_pathPrefix --- examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h index c3efdb0ce..a0e8fe00f 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h +++ b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h @@ -15,7 +15,7 @@ struct ZipFileIO : public CommonFileIOInterface m_zipfileName(zipfileName), m_numFileHandles(0) { - m_rootPath = m_zipfileName.c_str(); + m_pathPrefix = m_zipfileName.c_str(); for (int i=0;i Date: Mon, 29 Oct 2018 12:08:34 -0700 Subject: [PATCH 4/6] PyBullet: Use fileIOPlugin in processRestoreStateCommand PyBullet: Report debug information in case of failure in restoreState. --- .../btMultiBodyWorldImporter.cpp | 63 ++++++++++++++++++- .../PhysicsServerCommandProcessor.cpp | 51 ++++++++++----- 2 files changed, 95 insertions(+), 19 deletions(-) diff --git a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp index 884da0072..f18a5d2c5 100644 --- a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp +++ b/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 diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4085a6949..d251ca53e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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 buffer; + buffer.reserve(1024); + if (fileIO) { - sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName); - f = fopen(relativeFileName, "rb"); - if (f) + + int fileId = -1; + found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024); + if (found) { - found = true; - break; + fileId = fileIO->fileOpen(fileName,"rb"); + } + 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; From 2fa0ae9349d9d76f9e748df20c953a9c770f7159 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 29 Oct 2018 13:45:42 -0700 Subject: [PATCH 5/6] fix sprintf using nullptr string --- examples/Utils/b3BulletDefaultFileIO.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/examples/Utils/b3BulletDefaultFileIO.h b/examples/Utils/b3BulletDefaultFileIO.h index aa1dc5ce6..b4cf20c4b 100644 --- a/examples/Utils/b3BulletDefaultFileIO.h +++ b/examples/Utils/b3BulletDefaultFileIO.h @@ -24,7 +24,11 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface :CommonFileIOInterface(fileIOType, m_prefix), m_numFileHandles(0) { - sprintf(m_prefix,"%s", pathPrefix); + m_prefix[0] = 0; + if (pathPrefix) + { + sprintf(m_prefix,"%s", pathPrefix); + } for (int i=0;i Date: Mon, 29 Oct 2018 16:44:12 -0700 Subject: [PATCH 6/6] update to CartPoleBulletEnv-v1 and check with latest baselines v0.1.5, works fine. (make it more similar to classical control cartpole) --- .../pybullet/gym/pybullet_data/cartpole.urdf | 8 ++- .../pybullet/gym/pybullet_envs/__init__.py | 8 +-- .../baselines/enjoy_pybullet_cartpole.py | 4 +- .../pybullet_envs/bullet/cartpole_bullet.py | 56 ++++++++++--------- 4 files changed, 44 insertions(+), 32 deletions(-) diff --git a/examples/pybullet/gym/pybullet_data/cartpole.urdf b/examples/pybullet/gym/pybullet_data/cartpole.urdf index c22ca920c..75530c483 100644 --- a/examples/pybullet/gym/pybullet_data/cartpole.urdf +++ b/examples/pybullet/gym/pybullet_data/cartpole.urdf @@ -59,9 +59,15 @@ - + + + + + + + diff --git a/examples/pybullet/gym/pybullet_envs/__init__.py b/examples/pybullet/gym/pybullet_envs/__init__.py index 25b82e2ca..d2a5a95b4 100644 --- a/examples/pybullet/gym/pybullet_envs/__init__.py +++ b/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, - reward_threshold=950.0, + max_episode_steps=200, + 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( diff --git a/examples/pybullet/gym/pybullet_envs/baselines/enjoy_pybullet_cartpole.py b/examples/pybullet/gym/pybullet_envs/baselines/enjoy_pybullet_cartpole.py index 7ede98c6d..0de7dc074 100644 --- a/examples/pybullet/gym/pybullet_envs/baselines/enjoy_pybullet_cartpole.py +++ b/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) diff --git a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py b/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py index 38145c9bf..850ab9278 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py +++ b/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([ - 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.force_mag = 10 - self.action_space = spaces.Discrete(9) - self.observation_space = spaces.Box(-observation_high, observation_high) + self.action_space = spaces.Discrete(2) + 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,)) - initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,)) - p.resetJointState(self.cartpole, 1, initialAngle) - p.resetJointState(self.cartpole, 0, initialCartPos) - + randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,)) + p.resetJointState(self.cartpole, 1, randstate[0], randstate[1]) + p.resetJointState(self.cartpole, 0, randstate[2], randstate[3]) + #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):