From 5158c1ae9886ad88215f540806851686ced10c32 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 12 Apr 2017 15:02:47 -0700 Subject: [PATCH] add python dumpLog.py utility to view log files created using 'startStateLogging' don't enable for hotkeys yet add some more profile markers log objectId and linkIndex as signed int ('i') and not unsigned int 'I' fix issue in startStateLogging: number of parameters was wrong --- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 2 +- .../ImportMeshUtility/b3ImportMeshUtility.cpp | 7 +- .../ImportObjDemo/LoadMeshFromObj.cpp | 15 +++- .../ImportURDFDemo/BulletUrdfImporter.cpp | 25 +++++-- .../PhysicsServerCommandProcessor.cpp | 10 ++- examples/Utils/RobotLoggingUtil.cpp | 22 ++++++ examples/pybullet/examples/dumpLog.py | 74 +++++++++++++++++++ examples/pybullet/examples/manyspheres.py | 10 ++- examples/pybullet/examples/quadruped.py | 2 +- examples/pybullet/examples/vr_kuka_setup.py | 5 +- examples/pybullet/examples/vrtracker.py | 7 ++ examples/pybullet/pybullet.c | 4 +- 12 files changed, 158 insertions(+), 25 deletions(-) create mode 100644 examples/pybullet/examples/dumpLog.py 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);