diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 35cf3c60e..ab6bf5c6b 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -201,7 +201,7 @@ void MyKeyboardCallback(int key, int state) //if (handled) // return; - if (s_window && s_window->isModifierKeyPressed(B3G_CONTROL)) + //if (s_window && s_window->isModifierKeyPressed(B3G_CONTROL)) { if (key=='a' && state) { diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index 2466c63d9..442a2f777 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -10,7 +10,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData) { - + B3_PROFILE("loadAndRegisterMeshFromFileInternal"); meshData.m_gfxShape = 0; meshData.m_textureImage = 0; meshData.m_textureHeight = 0; @@ -26,7 +26,10 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& btVector3 shift(0,0,0); std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix); + { + B3_PROFILE("tinyobj::LoadObj"); + std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix); + } GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes); diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp index d90bedfe2..158d3456b 100644 --- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp +++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp @@ -9,9 +9,16 @@ GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath) { + B3_PROFILE("LoadMeshFromObj"); std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, relativeFileName, materialPrefixPath); - - GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes); - return gfxShape; + { + B3_PROFILE("tinyobj::LoadObj2"); + std::string err = tinyobj::LoadObj(shapes, relativeFileName, materialPrefixPath); + } + + { + B3_PROFILE("btgCreateGraphicsShapeFromWavefrontObj"); + GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes); + return gfxShape; + } } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index f81bce7bf..6dd01345d 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -421,6 +421,7 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale) { + B3_PROFILE("createConvexHullFromShapes"); btCompoundShape* compound = new btCompoundShape(); compound->setMargin(gUrdfDefaultCollisionMargin); @@ -633,6 +634,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co case UrdfGeometry::FILE_OBJ: if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) { + glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), 0); } else @@ -744,16 +746,23 @@ upAxisMat.setIdentity(); { BT_PROFILE("convert trimesh"); btTriangleMesh* meshInterface = new btTriangleMesh(); - for (int i=0; im_numIndices/3; i++) { - const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i*3)]; - const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i*3+1)]; - const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i*3+2)]; - meshInterface->addTriangle(v0,v1,v2); + BT_PROFILE("convert vertices"); + + for (int i=0; im_numIndices/3; i++) + { + const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i*3)]; + const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i*3+1)]; + const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i*3+2)]; + meshInterface->addTriangle(v0,v1,v2); + } + } + { + BT_PROFILE("create btBvhTriangleMeshShape"); + btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); + //trimesh->setLocalScaling(collision->m_geometry.m_meshScale); + shape = trimesh; } - btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); - //trimesh->setLocalScaling(collision->m_geometry.m_meshScale); - shape = trimesh; } else { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f7dceb4f9..46bd40a31 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -696,7 +696,8 @@ struct VRControllerStateLogger : public InternalStateLogger structNames.push_back("buttons4"); structNames.push_back("buttons5"); structNames.push_back("buttons6"); - m_structTypes = "IfIIIffffffffIIIIIII"; + structNames.push_back("deviceType"); + m_structTypes = "IfIIIffffffffIIIIIIII"; const char* fileNameC = fileName.c_str(); m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes); @@ -770,7 +771,7 @@ struct VRControllerStateLogger : public InternalStateLogger { logData.m_values.push_back(packedButtons[b]); } - + logData.m_values.push_back(event.m_deviceType); appendMinitaurLogData(m_logFileHandle, m_structTypes, logData); event.m_numButtonEvents = 0; @@ -829,7 +830,7 @@ struct GenericRobotStateLogger : public InternalStateLogger structNames.push_back("omegaZ"); structNames.push_back("qNum"); - m_structTypes = "IfIfffffffffffffI"; + m_structTypes = "IfifffffffffffffI"; for (int i=0;im_guiHelper) { m_data->m_guiHelper->removeAllGraphicsInstances(); + m_data->m_guiHelper->removeAllUserDebugItems(); } if (m_data) { diff --git a/examples/Utils/RobotLoggingUtil.cpp b/examples/Utils/RobotLoggingUtil.cpp index 0499a5aa9..620b31849 100644 --- a/examples/Utils/RobotLoggingUtil.cpp +++ b/examples/Utils/RobotLoggingUtil.cpp @@ -84,6 +84,27 @@ int readMinitaurLogFile(const char* fileName, btAlignedObjectArray& switch (structTypes[i]) { case 'I': + { + size_t s = fread(blaat,sizeof(int),1,f); + if (s != 1) + { + eof = true; + retVal = eCorruptValue; + break; + + } + int v = (int) *(unsigned int*)blaat; + if (s==1) + { + if (verbose) + { + printf("%s = %d\n",structNames[i].c_str(),v); + } + record.m_values.push_back(v); + } + break; + } + case 'i': { size_t s = fread(blaat,sizeof(int),1,f); if (s != 1) @@ -219,6 +240,7 @@ void appendMinitaurLogData(FILE* f, std::string& structTypes, const MinitaurLogR { switch(structTypes[i]) { + case 'i': case 'I': { fwrite(&logData.m_values[i].m_intVal,sizeof(int),1,f); diff --git a/examples/pybullet/examples/dumpLog.py b/examples/pybullet/examples/dumpLog.py new file mode 100644 index 000000000..8cec0af5b --- /dev/null +++ b/examples/pybullet/examples/dumpLog.py @@ -0,0 +1,74 @@ +import time +import math +from datetime import datetime +import struct +import sys +import os, fnmatch +import argparse +from time import sleep + +def readLogFile(filename, verbose = True): + f = open(filename, 'rb') + + print('Opened'), + print(filename) + + keys = f.readline().decode('utf8').rstrip('\n').split(',') + fmt = f.readline().decode('utf8').rstrip('\n') + + # The byte number of one record + sz = struct.calcsize(fmt) + # The type number of one record + ncols = len(fmt) + + if verbose: + print('Keys:'), + print(keys) + print('Format:'), + print(fmt) + print('Size:'), + print(sz) + print('Columns:'), + print(ncols) + + # Read data + wholeFile = f.read() + # split by alignment word + chunks = wholeFile.split(b'\xaa\xbb') + log = list() + if verbose: + print("num chunks:") + print(len(chunks)) + chunkIndex = 0 + for chunk in chunks: + print("len(chunk)=",len(chunk)," sz = ", sz) + if len(chunk) == sz: + print("chunk #",chunkIndex) + chunkIndex=chunkIndex+1 + values = struct.unpack(fmt, chunk) + record = list() + for i in range(ncols): + record.append(values[i]) + if verbose: + print(" ",keys[i],"=",values[i]) + + log.append(record) + + return log + + +numArgs = len(sys.argv) + +print ('Number of arguments:', numArgs, 'arguments.') +print ('Argument List:', str(sys.argv)) +fileName = "log.bin" + +if (numArgs>1): + fileName = sys.argv[1] + +print("filename=") +print(fileName) + +verbose = True + +readLogFile(fileName,verbose) diff --git a/examples/pybullet/examples/manyspheres.py b/examples/pybullet/examples/manyspheres.py index 1c4c37b97..942bd02a7 100644 --- a/examples/pybullet/examples/manyspheres.py +++ b/examples/pybullet/examples/manyspheres.py @@ -1,7 +1,13 @@ import pybullet as p import time -p.connect(p.GUI) +conid = p.connect(p.SHARED_MEMORY) +if (conid<0): + p.connect(p.GUI) + +p.setInternalSimFlags(0) +p.resetSimulation() + p.loadURDF("plane.urdf",useMaximalCoordinates=True) p.loadURDF("tray/traybox.urdf",useMaximalCoordinates=True) @@ -12,7 +18,7 @@ p.setPhysicsEngineParameter(numSolverIterations=10) p.setPhysicsEngineParameter(contactBreakingThreshold=0.001) for i in range (10): for j in range (10): - for k in range (5): + for k in range (10): ob = p.loadURDF("sphere_1cm.urdf",[0.02*i,0.02*j,0.2+0.02*k],useMaximalCoordinates=True) p.setGravity(0,0,-10) p.setRealTimeSimulation(1) diff --git a/examples/pybullet/examples/quadruped.py b/examples/pybullet/examples/quadruped.py index cf097941e..97c5c3889 100644 --- a/examples/pybullet/examples/quadruped.py +++ b/examples/pybullet/examples/quadruped.py @@ -143,7 +143,7 @@ p.setRealTimeSimulation(useRealTime) print("quadruped Id = ") print(quadruped) p.saveWorld("quadru.py") -logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.txt",[quadruped]) +logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.bin",[quadruped]) diff --git a/examples/pybullet/examples/vr_kuka_setup.py b/examples/pybullet/examples/vr_kuka_setup.py index db80f4198..6d851c438 100644 --- a/examples/pybullet/examples/vr_kuka_setup.py +++ b/examples/pybullet/examples/vr_kuka_setup.py @@ -1,6 +1,9 @@ import pybullet as p #p.connect(p.UDP,"192.168.86.100") -p.connect(p.SHARED_MEMORY) + +cid = p.connect(p.SHARED_MEMORY) +if (cid<0): + p.connect(p.GUI) p.resetSimulation() objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] diff --git a/examples/pybullet/examples/vrtracker.py b/examples/pybullet/examples/vrtracker.py index 501b4b0d0..af5a71035 100644 --- a/examples/pybullet/examples/vrtracker.py +++ b/examples/pybullet/examples/vrtracker.py @@ -19,6 +19,9 @@ if (c<0): p.setInternalSimFlags(0)#don't load default robot assets etc p.resetSimulation() p.loadURDF("plane.urdf") +p.loadURDF("cube.urdf",0,0,1) +p.setGravity(0,0,-10) +p.setRealTimeSimulation(1) prevPosition=[None]*p.VR_MAX_CONTROLLERS colors=[0.,0.5,0.5]*p.VR_MAX_CONTROLLERS @@ -32,6 +35,10 @@ colors[3] = [0,0,0.5] colors[4] = [0.5,0.5,0.] colors[5] = [.5,.5,.5] +p.startStateLogging(p.STATE_LOGGING_VR_CONTROLLERS, "vr_hmd.bin",deviceTypeFilter=p.VR_DEVICE_HMD) +p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "generic_data.bin") +p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS, "contact_points.bin") + while True: events = p.getVREvents(p.VR_DEVICE_HMD+p.VR_DEVICE_GENERIC_TRACKER) for e in (events): diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d3160dcf6..9662e9895 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2697,8 +2697,8 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyUniqueIdA", "bodyUniqueIdB", "linkIndexA", "linkIndexB", "deviceTypeFilter", "physicsClientId", NULL}; int physicsClientId = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiii", kwlist, - &loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &bodyUniqueIdA, &bodyUniqueIdB, &linkIndexA, &linkIndexB, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiiii", kwlist, + &loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &bodyUniqueIdA, &bodyUniqueIdB, &linkIndexA, &linkIndexB, &deviceTypeFilter, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId);