From 34c3fca8d5ecddeb4a1dbddb1e0f846c2933128e Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 17 Feb 2017 10:47:55 -0800 Subject: [PATCH 1/3] prepare state logging system (log state of robot, vr controllers after each stepSimulation) --- examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/premake4.lua | 3 + examples/SharedMemory/CMakeLists.txt | 2 + examples/SharedMemory/PhysicsClientC_API.cpp | 39 ++++++++ examples/SharedMemory/PhysicsClientC_API.h | 6 +- .../PhysicsServerCommandProcessor.cpp | 93 +++++++++++++++++++ .../PhysicsServerCommandProcessor.h | 9 +- examples/SharedMemory/SharedMemoryCommands.h | 8 ++ examples/SharedMemory/SharedMemoryPublic.h | 7 ++ examples/SharedMemory/premake4.lua | 4 +- examples/SharedMemory/udp/premake4.lua | 4 +- examples/pybullet/CMakeLists.txt | 3 + examples/pybullet/premake4.lua | 2 + examples/pybullet/pybullet.c | 65 ++++++++++++- test/CMakeLists.txt | 2 +- test/RobotLogging/main.cpp | 81 ++++++++++++++++ test/SharedMemory/CMakeLists.txt | 2 + test/SharedMemory/premake4.lua | 6 ++ 18 files changed, 330 insertions(+), 8 deletions(-) create mode 100644 test/RobotLogging/main.cpp diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 68b5d6d1f..b7cf1fd06 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -317,6 +317,8 @@ SET(BulletExampleBrowser_SRCS ../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp ../Importers/ImportURDFDemo/MyMultiBodyCreator.h ../Importers/ImportURDFDemo/UrdfParser.cpp + ../Utils/RobotLoggingUtil.cpp + ../Utils/RobotLoggingUtil.h ../Importers/ImportURDFDemo/urdfStringSplit.cpp ../Importers/ImportURDFDemo/urdfStringSplit.h ../Importers/ImportURDFDemo/BulletUrdfImporter.cpp diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 5e6e41351..0c44e93bb 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -97,6 +97,8 @@ project "App_BulletExampleBrowser" "../BasicDemo/BasicExample.*", "../Tutorial/*", "../ExtendedTutorials/*", + "../Utils/RobotLoggingUtil.cpp", + "../Utils/RobotLoggingUtil.h", "../Evolution/NN3DWalkers.cpp", "../Evolution/NN3DWalkers.h", "../Collision/*", @@ -193,6 +195,7 @@ project "BulletExampleBrowserLib" "OpenGLGuiHelper.cpp", "OpenGLExampleBrowser.cpp", "../Utils/b3Clock.cpp", + "../Utils/b3Clock.h", "../Utils/ChromeTraceUtil.cpp", "../Utils/ChromeTraceUtil.h", "*.h", diff --git a/examples/SharedMemory/CMakeLists.txt b/examples/SharedMemory/CMakeLists.txt index 5db17f0e5..a2c852581 100644 --- a/examples/SharedMemory/CMakeLists.txt +++ b/examples/SharedMemory/CMakeLists.txt @@ -61,6 +61,8 @@ SET(SharedMemory_SRCS ../Importers/ImportMJCFDemo/BulletMJCFImporter.h ../Utils/b3ResourcePath.cpp ../Utils/b3Clock.cpp + ../Utils/RobotLoggingUtil.cpp + ../Utils/RobotLoggingUtil.h ../Utils/ChromeTraceUtil.cpp ../Utils/ChromeTraceUtil.h ../Importers/ImportURDFDemo/URDFImporterInterface.h diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index f02e332a4..30e543a7f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2457,3 +2457,42 @@ int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int o return 0; } +b3SharedMemoryCommandHandle b3RobotLoggingCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_ROBOT_LOGGING; + command->m_updateFlags = 0; + + return (b3SharedMemoryCommandHandle)command; + +} + +int b3RobotLoggingStartMinitaurLog(b3SharedMemoryCommandHandle commandHandle, const char* fileName, int objectUniqueId) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_ROBOT_LOGGING); + if (command->m_type == CMD_ROBOT_LOGGING) + { + command->m_updateFlags |= ROBOT_LOGGING_START_MINITAUR_LOG; + } + return 0; +} + +int b3RobotLoggingStopMinitaurLog(b3SharedMemoryCommandHandle commandHandle) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_ROBOT_LOGGING); + if (command->m_type == CMD_ROBOT_LOGGING) + { + command->m_updateFlags |= ROBOT_LOGGING_STOP_MINITAUR_LOG; + } + return 0; +} + diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index febe08396..63a5a9847 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -343,8 +343,10 @@ int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]); int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); - - +b3SharedMemoryCommandHandle b3RobotLoggingCommandInit(b3PhysicsClientHandle physClient); +int b3RobotLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName); +int b3RobotLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName); +int b3RobotLoggingStop(b3SharedMemoryCommandHandle commandHandle); #ifdef __cplusplus } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index cd4260469..fb35cb66e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -757,6 +757,97 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() delete m_data; } +void logCallback(btDynamicsWorld *world, btScalar timeStep) +{ + PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*) world->getWorldUserInfo(); + proc->logObjectStates(timeStep); + +} +#include "../Utils/RobotLoggingUtil.h" + +void PhysicsServerCommandProcessor::logObjectStates(btScalar timeStep) +{ + //quick hack + static FILE* logFile = 0; + static btScalar logTime = 0; + //printf("log state at time %f\n",logTime); + + btAlignedObjectArray structNames; + std::string structTypes; + //'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo' + structNames.push_back("t"); + structNames.push_back("r"); + structNames.push_back("p"); + structNames.push_back("y"); + + structNames.push_back("q0"); + structNames.push_back("q1"); + structNames.push_back("q2"); + structNames.push_back("q3"); + structNames.push_back("q4"); + structNames.push_back("q5"); + structNames.push_back("q6"); + structNames.push_back("q7"); + + structNames.push_back("u0"); + structNames.push_back("u1"); + structNames.push_back("u2"); + structNames.push_back("u3"); + structNames.push_back("u4"); + structNames.push_back("u5"); + structNames.push_back("u6"); + structNames.push_back("u7"); + + structNames.push_back("dx"); + structNames.push_back("mo"); + + + +/* structNames.push_back("timeStamp"); + structNames.push_back("objectId"); + structNames.push_back("posX"); + structNames.push_back("posY"); + structNames.push_back("posZ"); + */ + + structTypes = "fIfff";//I = int, f = float, B char + + if (logFile==0) + { + logFile = createMinitaurLogFile("d:/logTest.txt", structNames, structTypes); + } + + if (logFile) + { + for (int i=0;im_dynamicsWorld->getNumMultibodies();i++) + { + btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i); + btVector3 pos = mb->getBasePos(); + + MinitaurLogRecord logData; + float timeStamp = logTime; + int objectUniqueId = mb->getUserIndex2(); + float posX = pos[0]; + float posY = pos[1]; + float posZ = pos[2]; + + logData.m_values.push_back(timeStamp); + logData.m_values.push_back(objectUniqueId); + logData.m_values.push_back(posX); + logData.m_values.push_back(posY); + logData.m_values.push_back(posZ); + //at the moment, appendMinitaurLogData will directly write to disk (potential delay) + //better to fill a huge memory buffer and once in a while write it to disk + appendMinitaurLogData(logFile, structTypes, logData); + + } + fflush(logFile); + } + //void closeMinitaurLogFile(FILE* f); + + logTime += timeStep; + +} void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() { @@ -803,6 +894,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() // m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2; //todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp) + + m_data->m_dynamicsWorld->setInternalTickCallback(logCallback,this); } void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies() diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 66dc230f9..481e46f85 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -20,6 +20,8 @@ class PhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface struct PhysicsServerCommandProcessorInternalData* m_data; + + //todo: move this to physics client side / Python void createDefaultRobotAssets(); @@ -84,10 +86,15 @@ public: virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); virtual void removePickingConstraint(); - + + //logging /playback the shared memory commands void enableCommandLogging(bool enable, const char* fileName); void replayFromLogFile(const char* fileName); void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes ); + + //logging of object states (position etc) + void logObjectStates(btScalar timeStep); + void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents); void enableRealTimeSimulation(bool enableRealTimeSim); void applyJointDamping(int bodyUniqueId); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 847a0de75..a4f665abd 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -611,6 +611,14 @@ enum eVRCameraEnums VR_CAMERA_ROOT_TRACKING_OBJECT=4 }; +enum eRobotLoggingEnums +{ + ROBOT_LOGGING_START_MINITAUR_LOG=1, + ROBOT_LOGGING_STOP_MINITAUR_LOG=1, + ROBOT_LOGGING_START_GENERIC_LOG=1, + ROBOT_LOGGING_STOP_GENERIC_LOG=1, +}; + struct VRCameraState { double m_rootPosition[3]; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 913fe086b..11a8dc93a 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -48,6 +48,7 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_VR_EVENTS_DATA, CMD_SET_VR_CAMERA_STATE, CMD_SYNC_BODY_INFO, + CMD_ROBOT_LOGGING, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -301,6 +302,12 @@ enum CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS = 1, }; +enum +{ + ROBOT_LOGGING_MINITAUR = 0, + ROBOT_LOGGING_GENERIC_ROBOT = 1, + ROBOT_LOGGING_VR_CONTROLLERS = 2, +}; struct b3ContactInformation diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index 3bb14c6b0..399cf1473 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -76,7 +76,9 @@ myfiles = "../Importers/ImportMJCFDemo/BulletMJCFImporter.cpp", "../Importers/ImportMJCFDemo/BulletMJCFImporter.h", "../Utils/b3ResourcePath.cpp", - "../Utils/b3Clock.cpp", + "../Utils/b3Clock.cpp", + "../Utils/RobotLoggingUtil.cpp", + "../Utils/RobotLoggingUtil.h", "../Utils/ChromeTraceUtil.cpp", "../Utils/ChromeTraceUtil.h", "../../Extras/Serialize/BulletWorldImporter/*", diff --git a/examples/SharedMemory/udp/premake4.lua b/examples/SharedMemory/udp/premake4.lua index 7f3ef58a8..174625ed3 100644 --- a/examples/SharedMemory/udp/premake4.lua +++ b/examples/SharedMemory/udp/premake4.lua @@ -103,7 +103,9 @@ myfiles = "../../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../../Importers/ImportURDFDemo/URDF2Bullet.h", "../../Utils/b3ResourcePath.cpp", - "../../Utils/b3Clock.cpp", + "../../Utils/b3Clock.cpp", + "../../Utils/RobotLoggingUtil.cpp", + "../../Utils/RobotLoggingUtil.h", "../../../Extras/Serialize/BulletWorldImporter/*", "../../../Extras/Serialize/BulletFileLoader/*", "../../Importers/ImportURDFDemo/URDFImporterInterface.h", diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index 0a10c0677..e2d302370 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -54,6 +54,9 @@ SET(pybullet_SRCS ../../examples/SharedMemory/PosixSharedMemory.h ../../examples/Utils/b3ResourcePath.cpp ../../examples/Utils/b3ResourcePath.h + ../../examples/Utils/RobotLoggingUtil.cpp + ../../examples/Utils/RobotLoggingUtil.h + ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index 0b856bd4b..a47016c8b 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -101,6 +101,8 @@ if not _OPTIONS["no-enet"] then "../../examples/SharedMemory/PosixSharedMemory.h", "../../examples/Utils/b3ResourcePath.cpp", "../../examples/Utils/b3ResourcePath.h", + "../../examples/Utils/RobotLoggingUtil.cpp", + "../../examples/Utils/RobotLoggingUtil.h", "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp", "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 65cc7e108..2e3fe51e7 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2583,6 +2583,58 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args return Py_None; } +static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyObject *keywds) +{ + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + b3PhysicsClientHandle sm = 0; + int loggingType = -1; + char* fileName = 0; + PyObject* objectUniqueIdsObj = 0; + + static char *kwlist[] = { "loggingType", "fileName", "objectUniqueIds", "physicsClientId", NULL }; + int physicsClientId = 0; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oi", kwlist, + &loggingType, &fileName, &objectUniqueIdsObj,&physicsClientId)) + return NULL; + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } +} + +static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObject *keywds) +{ + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + PyObject* rayFromObj=0; + PyObject* rayToObj=0; + double from[3]; + double to[3]; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "loggingId", "physicsClientId", NULL }; + int physicsClientId = 0; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, + &rayFromObj, &rayToObj,&physicsClientId)) + return NULL; + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } +} + + static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keywds) { @@ -4960,8 +5012,14 @@ static PyMethodDef SpamMethods[] = { "Set properties of the VR Camera such as its root transform " "for teleporting or to track objects (camera inside a vehicle for example)." }, - - + { "startRobotStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS, + "Start logging of state, such as robot base position, orientation, joint positions etc. " + "Specify loggingType (ROBOT_LOG_MINITAUR, ROBOT_LOG_GENERIC_ROBOT, ROBOT_LOG_VR_CONTROLLERS etc), " + "fileName, optional objectUniqueId. Function returns int loggingUniqueId" + }, + { "stopRobotStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS, + "Stop logging of robot state, given a loggingUniqueId." + }, { "rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS, "Cast a ray and return the first object hit, if any. " "Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates" @@ -5047,6 +5105,9 @@ initpybullet(void) PyModule_AddIntConstant(m, "VR_MAX_CONTROLLERS", MAX_VR_CONTROLLERS); PyModule_AddIntConstant(m, "VR_MAX_BUTTONS", MAX_VR_BUTTONS); + PyModule_AddIntConstant(m, "ROBOT_LOGGING_MINITAUR", ROBOT_LOGGING_MINITAUR); + PyModule_AddIntConstant(m, "ROBOT_LOGGING_GENERIC_ROBOT", ROBOT_LOGGING_GENERIC_ROBOT); + PyModule_AddIntConstant(m, "ROBOT_LOGGING_VR_CONTROLLERS", ROBOT_LOGGING_VR_CONTROLLERS); SpamError = PyErr_NewException("pybullet.error", NULL, NULL); Py_INCREF(SpamError); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 2b08544e7..713e4937e 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -3,5 +3,5 @@ IF(BUILD_BULLET3) SUBDIRS( InverseDynamics SharedMemory ) ENDIF(BUILD_BULLET3) -SUBDIRS( gtest-1.7.0 collision BulletDynamics/pendulum ) +SUBDIRS( gtest-1.7.0 collision RobotLogging BulletDynamics/pendulum ) diff --git a/test/RobotLogging/main.cpp b/test/RobotLogging/main.cpp new file mode 100644 index 000000000..a2e1b3992 --- /dev/null +++ b/test/RobotLogging/main.cpp @@ -0,0 +1,81 @@ +#include "Utils/RobotLoggingUtil.h" + +#ifndef ENABLE_GTEST + +#include +#define ASSERT_EQ(a,b) assert((a)==(b)); +#else +#include +#define printf +#endif + + +void testMinitaurLogging() +{ + const char* fileName = "d:/logTest.txt"; + btAlignedObjectArray structNames; + std::string structTypes; + btAlignedObjectArray logRecords; + bool verbose = false; + + int status = readMinitaurLogFile(fileName, structNames, structTypes, logRecords, verbose); + + for (int i=0;i Date: Fri, 17 Feb 2017 14:25:53 -0800 Subject: [PATCH 2/3] initial implementation of state logging. see examples/pybullet/logMinitaur.py for example. Other state logging will include general robot states and VR controllers state. --- examples/SharedMemory/PhysicsClientC_API.cpp | 59 +++- examples/SharedMemory/PhysicsClientC_API.h | 9 +- .../PhysicsClientSharedMemory.cpp | 14 + .../PhysicsServerCommandProcessor.cpp | 314 +++++++++++++----- .../PhysicsServerCommandProcessor.h | 1 + examples/SharedMemory/SharedMemoryCommands.h | 27 +- examples/SharedMemory/SharedMemoryPublic.h | 12 +- examples/pybullet/logMinitaur.py | 15 + examples/pybullet/pybullet.c | 67 +++- 9 files changed, 402 insertions(+), 116 deletions(-) create mode 100644 examples/pybullet/logMinitaur.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 8605fd986..4e5b4982b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2467,7 +2467,7 @@ int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int o return 0; } -b3SharedMemoryCommandHandle b3RobotLoggingCommandInit(b3PhysicsClientHandle physClient) +b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -2475,33 +2475,72 @@ b3SharedMemoryCommandHandle b3RobotLoggingCommandInit(b3PhysicsClientHandle phys struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); - command->m_type = CMD_ROBOT_LOGGING; + command->m_type = CMD_STATE_LOGGING; command->m_updateFlags = 0; + command->m_stateLoggingArguments.m_numBodyUniqueIds = 0; return (b3SharedMemoryCommandHandle)command; } -int b3RobotLoggingStartMinitaurLog(b3SharedMemoryCommandHandle commandHandle, const char* fileName, int objectUniqueId) +int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); - b3Assert(command->m_type == CMD_ROBOT_LOGGING); - if (command->m_type == CMD_ROBOT_LOGGING) + b3Assert(command->m_type == CMD_STATE_LOGGING); + if (command->m_type == CMD_STATE_LOGGING) { - command->m_updateFlags |= ROBOT_LOGGING_START_MINITAUR_LOG; + command->m_updateFlags |= STATE_LOGGING_START_LOG; + int len = strlen(fileName); + if (len < MAX_FILENAME_LENGTH) + { + strcpy(command->m_stateLoggingArguments.m_fileName, fileName); + } + else + { + command->m_stateLoggingArguments.m_fileName[0] = 0; + } + command->m_stateLoggingArguments.m_logType = loggingType; } return 0; } -int b3RobotLoggingStopMinitaurLog(b3SharedMemoryCommandHandle commandHandle) +int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + b3Assert(status->m_type == CMD_STATE_LOGGING_START_COMPLETED); + if (status && status->m_type == CMD_STATE_LOGGING_START_COMPLETED) + { + return status->m_stateLoggingResultArgs.m_loggingUniqueId; + } + return -1; +} + +int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); - b3Assert(command->m_type == CMD_ROBOT_LOGGING); - if (command->m_type == CMD_ROBOT_LOGGING) + b3Assert(command->m_type == CMD_STATE_LOGGING); + if (command->m_type == CMD_STATE_LOGGING) { - command->m_updateFlags |= ROBOT_LOGGING_STOP_MINITAUR_LOG; + command->m_updateFlags |= STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID; + if (command->m_stateLoggingArguments.m_numBodyUniqueIds < MAX_SDF_BODIES) + { + command->m_stateLoggingArguments.m_bodyUniqueIds[command->m_stateLoggingArguments.m_numBodyUniqueIds++] = objectUniqueId; + } + } + return 0; +} +int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_STATE_LOGGING); + if (command->m_type == CMD_STATE_LOGGING) + { + command->m_updateFlags |= STATE_LOGGING_STOP_LOG; + command->m_stateLoggingArguments.m_loggingUniqueId = loggingUid; } return 0; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4993affcc..b22b9635c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -343,10 +343,11 @@ int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]); int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); -b3SharedMemoryCommandHandle b3RobotLoggingCommandInit(b3PhysicsClientHandle physClient); -int b3RobotLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName); -int b3RobotLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName); -int b3RobotLoggingStop(b3SharedMemoryCommandHandle commandHandle); +b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient); +int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName); +int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); +int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle); +int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId); #ifdef __cplusplus } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index d635012a1..b9b187b92 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -899,6 +899,20 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { { break; } + case CMD_STATE_LOGGING_START_COMPLETED: + { + break; + }; + case CMD_STATE_LOGGING_COMPLETED: + { + break; + }; + + case CMD_STATE_LOGGING_FAILED: + { + b3Warning("State Logging failed"); + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index eafdedea7..2027e1514 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -17,7 +17,7 @@ #include "BulletInverseDynamics/MultiBodyTree.hpp" #include "IKTrajectoryHelper.h" #include "btBulletDynamicsCommon.h" - +#include "../Utils/RobotLoggingUtil.h" #include "LinearMath/btTransform.h" #include "../Importers/ImportMJCFDemo/BulletMJCFImporter.h" #include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h" @@ -420,6 +420,142 @@ struct MyOverlapFilterCallback : public btOverlapFilterCallback }; +struct InternalStateLogger +{ + int m_loggingUniqueId; + int m_loggingType; + + InternalStateLogger() + :m_loggingUniqueId(0), + m_loggingType(0) + { + } + + virtual void stop() = 0; + virtual void logState(btScalar timeStamp)=0; + +}; + +struct MinitaurStateLogger : public InternalStateLogger +{ + int m_loggingTimeStamp; + std::string m_fileName; + int m_minitaurBodyUniqueId; + FILE* m_logFileHandle; + + std::string m_structTypes; + btMultiBody* m_minitaurMultiBody; + btAlignedObjectArray m_motorIdList; + + MinitaurStateLogger(int loggingUniqueId, std::string fileName, btMultiBody* minitaurMultiBody, btAlignedObjectArray& motorIdList) + :m_loggingTimeStamp(0), + m_logFileHandle(0), + m_minitaurMultiBody(minitaurMultiBody) + { + m_loggingType = STATE_LOGGING_MINITAUR; + m_motorIdList.resize(motorIdList.size()); + for (int m=0;m structNames; + //'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo' + structNames.push_back("t"); + structNames.push_back("r"); + structNames.push_back("p"); + structNames.push_back("y"); + + structNames.push_back("q0"); + structNames.push_back("q1"); + structNames.push_back("q2"); + structNames.push_back("q3"); + structNames.push_back("q4"); + structNames.push_back("q5"); + structNames.push_back("q6"); + structNames.push_back("q7"); + + structNames.push_back("u0"); + structNames.push_back("u1"); + structNames.push_back("u2"); + structNames.push_back("u3"); + structNames.push_back("u4"); + structNames.push_back("u5"); + structNames.push_back("u6"); + structNames.push_back("u7"); + + structNames.push_back("dx"); + structNames.push_back("mo"); + + m_structTypes = "IffffffffffffffffffffB"; + const char* fileNameC = fileName.c_str(); + + m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes); + } + virtual void stop() + { + if (m_logFileHandle) + { + closeMinitaurLogFile(m_logFileHandle); + m_logFileHandle = 0; + } + } + + virtual void logState(btScalar timeStep) + { + if (m_logFileHandle) + { + + btVector3 pos = m_minitaurMultiBody->getBasePos(); + + MinitaurLogRecord logData; + //'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo' + btScalar motorDir[8] = {1, -1, 1, -1, -1, 1, -1, 1}; + + btQuaternion orn = m_minitaurMultiBody->getBaseWorldTransform().getRotation(); + btMatrix3x3 mat(orn); + btScalar roll=0; + btScalar pitch=0; + btScalar yaw = 0; + + mat.getEulerZYX(yaw,pitch,roll); + + logData.m_values.push_back(m_loggingTimeStamp); + logData.m_values.push_back((float)roll); + logData.m_values.push_back((float)pitch); + logData.m_values.push_back((float)yaw); + + for (int i=0;i<8;i++) + { + float jointAngle = (float)motorDir[i]*m_minitaurMultiBody->getJointPos(m_motorIdList[i]); + logData.m_values.push_back(jointAngle); + } + for (int i=0;i<8;i++) + { + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_minitaurMultiBody->getLink(m_motorIdList[i]).m_userPtr; + + if (motor && timeStep>btScalar(0)) + { + btScalar force = motor->getAppliedImpulse(0)/timeStep; + logData.m_values.push_back((float)force); + } + } + //x is forward component, estimated speed forward + float xd_speed = m_minitaurMultiBody->getBaseVel()[0]; + logData.m_values.push_back(xd_speed); + char mode = 6; + logData.m_values.push_back(mode); + + //at the moment, appendMinitaurLogData will directly write to disk (potential delay) + //better to fill a huge memory buffer and once in a while write it to disk + appendMinitaurLogData(m_logFileHandle, m_structTypes, logData); + + fflush(m_logFileHandle); + + m_loggingTimeStamp++; + } + } +}; struct PhysicsServerCommandProcessorInternalData @@ -577,7 +713,8 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray m_sdfRecentLoadedBodies; - + btAlignedObjectArray m_stateLoggers; + int m_stateLoggersUniqueId; struct GUIHelperInterface* m_guiHelper; int m_sharedMemoryKey; @@ -622,6 +759,7 @@ struct PhysicsServerCommandProcessorInternalData m_collisionConfiguration(0), m_dynamicsWorld(0), m_remoteDebugDrawer(0), + m_stateLoggersUniqueId(0), m_guiHelper(0), m_sharedMemoryKey(SHARED_MEMORY_KEY), m_verboseOutput(false), @@ -764,90 +902,15 @@ void logCallback(btDynamicsWorld *world, btScalar timeStep) proc->logObjectStates(timeStep); } -#include "../Utils/RobotLoggingUtil.h" + void PhysicsServerCommandProcessor::logObjectStates(btScalar timeStep) { - //quick hack - static FILE* logFile = 0; - static btScalar logTime = 0; - //printf("log state at time %f\n",logTime); - - btAlignedObjectArray structNames; - std::string structTypes; - //'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo' - structNames.push_back("t"); - structNames.push_back("r"); - structNames.push_back("p"); - structNames.push_back("y"); - - structNames.push_back("q0"); - structNames.push_back("q1"); - structNames.push_back("q2"); - structNames.push_back("q3"); - structNames.push_back("q4"); - structNames.push_back("q5"); - structNames.push_back("q6"); - structNames.push_back("q7"); - - structNames.push_back("u0"); - structNames.push_back("u1"); - structNames.push_back("u2"); - structNames.push_back("u3"); - structNames.push_back("u4"); - structNames.push_back("u5"); - structNames.push_back("u6"); - structNames.push_back("u7"); - - structNames.push_back("dx"); - structNames.push_back("mo"); - - - -/* structNames.push_back("timeStamp"); - structNames.push_back("objectId"); - structNames.push_back("posX"); - structNames.push_back("posY"); - structNames.push_back("posZ"); - */ - - structTypes = "fIfff";//I = int, f = float, B char - - if (logFile==0) + for (int i=0;im_stateLoggers.size();i++) { - logFile = createMinitaurLogFile("d:/logTest.txt", structNames, structTypes); + m_data->m_stateLoggers[i]->logState(timeStep); } - if (logFile) - { - for (int i=0;im_dynamicsWorld->getNumMultibodies();i++) - { - btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i); - btVector3 pos = mb->getBasePos(); - - MinitaurLogRecord logData; - float timeStamp = logTime; - int objectUniqueId = mb->getUserIndex2(); - float posX = pos[0]; - float posY = pos[1]; - float posZ = pos[2]; - - logData.m_values.push_back(timeStamp); - logData.m_values.push_back(objectUniqueId); - logData.m_values.push_back(posX); - logData.m_values.push_back(posY); - logData.m_values.push_back(posZ); - //at the moment, appendMinitaurLogData will directly write to disk (potential delay) - //better to fill a huge memory buffer and once in a while write it to disk - appendMinitaurLogData(logFile, structTypes, logData); - - } - fflush(logFile); - } - //void closeMinitaurLogFile(FILE* f); - - logTime += timeStep; - } void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() @@ -899,6 +962,17 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld->setInternalTickCallback(logCallback,this); } +void PhysicsServerCommandProcessor::deleteStateLoggers() +{ + for (int i=0;im_stateLoggers.size();i++) + { + m_data->m_stateLoggers[i]->stop(); + delete m_data->m_stateLoggers[i]; + } + m_data->m_stateLoggers.clear(); + +} + void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies() { for (int i = 0; i < m_data->m_inverseKinematicsHelpers.size(); i++) @@ -931,6 +1005,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() { deleteCachedInverseDynamicsBodies(); deleteCachedInverseKinematicsBodies(); + deleteStateLoggers(); m_data->m_userConstraints.clear(); m_data->m_saveWorldBodyData.clear(); @@ -1544,7 +1619,85 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } #endif + case CMD_STATE_LOGGING: + { + serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED; + hasStatus = true; + if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG) + { + + + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_MINITAUR) + { + + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + //either provide the minitaur by object unique Id, or search for first multibody with 8 motors... + + + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID)&& (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0)) + { + int bodyUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[0]; + InteralBodyData* body = m_data->getHandle(bodyUniqueId); + if (body) + { + if (body->m_multiBody) + { + btAlignedObjectArray motorNames; + motorNames.push_back("motor_front_leftR_joint"); + motorNames.push_back("motor_front_leftL_joint"); + motorNames.push_back("motor_back_leftR_joint"); + motorNames.push_back("motor_back_leftL_joint"); + motorNames.push_back("motor_front_rightL_joint"); + motorNames.push_back("motor_front_rightR_joint"); + motorNames.push_back("motor_back_rightL_joint"); + motorNames.push_back("motor_back_rightR_joint"); + + btAlignedObjectArray motorIdList; + for (int m=0;mm_multiBody->getNumLinks();i++) + { + std::string jointName; + if (body->m_multiBody->getLink(i).m_jointName) + { + jointName = body->m_multiBody->getLink(i).m_jointName; + } + if (motorNames[m]==jointName) + { + motorIdList.push_back(i); + } + } + } + + if (motorIdList.size()==8) + { + int loggerUid = m_data->m_stateLoggersUniqueId++; + MinitaurStateLogger* logger = new MinitaurStateLogger(loggerUid,fileName,body->m_multiBody, motorIdList); + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + } + } + } + } + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId>=0) + { + serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; + for (int i=0;im_stateLoggers.size();i++) + { + if (m_data->m_stateLoggers[i]->m_loggingUniqueId==clientCmd.m_stateLoggingArguments.m_loggingUniqueId) + { + m_data->m_stateLoggers[i]->stop(); + delete m_data->m_stateLoggers[i]; + m_data->m_stateLoggers.removeAtIndex(i); + } + } + } + break; + } case CMD_SET_VR_CAMERA_STATE: { @@ -4786,7 +4939,6 @@ void PhysicsServerCommandProcessor::applyJointDamping(int bodyUniqueId) void PhysicsServerCommandProcessor::resetSimulation() { //clean up all data - deleteCachedInverseDynamicsBodies(); if (m_data && m_data->m_guiHelper) { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 481e46f85..f90716665 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -46,6 +46,7 @@ protected: int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes); void deleteCachedInverseDynamicsBodies(); void deleteCachedInverseKinematicsBodies(); + void deleteStateLoggers(); public: PhysicsServerCommandProcessor(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 3e8d93324..5bc2a508d 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -613,12 +613,11 @@ enum eVRCameraEnums VR_CAMERA_ROOT_TRACKING_OBJECT=4 }; -enum eRobotLoggingEnums +enum eStateLoggingEnums { - ROBOT_LOGGING_START_MINITAUR_LOG=1, - ROBOT_LOGGING_STOP_MINITAUR_LOG=1, - ROBOT_LOGGING_START_GENERIC_LOG=1, - ROBOT_LOGGING_STOP_GENERIC_LOG=1, + STATE_LOGGING_START_LOG=1, + STATE_LOGGING_STOP_LOG=2, + STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID=4, }; struct VRCameraState @@ -629,6 +628,21 @@ struct VRCameraState }; + +struct StateLoggingRequest +{ + char m_fileName[MAX_FILENAME_LENGTH]; + int m_logType;//Minitaur, generic robot, VR states + int m_numBodyUniqueIds;////only if ROBOT_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set + int m_bodyUniqueIds[MAX_SDF_BODIES]; + int m_loggingUniqueId; +}; + +struct StateLoggingResultArgs +{ + int m_loggingUniqueId; +}; + struct SharedMemoryCommand { int m_type; @@ -670,6 +684,7 @@ struct SharedMemoryCommand struct RequestRaycastIntersections m_requestRaycastIntersections; struct LoadBunnyArgs m_loadBunnyArguments; struct VRCameraState m_vrCameraStateArguments; + struct StateLoggingRequest m_stateLoggingArguments; }; }; @@ -730,6 +745,8 @@ struct SharedMemoryStatus struct b3UserConstraint m_userConstraintResultArgs; struct SendVREvents m_sendVREvents; struct SendRaycastHits m_raycastHits; + struct StateLoggingResultArgs m_stateLoggingResultArgs; + }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 11a8dc93a..526a4ec4d 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -48,7 +48,7 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_VR_EVENTS_DATA, CMD_SET_VR_CAMERA_STATE, CMD_SYNC_BODY_INFO, - CMD_ROBOT_LOGGING, + CMD_STATE_LOGGING, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -123,6 +123,9 @@ enum EnumSharedMemoryServerStatus CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED, CMD_SYNC_BODY_INFO_COMPLETED, CMD_SYNC_BODY_INFO_FAILED, + CMD_STATE_LOGGING_COMPLETED, + CMD_STATE_LOGGING_START_COMPLETED, + CMD_STATE_LOGGING_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -304,9 +307,10 @@ enum enum { - ROBOT_LOGGING_MINITAUR = 0, - ROBOT_LOGGING_GENERIC_ROBOT = 1, - ROBOT_LOGGING_VR_CONTROLLERS = 2, + STATE_LOGGING_MINITAUR = 0, + STATE_LOGGING_GENERIC_ROBOT = 1, + STATE_LOGGING_VR_CONTROLLERS = 2, + STATE_LOGGING_COMMANDS = 3, }; diff --git a/examples/pybullet/logMinitaur.py b/examples/pybullet/logMinitaur.py new file mode 100644 index 000000000..b995ee8f7 --- /dev/null +++ b/examples/pybullet/logMinitaur.py @@ -0,0 +1,15 @@ +import pybullet as p +cid = p.connect(p.SHARED_MEMORY) +if (cid < 0) : + p.connect(p.GUI) +p.loadURDF("plane.urdf") + +quadruped = p.loadURDF("quadruped/quadruped.urdf") +logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"LOG00048.TXT",[quadruped]) +p.stepSimulation() +p.stepSimulation() +p.stepSimulation() +p.stepSimulation() +p.stepSimulation() + +p.stopStateLogging(logId) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 93a1093fc..e5400eef6 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2614,6 +2614,40 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } + { + b3SharedMemoryCommandHandle commandHandle; + commandHandle = b3StateLoggingCommandInit(sm); + + b3StateLoggingStart(commandHandle,loggingType,fileName); + + if (objectUniqueIdsObj) + { + PyObject* seq = PySequence_Fast(objectUniqueIdsObj, "expected a sequence of object unique ids"); + if (seq) + { + int len = PySequence_Size(objectUniqueIdsObj); + for (int i=0;i=0) + { + b3SharedMemoryCommandHandle commandHandle; + commandHandle = b3StateLoggingCommandInit(sm); + b3StateLoggingStop(commandHandle, loggingId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + } + Py_INCREF(Py_None); + return Py_None; } @@ -5019,12 +5062,12 @@ static PyMethodDef SpamMethods[] = { "Set properties of the VR Camera such as its root transform " "for teleporting or to track objects (camera inside a vehicle for example)." }, - { "startRobotStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS, + { "startStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS, "Start logging of state, such as robot base position, orientation, joint positions etc. " - "Specify loggingType (ROBOT_LOG_MINITAUR, ROBOT_LOG_GENERIC_ROBOT, ROBOT_LOG_VR_CONTROLLERS etc), " + "Specify loggingType (STATE_LOG_MINITAUR, STATE_LOG_GENERIC_ROBOT, STATE_LOG_VR_CONTROLLERS etc), " "fileName, optional objectUniqueId. Function returns int loggingUniqueId" }, - { "stopRobotStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS, + { "stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS, "Stop logging of robot state, given a loggingUniqueId." }, { "rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS, @@ -5112,9 +5155,9 @@ initpybullet(void) PyModule_AddIntConstant(m, "VR_MAX_CONTROLLERS", MAX_VR_CONTROLLERS); PyModule_AddIntConstant(m, "VR_MAX_BUTTONS", MAX_VR_BUTTONS); - PyModule_AddIntConstant(m, "ROBOT_LOGGING_MINITAUR", ROBOT_LOGGING_MINITAUR); - PyModule_AddIntConstant(m, "ROBOT_LOGGING_GENERIC_ROBOT", ROBOT_LOGGING_GENERIC_ROBOT); - PyModule_AddIntConstant(m, "ROBOT_LOGGING_VR_CONTROLLERS", ROBOT_LOGGING_VR_CONTROLLERS); + PyModule_AddIntConstant(m, "STATE_LOGGING_MINITAUR", STATE_LOGGING_MINITAUR); + PyModule_AddIntConstant(m, "STATE_LOGGING_GENERIC_ROBOT", STATE_LOGGING_GENERIC_ROBOT); + PyModule_AddIntConstant(m, "STATE_LOGGING_VR_CONTROLLERS", STATE_LOGGING_VR_CONTROLLERS); SpamError = PyErr_NewException("pybullet.error", NULL, NULL); Py_INCREF(SpamError); From 8af62239b04646779eaa6bcd531fdec0a4cd398e Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 17 Feb 2017 15:15:13 -0800 Subject: [PATCH 3/3] Update pybullet.c --- examples/pybullet/pybullet.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e5400eef6..a33b3afda 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2626,7 +2626,8 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb if (seq) { int len = PySequence_Size(objectUniqueIdsObj); - for (int i=0;i