diff --git a/VERSION b/VERSION index 7cf322cf9..0104e57c4 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.85 +2.86 diff --git a/docs/pybullet_quickstartguide.pdf b/docs/pybullet_quickstartguide.pdf index 535caa8cf..9d0b61c0d 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 65d882296..4041feb01 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -8,6 +8,10 @@ #include "../ImportURDFDemo/UrdfParser.h" #include "../ImportURDFDemo/urdfStringSplit.h" #include "../ImportURDFDemo/urdfLexicalCast.h" +#include "../ImportObjDemo/LoadMeshFromObj.h" +#include "../ImportSTLDemo/LoadMeshFromSTL.h" +#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" +#include "../ImportMeshUtility/b3ImportMeshUtility.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" @@ -16,9 +20,22 @@ #include "BulletCollision/CollisionShapes/btCapsuleShape.h" #include "BulletCollision/CollisionShapes/btCylinderShape.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" +#include "BulletCollision/CollisionShapes/btConvexHullShape.h" +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btTriangleMesh.h" + + +#include + +enum eMJCF_FILE_TYPE_ENUMS +{ + MJCF_FILE_STL = 1, + MJCF_FILE_OBJ = 2 +}; + enum ePARENT_LINK_ENUMS { BASE_LINK_INDEX=-1, @@ -112,6 +129,10 @@ static bool parseVector6(btVector3& v0, btVector3& v1, const std::string& vector } +struct MyMJCFAsset +{ + std::string m_fileName; +}; struct BulletMJCFImporterInternalData { @@ -119,12 +140,20 @@ struct BulletMJCFImporterInternalData char m_pathPrefix[1024]; std::string m_fileModelName; + btHashMap m_assets; btAlignedObjectArray m_models; + + // + std::string m_meshDir; + std::string m_textureDir; + + int m_activeModel; //todo: for full MJCF compatibility, we would need a stack of default values int m_defaultCollisionGroup; int m_defaultCollisionMask; + btScalar m_defaultCollisionMargin; //those collision shapes are deleted by caller (todo: make sure this happens!) btAlignedObjectArray m_allocatedCollisionShapes; @@ -132,7 +161,8 @@ struct BulletMJCFImporterInternalData BulletMJCFImporterInternalData() :m_activeModel(-1), m_defaultCollisionGroup(1), - m_defaultCollisionMask(1) + m_defaultCollisionMask(1), + m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm { m_pathPrefix[0] = 0; } @@ -151,6 +181,48 @@ struct BulletMJCFImporterInternalData return 0; } + void parseCompiler(TiXmlElement* root_xml, MJCFErrorLogger* logger) + { + + const char* meshDirStr = root_xml->Attribute("meshdir"); + if (meshDirStr) + { + m_meshDir = meshDirStr; + } + const char* textureDirStr = root_xml->Attribute("texturedir"); + if (textureDirStr) + { + m_textureDir = textureDirStr; + } +#if 0 + for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement()) + { + std::string n = child_xml->Value(); + } +#endif + } + + void parseAssets(TiXmlElement* root_xml, MJCFErrorLogger* logger) + { + // + for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement()) + { + std::string n = child_xml->Value(); + if (n=="mesh") + { + const char* assetNameStr = child_xml->Attribute("name"); + const char* fileNameStr = child_xml->Attribute("file"); + if (assetNameStr && fileNameStr) + { + btHashString assetName = assetNameStr; + MyMJCFAsset asset; + asset.m_fileName = m_meshDir + fileNameStr; + m_assets.insert(assetName,asset); + } + } + + } + } bool parseDefaults(TiXmlElement* root_xml, MJCFErrorLogger* logger) { bool handled= false; @@ -158,9 +230,14 @@ struct BulletMJCFImporterInternalData for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement()) { std::string n = child_xml->Value(); + if (n=="inertial") { } + if (n=="asset") + { + parseAssets(child_xml,logger); + } if (n=="geom") { //contype, conaffinity @@ -534,6 +611,25 @@ struct BulletMJCFImporterInternalData } } } + if (geomType=="mesh") + { + const char* meshStr = link_xml->Attribute("mesh"); + if (meshStr) + { + MyMJCFAsset* assetPtr = m_assets[meshStr]; + if (assetPtr) + { + handledGeomType = true; + geom.m_type = URDF_GEOM_MESH; + geom.m_meshFileName = assetPtr->m_fileName; + geom.m_meshScale.setValue(1,1,1); + //todo: parse mesh scale + if (sz) + { + } + } + } + } #if 0 if (geomType == "cylinder") { @@ -1081,6 +1177,17 @@ bool BulletMJCFImporter::parseMJCFString(const char* xmlText, MJCFErrorLogger* l { m_data->parseDefaults(link_xml,logger); } + + for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("compiler"); link_xml; link_xml = link_xml->NextSiblingElement("compiler")) + { + m_data->parseCompiler(link_xml,logger); + } + + + for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("asset"); link_xml; link_xml = link_xml->NextSiblingElement("asset")) + { + m_data->parseAssets(link_xml,logger); + } for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("body"); link_xml; link_xml = link_xml->NextSiblingElement("body")) { @@ -1281,6 +1388,52 @@ int BulletMJCFImporter::getBodyUniqueId() const { return 0; } + + +static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale, btScalar collisionMargin) +{ + btCompoundShape* compound = new btCompoundShape(); + compound->setMargin(collisionMargin); + + btTransform identity; + identity.setIdentity(); + + for (int s = 0; s<(int)shapes.size(); s++) + { + btConvexHullShape* convexHull = new btConvexHullShape(); + convexHull->setMargin(collisionMargin); + tinyobj::shape_t& shape = shapes[s]; + int faceCount = shape.mesh.indices.size(); + + for (int f = 0; faddPoint(pt*geomScale,false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + convexHull->addPoint(pt*geomScale, false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + convexHull->addPoint(pt*geomScale, false); + } + + convexHull->recalcLocalAabb(); + convexHull->optimizeConvexHull(); + compound->addChildShape(identity,convexHull); + } + + return compound; +} + class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const { @@ -1319,6 +1472,129 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn } case URDF_GEOM_MESH: { + ////////////////////// + if (1) + { + if (col->m_geometry.m_meshFileName.length()) + { + const char* filename = col->m_geometry.m_meshFileName.c_str(); + //b3Printf("mesh->filename=%s\n",filename); + char fullPath[1024]; + int fileType = 0; + sprintf(fullPath,"%s%s",pathPrefix,filename); + b3FileUtils::toLower(fullPath); + char tmpPathPrefix[1024]; + int maxPathLen = 1024; + b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); + + char collisionPathPrefix[1024]; + sprintf(collisionPathPrefix,"%s%s",pathPrefix,tmpPathPrefix); + + if (strstr(fullPath,".stl")) + { + fileType = MJCF_FILE_STL; + } + if (strstr(fullPath,".obj")) + { + fileType = MJCF_FILE_OBJ; + } + + sprintf(fullPath,"%s%s",pathPrefix,filename); + FILE* f = fopen(fullPath,"rb"); + if (f) + { + fclose(f); + GLInstanceGraphicsShape* glmesh = 0; + + + switch (fileType) + { + case MJCF_FILE_OBJ: + { + if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH) + { + glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix); + } + else + { + std::vector shapes; + std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix); + //create a convex hull for each shape, and store it in a btCompoundShape + + childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin); + + } + break; + } + case MJCF_FILE_STL: + { + glmesh = LoadMeshFromSTL(fullPath); + break; + } + + default: + { + b3Warning("Unsupported file type in Collision: %s\n",fullPath); + + } + } + + + if (!childShape && glmesh && (glmesh->m_numvertices>0)) + { + //b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath); + //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size()); + //convex->setUserIndex(shapeId); + btAlignedObjectArray convertedVerts; + convertedVerts.reserve(glmesh->m_numvertices); + for (int i=0;im_numvertices;i++) + { + convertedVerts.push_back(btVector3( + glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0], + glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1], + glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2])); + } + + if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH) + { + + btTriangleMesh* meshInterface = new btTriangleMesh(); + for (int i=0;im_numIndices/3;i++) + { + float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw; + float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw; + float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw; + meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]), + btVector3(v1[0],v1[1],v1[2]), + btVector3(v2[0],v2[1],v2[2])); + } + + btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); + childShape = trimesh; + } else + { + btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); + convexHull->optimizeConvexHull(); + //convexHull->initializePolyhedralFeatures(); + convexHull->setMargin(m_data->m_defaultCollisionMargin); + childShape = convexHull; + } + } else + { + b3Warning("issue extracting mesh from STL file %s\n", fullPath); + } + + delete glmesh; + + } else + { + b3Warning("mesh geometry not found %s\n",fullPath); + } + + } + } + + ////////////////////// break; } @@ -1349,6 +1625,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn } default: { + } } if (childShape) diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp index 2008d52e9..75f6b83b3 100644 --- a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp +++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp @@ -119,6 +119,7 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option, if (gMCFJFileNameArray.size()==0) { + gMCFJFileNameArray.push_back("MPL/MPL.xml"); gMCFJFileNameArray.push_back("mjcf/humanoid.xml"); gMCFJFileNameArray.push_back("mjcf/inverted_pendulum.xml"); gMCFJFileNameArray.push_back("mjcf/ant.xml"); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 74c06f765..ab18dbe0a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -455,10 +455,13 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); - command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value; - command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP; - command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP; - + b3Assert(dofIndex>=0); + if (dofIndex>=0) + { + command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP; + } return 0; } @@ -466,10 +469,13 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); - command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value; - command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD; - command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD; - + b3Assert(dofIndex>=0); + if (dofIndex>=0) + { + command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD; + } return 0; } @@ -477,10 +483,13 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); - command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value; - command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT; - command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT; - + b3Assert(dofIndex>=0); + if (dofIndex>=0) + { + command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT; + } return 0; } @@ -489,10 +498,13 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); - command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; - command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; - command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE; - + b3Assert(dofIndex>=0); + if (dofIndex>=0) + { + command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + } return 0; } @@ -500,10 +512,13 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); - command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; - command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; - command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE; - + b3Assert(dofIndex>=0); + if (dofIndex>=0) + { + command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + } return 0; } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 23571a2f2..487f46e98 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1119,7 +1119,7 @@ public: if (args.CheckCmdLineFlag("norobotassets")) { -// gCreateDefaultRobotAssets = false; + gCreateDefaultRobotAssets = false; } diff --git a/examples/pybullet/vrEvent.py b/examples/pybullet/vrEvent.py index 970757eae..32eb1edd9 100644 --- a/examples/pybullet/vrEvent.py +++ b/examples/pybullet/vrEvent.py @@ -10,7 +10,11 @@ POSITION=1 BUTTONS=6 #assume that the VR physics server is already started before -p.connect(p.SHARED_MEMORY) +c = p.connect(p.SHARED_MEMORY) +print(c) +if (c<0): + p.connect(p.GUI) + p.setInternalSimFlags(0)#don't load default robot assets etc p.resetSimulation() p.loadURDF("plane.urdf") diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index df907e128..00754567b 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -29,7 +29,7 @@ subject to the following restrictions: #include /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ -#define BT_BULLET_VERSION 285 +#define BT_BULLET_VERSION 286 inline int btGetVersion() { diff --git a/src/LinearMath/btSerializer.h b/src/LinearMath/btSerializer.h index 6f03df158..9d95e1c8a 100644 --- a/src/LinearMath/btSerializer.h +++ b/src/LinearMath/btSerializer.h @@ -505,7 +505,7 @@ public: buffer[9] = '2'; buffer[10] = '8'; - buffer[11] = '5'; + buffer[11] = '6'; }