diff --git a/docs/pybullet_quickstartguide.pdf b/docs/pybullet_quickstartguide.pdf index d1e10cca7..a46bbf128 100644 Binary files a/docs/pybullet_quickstartguide.pdf and b/docs/pybullet_quickstartguide.pdf differ diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index be7832408..1e52a572d 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -120,7 +120,10 @@ struct BulletMJCFImporterInternalData btAlignedObjectArray m_models; int m_activeModel; - + + //those collision shapes are deleted by caller (todo: make sure this happens!) + btAlignedObjectArray m_allocatedCollisionShapes; + BulletMJCFImporterInternalData() :m_activeModel(-1) { @@ -794,7 +797,7 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger, m_data->m_pathPrefix[0] = 0; if (!fileFound){ - std::cerr << "URDF file not found" << std::endl; + std::cerr << "MJCF file not found" << std::endl; return false; } else { @@ -1017,6 +1020,8 @@ int BulletMJCFImporter::getBodyUniqueId() const class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const { btCompoundShape* compound = new btCompoundShape(); + m_data->m_allocatedCollisionShapes.push_back(compound); + const UrdfLink* link = m_data->getLink(m_data->m_activeModel,linkIndex); if (link) { @@ -1075,6 +1080,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn } if (childShape) { + m_data->m_allocatedCollisionShapes.push_back(childShape); compound->addChildShape(col->m_linkLocalFrame,childShape); } } @@ -1082,6 +1088,15 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn return compound; } +int BulletMJCFImporter::getNumAllocatedCollisionShapes() const +{ + return m_data->m_allocatedCollisionShapes.size(); +} +class btCollisionShape* BulletMJCFImporter::getAllocatedCollisionShape(int index) +{ + return m_data->m_allocatedCollisionShapes[index]; +} + int BulletMJCFImporter::getNumModels() const { diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h index 9cd59d90c..454f6df29 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h @@ -27,9 +27,6 @@ public: virtual bool loadMJCF(const char* fileName, MJCFErrorLogger* logger, bool forceFixedBase = false); - int getNumModels() const; - - void activateModel(int modelIndex); virtual bool loadURDF(const char* fileName, bool forceFixedBase = false) { @@ -71,6 +68,10 @@ public: virtual int getBodyUniqueId() const; virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const ; + virtual int getNumAllocatedCollisionShapes() const; + virtual class btCollisionShape* getAllocatedCollisionShape(int index); + virtual int getNumModels() const; + virtual void activateModel(int modelIndex); }; diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp index 66a1e6c0b..eb62a2826 100644 --- a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp +++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp @@ -77,7 +77,7 @@ struct ImportMJCFInternalData ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName) :CommonMultiBodyBase(helper), - m_grav(-10), + m_grav(0), m_upAxis(2) { m_data = new ImportMJCFInternalData; @@ -238,7 +238,7 @@ void ImportMJCFSetup::initPhysics() ConvertURDF2Bullet(importer,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,importer.getPathPrefix(),CUF_USE_MJCF); mb = creation.getBulletMultiBody(); - if (mb) + if (0)//mb) { printf("first MJCF file converted!\n"); std::string* name = new std::string(importer.getLinkName(importer.getRootLinkIndex())); diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index ce688ec90..a16d34f89 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -53,6 +53,12 @@ public: virtual int getBodyUniqueId() const { return 0;} virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0; + + virtual int getNumAllocatedCollisionShapes() const = 0; + virtual class btCollisionShape* getAllocatedCollisionShape(int index) = 0; + virtual int getNumModels() const=0; + virtual void activateModel(int modelIndex)=0; + }; #endif //URDF_IMPORTER_INTERFACE_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 89c180374..e30c1519c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -146,11 +146,11 @@ b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClie int len = strlen(fileName); if (len < MAX_URDF_FILENAME_LENGTH) { - strcpy(command->m_fileArguments.m_fileName, fileName); + strcpy(command->m_mjcfArguments.m_mjcfFileName, fileName); } else { - command->m_fileArguments.m_fileName[0] = 0; + command->m_mjcfArguments.m_mjcfFileName[0] = 0; } command->m_updateFlags = 0; @@ -849,6 +849,7 @@ int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyInd { switch (status->m_type) { + case CMD_MJCF_LOADING_COMPLETED: case CMD_BULLET_LOADING_COMPLETED: case CMD_SDF_LOADING_COMPLETED: { diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 61d2390bd..de6b08f6e 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -318,6 +318,14 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { } break; } + + case CMD_MJCF_LOADING_COMPLETED: + { + if (m_data->m_verboseOutput) { + b3Printf("Server loading the MJCF OK\n"); + } + break; + } case CMD_SDF_LOADING_COMPLETED: { if (m_data->m_verboseOutput) { @@ -402,6 +410,13 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { break; } + case CMD_MJCF_LOADING_FAILED: + { + if (m_data->m_verboseOutput) { + b3Printf("Server failed loading the MJCF...\n"); + } + break; + } case CMD_SDF_LOADING_FAILED: { if (m_data->m_verboseOutput) { b3Printf("Server failed loading the SDF...\n"); @@ -778,11 +793,6 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Save .bullet failed"); break; } - case CMD_MJCF_LOADING_FAILED: - { - b3Warning("Load .mjcf failed"); - break; - } case CMD_USER_DEBUG_DRAW_COMPLETED: { break; @@ -819,7 +829,8 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { m_data->m_waitingForServer = true; } - if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) + + if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED)) { int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; if (numBodies>0) diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index b12438cc2..2dd96b161 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -653,6 +653,8 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd break; } + + case CMD_MJCF_LOADING_COMPLETED: case CMD_SDF_LOADING_COMPLETED: { //we'll stream further info from the physics server diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 8448d98f4..911a501f3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -18,7 +18,7 @@ #include "btBulletDynamicsCommon.h" #include "LinearMath/btTransform.h" - +#include "../Importers/ImportMJCFDemo/BulletMJCFImporter.h" #include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h" #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "LinearMath/btSerializer.h" @@ -909,8 +909,162 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) } } +bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, URDFImporterInterface& u2b) +{ + bool loadOk = true; + -bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody) + btTransform rootTrans; + rootTrans.setIdentity(); + if (m_data->m_verboseOutput) + { + b3Printf("loaded %s OK!", fileName); + } + SaveWorldObjectData sd; + sd.m_fileName = fileName; + + + for (int m =0; mallocHandle(); + + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + + sd.m_bodyUniqueIds.push_back(bodyUniqueId); + + u2b.setBodyUniqueId(bodyUniqueId); + { + btScalar mass = 0; + bodyHandle->m_rootLocalInertialFrame.setIdentity(); + btVector3 localInertiaDiagonal(0,0,0); + int urdfLinkIndex = u2b.getRootLinkIndex(); + u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame); + } + + + + //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user + int rootLinkIndex = u2b.getRootLinkIndex(); + b3Printf("urdf root link index = %d\n",rootLinkIndex); + MyMultiBodyCreator creation(m_data->m_guiHelper); + + u2b.getRootTransformInWorld(rootTrans); + ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); + + + + mb = creation.getBulletMultiBody(); + rb = creation.getRigidBody(); + if (rb) + rb->setUserIndex2(bodyUniqueId); + + if (mb) + mb->setUserIndex2(bodyUniqueId); + + + if (mb) + { + bodyHandle->m_multiBody = mb; + + + m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId); + + createJointMotors(mb); + + + //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); + + bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); + for (int i=0;igetNumLinks();i++) + { + //disable serialization of the collision objects + + int urdfLinkIndex = creation.m_mb2urdfLink[i]; + btScalar mass; + btVector3 localInertiaDiagonal(0,0,0); + btTransform localInertialFrame; + u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame); + bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame); + + std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str()); + m_data->m_strings.push_back(linkName); + + mb->getLink(i).m_linkName = linkName->c_str(); + + std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str()); + m_data->m_strings.push_back(jointName); + + mb->getLink(i).m_jointName = jointName->c_str(); + } + std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); + m_data->m_strings.push_back(baseName); + mb->setBaseName(baseName->c_str()); + } else + { + b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); + bodyHandle->m_rigidBody = rb; + } + + } + + for (int i=0;im_collisionShapes.push_back(shape); + } + + m_data->m_saveWorldBodyData.push_back(sd); + + return loadOk; +} + +struct MyMJCFLogger2 : public MJCFErrorLogger +{ + virtual void reportError(const char* error) + { + b3Error(error); + } + virtual void reportWarning(const char* warning) + { + b3Warning(warning); + } + virtual void printMessage(const char* msg) + { + b3Printf(msg); + } +}; + +bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags) +{ + btAssert(m_data->m_dynamicsWorld); + if (!m_data->m_dynamicsWorld) + { + b3Error("loadSdf: No valid m_dynamicsWorld"); + return false; + } + + m_data->m_sdfRecentLoadedBodies.clear(); + + BulletMJCFImporter u2b(m_data->m_guiHelper); //, &m_data->m_visualConverter + + bool useFixedBase = false; + MyMJCFLogger2 logger; + bool loadOk = u2b.loadMJCF(fileName, &logger, useFixedBase); + if (loadOk) + { + + processImportedObjects(fileName,bufferServerToClient,bufferSizeInBytes,useMultiBody,flags, u2b); + } + return loadOk; +} + +bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags) { btAssert(m_data->m_dynamicsWorld); if (!m_data->m_dynamicsWorld) @@ -923,118 +1077,13 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); - bool useFixedBase = false; - bool loadOk = u2b.loadSDF(fileName, useFixedBase); - if (loadOk) - { - for (int i=0;im_collisionShapes.push_back(shape); - } - - btTransform rootTrans; - rootTrans.setIdentity(); - if (m_data->m_verboseOutput) - { - b3Printf("loaded %s OK!", fileName); - } - SaveWorldObjectData sd; - sd.m_fileName = fileName; - - - for (int m =0; mallocHandle(); - - InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); - - sd.m_bodyUniqueIds.push_back(bodyUniqueId); - - u2b.setBodyUniqueId(bodyUniqueId); - { - btScalar mass = 0; - bodyHandle->m_rootLocalInertialFrame.setIdentity(); - btVector3 localInertiaDiagonal(0,0,0); - int urdfLinkIndex = u2b.getRootLinkIndex(); - u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame); - } - - - - //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user - int rootLinkIndex = u2b.getRootLinkIndex(); - b3Printf("urdf root link index = %d\n",rootLinkIndex); - MyMultiBodyCreator creation(m_data->m_guiHelper); - - u2b.getRootTransformInWorld(rootTrans); - ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),CUF_USE_SDF); - - - - mb = creation.getBulletMultiBody(); - rb = creation.getRigidBody(); - if (rb) - rb->setUserIndex2(bodyUniqueId); - - if (mb) - mb->setUserIndex2(bodyUniqueId); - - - if (mb) - { - bodyHandle->m_multiBody = mb; - - - m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId); - - createJointMotors(mb); - - - //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); - - bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); - for (int i=0;igetNumLinks();i++) - { - //disable serialization of the collision objects - - int urdfLinkIndex = creation.m_mb2urdfLink[i]; - btScalar mass; - btVector3 localInertiaDiagonal(0,0,0); - btTransform localInertialFrame; - u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame); - bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame); - - std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str()); - m_data->m_strings.push_back(linkName); - - mb->getLink(i).m_linkName = linkName->c_str(); - - std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str()); - m_data->m_strings.push_back(jointName); - - mb->getLink(i).m_jointName = jointName->c_str(); - } - std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); - m_data->m_strings.push_back(baseName); - mb->setBaseName(baseName->c_str()); - } else - { - b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); - bodyHandle->m_rigidBody = rb; - } - - } - - m_data->m_saveWorldBodyData.push_back(sd); - - } + bool forceFixedBase = false; + bool loadOk =u2b.loadSDF(fileName,forceFixedBase); + + if (loadOk) + { + processImportedObjects(fileName,bufferServerToClient,bufferSizeInBytes,useMultiBody,flags, u2b); + } return loadOk; } @@ -1767,9 +1816,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? sdfArgs.m_useMultiBody : true; - bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody); + int flags = CUF_USE_SDF; //CUF_USE_URDF_INERTIA + bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags); if (completedOk) { + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); @@ -3812,8 +3864,33 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_MJCF_LOADING_FAILED; - hasStatus = true; - break; + const MjcfArgs& mjcfArgs = clientCmd.m_mjcfArguments; + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName); + } + bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? mjcfArgs.m_useMultiBody : true; + int flags = CUF_USE_MJCF;//CUF_USE_URDF_INERTIA + bool completedOk = loadMjcf(mjcfArgs.m_mjcfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags); + if (completedOk) + { + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + + serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); + int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); + for (int i=0;im_sdfRecentLoadedBodies[i]; + } + + serverStatusOut.m_type = CMD_MJCF_LOADING_COMPLETED; + } else + { + serverStatusOut.m_type = CMD_MJCF_LOADING_FAILED; + } + hasStatus = true; + break; + } case CMD_USER_DEBUG_DRAW: @@ -4376,7 +4453,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() #if 1 // Load one motor gripper for kuka - loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); + loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true,CUF_USE_SDF); m_data->m_gripperId = bodyId + 1; { InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId); @@ -4503,7 +4580,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() //loadUrdf("Apple/apple.urdf", objectWorldTr[8].getOrigin(), objectWorldTr[8].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); // Shelf area - loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); + loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true, CUF_USE_SDF); loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index c4fd8e7a3..66dc230f9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -30,11 +30,15 @@ protected: - bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody); + bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags); bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn, bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes); + bool loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags); + + bool processImportedObjects(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, class URDFImporterInterface& u2b); + bool supportsJointMotor(class btMultiBody* body, int linkIndex); int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index acab7d351..4efee8023 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -91,6 +91,11 @@ struct UrdfArgs int m_useFixedBase; }; +struct MjcfArgs +{ + char m_mjcfFileName[MAX_URDF_FILENAME_LENGTH]; + int m_useMultiBody; +}; struct BulletDataStreamArgs { @@ -604,6 +609,7 @@ struct SharedMemoryCommand { struct UrdfArgs m_urdfArguments; struct SdfArgs m_sdfArguments; + struct MjcfArgs m_mjcfArguments; struct FileArgs m_fileArguments; struct SdfRequestInfoArgs m_sdfRequestInfoArgs; struct InitPoseArgs m_initPoseArgs; diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index a17afb1f8..e851dbc99 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -73,6 +73,8 @@ myfiles = "../Importers/ImportURDFDemo/UrdfParser.h", "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/URDF2Bullet.h", + "../Importers/ImportMJCFDemo/BulletMJCFImporter.cpp", + "../Importers/ImportMJCFDemo/BulletMJCFImporter.h", "../Utils/b3ResourcePath.cpp", "../Utils/b3Clock.cpp", "../../Extras/Serialize/BulletWorldImporter/*", diff --git a/examples/SharedMemory/udp/premake4.lua b/examples/SharedMemory/udp/premake4.lua index 531b098bf..8517cf51c 100644 --- a/examples/SharedMemory/udp/premake4.lua +++ b/examples/SharedMemory/udp/premake4.lua @@ -90,6 +90,8 @@ myfiles = "../../Importers/ImportURDFDemo/MultiBodyCreationInterface.h", "../../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../../Importers/ImportURDFDemo/MyMultiBodyCreator.h", + "../../Importers/ImportMJCFDemo/BulletMJCFImporter.cpp", + "../../Importers/ImportMJCFDemo/BulletMJCFImporter.h", "../../Importers/ImportURDFDemo/BulletUrdfImporter.cpp", "../../Importers/ImportURDFDemo/BulletUrdfImporter.h", "../../Importers/ImportURDFDemo/UrdfParser.cpp", diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index 85de30a69..0a10c0677 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -64,6 +64,7 @@ SET(pybullet_SRCS ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp + ../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index 86ed39611..05196cda1 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -109,6 +109,7 @@ if not _OPTIONS["no-enet"] then "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", + "../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp", "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 35ce25d60..da4cf300b 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -495,7 +495,10 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key int statusType; b3SharedMemoryCommandHandle command; b3PhysicsClientHandle sm = 0; - + int numBodies = 0; + int i; + int bodyIndicesOut[MAX_SDF_BODIES]; + PyObject* pylist = 0; int physicsClientId = 0; static char *kwlist[] = { "mjcfFileName","physicsClientId", NULL }; if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &mjcfFileName,&physicsClientId)) @@ -518,8 +521,22 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key PyErr_SetString(SpamError, "Couldn't load .mjcf file."); return NULL; } - Py_INCREF(Py_None); - return Py_None; + + numBodies = + b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); + if (numBodies > MAX_SDF_BODIES) { + PyErr_SetString(SpamError, "SDF exceeds body capacity"); + return NULL; + } + + pylist = PyTuple_New(numBodies); + + if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) { + for (i = 0; i < numBodies; i++) { + PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); + } + } + return pylist; } static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject *keywds) diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt index 52297594c..84f99dfd6 100644 --- a/test/SharedMemory/CMakeLists.txt +++ b/test/SharedMemory/CMakeLists.txt @@ -74,6 +74,7 @@ ENDIF() ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp + ../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index ca48cfea0..644aaa813 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -93,6 +93,7 @@ project ("Test_PhysicsServerLoopBack") "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", + "../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp", "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", @@ -166,6 +167,7 @@ project ("Test_PhysicsServerLoopBack") "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", + "../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp", "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", @@ -266,6 +268,7 @@ project ("Test_PhysicsServerInProcessExampleBrowser") "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", + "../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp", "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",