From 7c7d83176787feaaf2f8a49e4553d2e574458297 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Sun, 2 Apr 2017 15:09:40 -0700 Subject: [PATCH 1/5] Add contact point logger. --- .../PhysicsServerCommandProcessor.cpp | 163 ++++++++++++++++++ examples/SharedMemory/SharedMemoryPublic.h | 1 + 2 files changed, 164 insertions(+) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4ed8ea901..0b093eef7 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -950,6 +950,169 @@ struct GenericRobotStateLogger : public InternalStateLogger } }; +struct ContactPointsStateLogger : public InternalStateLogger +{ + int m_loggingTimeStamp; + + std::string m_fileName; + FILE* m_logFileHandle; + std::string m_structTypes; + btMultiBodyDynamicsWorld* m_dynamicsWorld; + bool m_filterLinkA; + bool m_filterLinkB; + int m_linkIndexA; + int m_linkIndexB; + int m_bodyIndexA; + int m_bodyIndexB; + + ContactPointsStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld) + :m_loggingTimeStamp(0), + m_fileName(fileName), + m_logFileHandle(0), + m_dynamicsWorld(dynamicsWorld), + m_filterLinkA(false), + m_filterLinkB(false), + m_linkIndexA(-2), + m_linkIndexB(-2), + m_bodyIndexA(-1), + m_bodyIndexB(-1) + { + m_loggingUniqueId = loggingUniqueId; + m_loggingType = STATE_LOGGING_CONTACT_POINTS; + + btAlignedObjectArray structNames; + structNames.push_back("stepCount"); + structNames.push_back("timeStamp"); + structNames.push_back("contactFlag"); + structNames.push_back("bodyUniqueIdA"); + structNames.push_back("bodyUniqueIdB"); + structNames.push_back("linkIndexA"); + structNames.push_back("linkIndexB"); + structNames.push_back("positionOnAX"); + structNames.push_back("positionOnAY"); + structNames.push_back("positionOnAZ"); + structNames.push_back("positionOnBX"); + structNames.push_back("positionOnBY"); + structNames.push_back("positionOnBZ"); + structNames.push_back("contactNormalOnBX"); + structNames.push_back("contactNormalOnBY"); + structNames.push_back("contactNormalOnBZ"); + structNames.push_back("contactDistance"); + structNames.push_back("normalForce"); + m_structTypes = "IfIIIIIfffffffffff"; + + 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) + { + int numContactManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds(); + for (int i = 0; i < numContactManifolds; i++) + { + const btPersistentManifold* manifold = m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i]; + int linkIndexA = -1; + int linkIndexB = -1; + + int objectIndexB = -1; + + const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1()); + if (bodyB) + { + objectIndexB = bodyB->getUserIndex2(); + } + const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + if (mblB && mblB->m_multiBody) + { + linkIndexB = mblB->m_link; + objectIndexB = mblB->m_multiBody->getUserIndex2(); + if (m_filterLinkB && (m_linkIndexB != linkIndexB)) + { + continue; + } + } + + int objectIndexA = -1; + const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0()); + if (bodyA) + { + objectIndexA = bodyA->getUserIndex2(); + } + const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + if (mblA && mblA->m_multiBody) + { + linkIndexA = mblA->m_link; + objectIndexA = mblA->m_multiBody->getUserIndex2(); + if (m_filterLinkA && (m_linkIndexA != linkIndexA)) + { + continue; + } + } + + btAssert(bodyA || mblA); + + //apply the filter, if the user provides it + if (m_bodyIndexA >= 0) + { + if ((m_bodyIndexA != objectIndexA) && + (m_bodyIndexA != objectIndexB)) + continue; + } + + //apply the second object filter, if the user provides it + if (m_bodyIndexB >= 0) + { + if ((m_bodyIndexB != objectIndexA) && + (m_bodyIndexB != objectIndexB)) + continue; + } + + for (int p = 0; p < manifold->getNumContacts(); p++) + { + MinitaurLogRecord logData; + int stepCount = m_loggingTimeStamp; + float timeStamp = m_loggingTimeStamp*timeStep; + logData.m_values.push_back(stepCount); + logData.m_values.push_back(timeStamp); + + const btManifoldPoint& srcPt = manifold->getContactPoint(p); + + logData.m_values.push_back(0); // reserved contact flag + logData.m_values.push_back(objectIndexA); + logData.m_values.push_back(objectIndexB); + logData.m_values.push_back(linkIndexA); + logData.m_values.push_back(linkIndexB); + logData.m_values.push_back((float)(srcPt.getPositionWorldOnA()[0])); + logData.m_values.push_back((float)(srcPt.getPositionWorldOnA()[1])); + logData.m_values.push_back((float)(srcPt.getPositionWorldOnA()[2])); + logData.m_values.push_back((float)(srcPt.getPositionWorldOnB()[0])); + logData.m_values.push_back((float)(srcPt.getPositionWorldOnB()[1])); + logData.m_values.push_back((float)(srcPt.getPositionWorldOnB()[2])); + logData.m_values.push_back((float)(srcPt.m_normalWorldOnB[0])); + logData.m_values.push_back((float)(srcPt.m_normalWorldOnB[1])); + logData.m_values.push_back((float)(srcPt.m_normalWorldOnB[2])); + logData.m_values.push_back((float)(srcPt.getDistance())); + logData.m_values.push_back((float)(srcPt.getAppliedImpulse() / timeStep)); + + appendMinitaurLogData(m_logFileHandle, m_structTypes, logData); + fflush(m_logFileHandle); + } + } + m_loggingTimeStamp++; + } + } +}; + struct PhysicsServerCommandProcessorInternalData { ///handle management diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7a201aee9..ac8195d06 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -337,6 +337,7 @@ enum b3StateLoggingType STATE_LOGGING_VR_CONTROLLERS = 2, STATE_LOGGING_VIDEO_MP4 = 3, STATE_LOGGING_COMMANDS = 4, + STATE_LOGGING_CONTACT_POINTS = 5, }; From 0d83667817f50093852c053c4cad4665b3e362b9 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Sun, 2 Apr 2017 15:45:48 -0700 Subject: [PATCH 2/5] Add C API to log contact points. --- examples/SharedMemory/PhysicsClientC_API.cpp | 52 +++++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 4 ++ .../PhysicsServerCommandProcessor.cpp | 28 +++++++++- examples/SharedMemory/SharedMemoryCommands.h | 14 +++-- 4 files changed, 94 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b0695cd20..ca7da8509 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2659,6 +2659,58 @@ int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHa return 0; } +int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA) +{ + 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_FILTER_LINK_INDEX_A; + command->m_stateLoggingArguments.m_linkIndexA = linkIndexA; + } + return 0; +} + +int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB) +{ + 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_FILTER_LINK_INDEX_B; + command->m_stateLoggingArguments.m_linkIndexB = linkIndexB; + } + return 0; +} + +int b3StateLoggingSetBodyIndexA(b3SharedMemoryCommandHandle commandHandle, int bodyIndexA) +{ + 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_FILTER_BODY_INDEX_A; + command->m_stateLoggingArguments.m_bodyIndexA = bodyIndexA; + } + return 0; +} + +int b3StateLoggingSetBodyIndexB(b3SharedMemoryCommandHandle commandHandle, int bodyIndexB) +{ + 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_FILTER_BODY_INDEX_B; + command->m_stateLoggingArguments.m_bodyIndexB = bodyIndexB; + } + return 0; +} + int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 35071ae34..29efca7d4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -363,6 +363,10 @@ b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle phys int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName); int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof); +int b3StateLoggingSetLinkIndexA(b3SharedMemoryStatusHandle commandHandle, int linkIndexA); +int b3StateLoggingSetLinkIndexB(b3SharedMemoryStatusHandle commandHandle, int linkIndexB); +int b3StateLoggingSetBodyIndexA(b3SharedMemoryStatusHandle commandHandle, int bodyIndexA); +int b3StateLoggingSetBodyIndexB(b3SharedMemoryStatusHandle commandHandle, int bodyIndexB); int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle); int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0b093eef7..691728a23 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2275,7 +2275,33 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; } - + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_CONTACT_POINTS) + { + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + int loggerUid = m_data->m_stateLoggersUniqueId++; + ContactPointsStateLogger* logger = new ContactPointsStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld); + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_A) && clientCmd.m_stateLoggingArguments.m_linkIndexA >= -1) + { + logger->m_filterLinkA = true; + logger->m_linkIndexA = clientCmd.m_stateLoggingArguments.m_linkIndexA; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_B) && clientCmd.m_stateLoggingArguments.m_linkIndexB >= -1) + { + logger->m_filterLinkB = true; + logger->m_linkIndexB = clientCmd.m_stateLoggingArguments.m_linkIndexB; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_INDEX_A) && clientCmd.m_stateLoggingArguments.m_bodyIndexA > -1) + { + logger->m_bodyIndexA = clientCmd.m_stateLoggingArguments.m_bodyIndexA; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_INDEX_B) && clientCmd.m_stateLoggingArguments.m_bodyIndexB > -1) + { + logger->m_bodyIndexB = clientCmd.m_stateLoggingArguments.m_bodyIndexB; + } + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } if (clientCmd.m_stateLoggingArguments.m_logType ==STATE_LOGGING_VR_CONTROLLERS) { std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index f5038c6a4..f3a54c2e8 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -628,7 +628,11 @@ enum eStateLoggingEnums STATE_LOGGING_START_LOG=1, STATE_LOGGING_STOP_LOG=2, STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID=4, - STATE_LOGGING_MAX_LOG_DOF=8 + STATE_LOGGING_MAX_LOG_DOF=8, + STATE_LOGGING_FILTER_LINK_INDEX_A=16, + STATE_LOGGING_FILTER_LINK_INDEX_B=32, + STATE_LOGGING_FILTER_BODY_INDEX_A=64, + STATE_LOGGING_FILTER_BODY_INDEX_B=128, }; struct VRCameraState @@ -643,11 +647,15 @@ 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_logType;//Minitaur, generic robot, VR states, contact points + int m_numBodyUniqueIds;////only if STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set int m_bodyUniqueIds[MAX_SDF_BODIES]; int m_loggingUniqueId; int m_maxLogDof; + int m_linkIndexA; // only if STATE_LOGGING_FILTER_LINK_INDEX_A flag is set + int m_linkIndexB; // only if STATE_LOGGING_FILTER_LINK_INDEX_B flag is set + int m_bodyIndexA; + int m_bodyIndexB; }; struct StateLoggingResultArgs From 1e91e784691b08c3e0b28612e8d9c3ae039d7a77 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Sun, 2 Apr 2017 16:03:20 -0700 Subject: [PATCH 3/5] Add pybullet API for logging contacts. --- examples/SharedMemory/PhysicsClientC_API.h | 8 ++--- examples/pybullet/examples/kuka_with_cube.py | 3 +- examples/pybullet/pybullet.c | 32 +++++++++++++++++--- 3 files changed, 33 insertions(+), 10 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 29efca7d4..b59029553 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -363,10 +363,10 @@ b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle phys int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName); int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof); -int b3StateLoggingSetLinkIndexA(b3SharedMemoryStatusHandle commandHandle, int linkIndexA); -int b3StateLoggingSetLinkIndexB(b3SharedMemoryStatusHandle commandHandle, int linkIndexB); -int b3StateLoggingSetBodyIndexA(b3SharedMemoryStatusHandle commandHandle, int bodyIndexA); -int b3StateLoggingSetBodyIndexB(b3SharedMemoryStatusHandle commandHandle, int bodyIndexB); +int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA); +int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB); +int b3StateLoggingSetBodyIndexA(b3SharedMemoryCommandHandle commandHandle, int bodyIndexA); +int b3StateLoggingSetBodyIndexB(b3SharedMemoryCommandHandle commandHandle, int bodyIndexB); int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle); int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId); diff --git a/examples/pybullet/examples/kuka_with_cube.py b/examples/pybullet/examples/kuka_with_cube.py index aad32f489..db6ceb247 100644 --- a/examples/pybullet/examples/kuka_with_cube.py +++ b/examples/pybullet/examples/kuka_with_cube.py @@ -47,7 +47,8 @@ p.setRealTimeSimulation(useRealTimeSimulation) #use 0 for no-removal trailDuration = 15 -logId = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2]) +logId1 = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2]) +logId2 = p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS,"LOG0002.txt",bodyIndexA=1) for i in xrange(5): print "Body %d's name is %s." % (i, p.getBodyInfo(i)[1]) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b041f88d8..467bbdcb1 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2678,12 +2678,16 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb char* fileName = 0; PyObject* objectUniqueIdsObj = 0; int maxLogDof = -1; + int bodyIndexA = -1; + int bodyIndexB = -1; + int linkIndexA = -2; + int linkIndexB = -2; - static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "physicsClientId", NULL}; + static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyIndexA", "bodyIndexB", "linkIndexA", "linkIndexB", "physicsClientId", NULL}; int physicsClientId = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oii", kwlist, - &loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiii", kwlist, + &loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &bodyIndexA, &bodyIndexB, &linkIndexA, &linkIndexB, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); @@ -2718,6 +2722,23 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb { b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof); } + + if (bodyIndexA > -1) + { + b3StateLoggingSetLinkIndexA(commandHandle, bodyIndexA); + } + if (bodyIndexB > -1) + { + b3StateLoggingSetBodyIndexB(commandHandle, bodyIndexB); + } + if (linkIndexA > -2) + { + b3StateLoggingSetLinkIndexA(commandHandle, linkIndexA); + } + if (linkIndexB > -2) + { + b3StateLoggingSetLinkIndexB(commandHandle, linkIndexB); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); @@ -5293,8 +5314,8 @@ static PyMethodDef SpamMethods[] = { {"startStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS, "Start logging of state, such as robot base position, orientation, joint positions etc. " - "Specify loggingType (STATE_LOG_MINITAUR, STATE_LOG_GENERIC_ROBOT, STATE_LOG_VR_CONTROLLERS etc), " - "fileName, optional objectUniqueId. Function returns int loggingUniqueId"}, + "Specify loggingType (STATE_LOGGING_MINITAUR, STATE_LOGGING_GENERIC_ROBOT, STATE_LOGGING_VR_CONTROLLERS, STATE_LOGGING_CONTACT_POINTS, etc), " + "fileName, optional objectUniqueId, maxLogDof, bodyIndexA, bodyIndexB, linkIndexA, linkIndexB. Function returns int loggingUniqueId"}, {"stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS, "Stop logging of robot state, given a loggingUniqueId."}, {"rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS, @@ -5425,6 +5446,7 @@ initpybullet(void) PyModule_AddIntConstant(m, "STATE_LOGGING_GENERIC_ROBOT", STATE_LOGGING_GENERIC_ROBOT); PyModule_AddIntConstant(m, "STATE_LOGGING_VR_CONTROLLERS", STATE_LOGGING_VR_CONTROLLERS); PyModule_AddIntConstant(m, "STATE_LOGGING_VIDEO_MP4", STATE_LOGGING_VIDEO_MP4); + PyModule_AddIntConstant(m, "STATE_LOGGING_CONTACT_POINTS", STATE_LOGGING_CONTACT_POINTS); PyModule_AddIntConstant(m, "COV_ENABLE_GUI", COV_ENABLE_GUI); PyModule_AddIntConstant(m, "COV_ENABLE_SHADOWS", COV_ENABLE_SHADOWS); From b168f53ed9fccd2a666cfbe58a1d68615da22134 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 4 Apr 2017 10:38:25 -0700 Subject: [PATCH 4/5] Rename body index to body unique id. --- examples/SharedMemory/PhysicsClientC_API.cpp | 12 ++++---- examples/SharedMemory/PhysicsClientC_API.h | 4 +-- .../PhysicsServerCommandProcessor.cpp | 28 +++++++++---------- examples/SharedMemory/SharedMemoryCommands.h | 8 +++--- examples/pybullet/examples/kuka_with_cube.py | 2 +- examples/pybullet/pybullet.c | 16 +++++------ 6 files changed, 35 insertions(+), 35 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ca7da8509..ac5258f2f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2685,28 +2685,28 @@ int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int l return 0; } -int b3StateLoggingSetBodyIndexA(b3SharedMemoryCommandHandle commandHandle, int bodyIndexA) +int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId) { 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_FILTER_BODY_INDEX_A; - command->m_stateLoggingArguments.m_bodyIndexA = bodyIndexA; + command->m_updateFlags |= STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A; + command->m_stateLoggingArguments.m_bodyUniqueIdA = bodyAUniqueId; } return 0; } -int b3StateLoggingSetBodyIndexB(b3SharedMemoryCommandHandle commandHandle, int bodyIndexB) +int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId) { 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_FILTER_BODY_INDEX_B; - command->m_stateLoggingArguments.m_bodyIndexB = bodyIndexB; + command->m_updateFlags |= STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B; + command->m_stateLoggingArguments.m_bodyUniqueIdB = bodyBUniqueId; } return 0; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b59029553..202240564 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -365,8 +365,8 @@ int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHa int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof); int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA); int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB); -int b3StateLoggingSetBodyIndexA(b3SharedMemoryCommandHandle commandHandle, int bodyIndexA); -int b3StateLoggingSetBodyIndexB(b3SharedMemoryCommandHandle commandHandle, int bodyIndexB); +int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId); +int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId); int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle); int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 691728a23..318c3c630 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -962,8 +962,8 @@ struct ContactPointsStateLogger : public InternalStateLogger bool m_filterLinkB; int m_linkIndexA; int m_linkIndexB; - int m_bodyIndexA; - int m_bodyIndexB; + int m_bodyUniqueIdA; + int m_bodyUniqueIdB; ContactPointsStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld) :m_loggingTimeStamp(0), @@ -974,8 +974,8 @@ struct ContactPointsStateLogger : public InternalStateLogger m_filterLinkB(false), m_linkIndexA(-2), m_linkIndexB(-2), - m_bodyIndexA(-1), - m_bodyIndexB(-1) + m_bodyUniqueIdA(-1), + m_bodyUniqueIdB(-1) { m_loggingUniqueId = loggingUniqueId; m_loggingType = STATE_LOGGING_CONTACT_POINTS; @@ -1062,18 +1062,18 @@ struct ContactPointsStateLogger : public InternalStateLogger btAssert(bodyA || mblA); //apply the filter, if the user provides it - if (m_bodyIndexA >= 0) + if (m_bodyUniqueIdA >= 0) { - if ((m_bodyIndexA != objectIndexA) && - (m_bodyIndexA != objectIndexB)) + if ((m_bodyUniqueIdA != objectIndexA) && + (m_bodyUniqueIdA != objectIndexB)) continue; } //apply the second object filter, if the user provides it - if (m_bodyIndexB >= 0) + if (m_bodyUniqueIdB >= 0) { - if ((m_bodyIndexB != objectIndexA) && - (m_bodyIndexB != objectIndexB)) + if ((m_bodyUniqueIdB != objectIndexA) && + (m_bodyUniqueIdB != objectIndexB)) continue; } @@ -2290,13 +2290,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm logger->m_filterLinkB = true; logger->m_linkIndexB = clientCmd.m_stateLoggingArguments.m_linkIndexB; } - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_INDEX_A) && clientCmd.m_stateLoggingArguments.m_bodyIndexA > -1) + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA > -1) { - logger->m_bodyIndexA = clientCmd.m_stateLoggingArguments.m_bodyIndexA; + logger->m_bodyUniqueIdA = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA; } - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_INDEX_B) && clientCmd.m_stateLoggingArguments.m_bodyIndexB > -1) + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB > -1) { - logger->m_bodyIndexB = clientCmd.m_stateLoggingArguments.m_bodyIndexB; + logger->m_bodyUniqueIdB = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB; } m_data->m_stateLoggers.push_back(logger); serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index f3a54c2e8..f91426eb7 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -631,8 +631,8 @@ enum eStateLoggingEnums STATE_LOGGING_MAX_LOG_DOF=8, STATE_LOGGING_FILTER_LINK_INDEX_A=16, STATE_LOGGING_FILTER_LINK_INDEX_B=32, - STATE_LOGGING_FILTER_BODY_INDEX_A=64, - STATE_LOGGING_FILTER_BODY_INDEX_B=128, + STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A=64, + STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B=128, }; struct VRCameraState @@ -654,8 +654,8 @@ struct StateLoggingRequest int m_maxLogDof; int m_linkIndexA; // only if STATE_LOGGING_FILTER_LINK_INDEX_A flag is set int m_linkIndexB; // only if STATE_LOGGING_FILTER_LINK_INDEX_B flag is set - int m_bodyIndexA; - int m_bodyIndexB; + int m_bodyUniqueIdA; // only if STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A flag is set + int m_bodyUniqueIdB; // only if STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B flag is set }; struct StateLoggingResultArgs diff --git a/examples/pybullet/examples/kuka_with_cube.py b/examples/pybullet/examples/kuka_with_cube.py index db6ceb247..a4668dd49 100644 --- a/examples/pybullet/examples/kuka_with_cube.py +++ b/examples/pybullet/examples/kuka_with_cube.py @@ -48,7 +48,7 @@ p.setRealTimeSimulation(useRealTimeSimulation) trailDuration = 15 logId1 = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2]) -logId2 = p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS,"LOG0002.txt",bodyIndexA=1) +logId2 = p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS,"LOG0002.txt",bodyUniqueIdA=2) for i in xrange(5): print "Body %d's name is %s." % (i, p.getBodyInfo(i)[1]) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 467bbdcb1..9cb36b849 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2678,16 +2678,16 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb char* fileName = 0; PyObject* objectUniqueIdsObj = 0; int maxLogDof = -1; - int bodyIndexA = -1; - int bodyIndexB = -1; + int bodyUniqueIdA = -1; + int bodyUniqueIdB = -1; int linkIndexA = -2; int linkIndexB = -2; - static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyIndexA", "bodyIndexB", "linkIndexA", "linkIndexB", "physicsClientId", NULL}; + static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyUniqueIdA", "bodyUniqueIdB", "linkIndexA", "linkIndexB", "physicsClientId", NULL}; int physicsClientId = 0; if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiii", kwlist, - &loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &bodyIndexA, &bodyIndexB, &linkIndexA, &linkIndexB, &physicsClientId)) + &loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &bodyUniqueIdA, &bodyUniqueIdB, &linkIndexA, &linkIndexB, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); @@ -2723,13 +2723,13 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof); } - if (bodyIndexA > -1) + if (bodyUniqueIdA > -1) { - b3StateLoggingSetLinkIndexA(commandHandle, bodyIndexA); + b3StateLoggingSetBodyAUniqueId(commandHandle, bodyUniqueIdA); } - if (bodyIndexB > -1) + if (bodyUniqueIdB > -1) { - b3StateLoggingSetBodyIndexB(commandHandle, bodyIndexB); + b3StateLoggingSetBodyBUniqueId(commandHandle, bodyUniqueIdB); } if (linkIndexA > -2) { From fedf718d9e2aecccffef80b29ba989397babc488 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 4 Apr 2017 13:45:49 -0700 Subject: [PATCH 5/5] minor fix for pybullet comment. --- examples/pybullet/pybullet.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 9cb36b849..7eb6a8cd1 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -5315,7 +5315,7 @@ static PyMethodDef SpamMethods[] = { {"startStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS, "Start logging of state, such as robot base position, orientation, joint positions etc. " "Specify loggingType (STATE_LOGGING_MINITAUR, STATE_LOGGING_GENERIC_ROBOT, STATE_LOGGING_VR_CONTROLLERS, STATE_LOGGING_CONTACT_POINTS, etc), " - "fileName, optional objectUniqueId, maxLogDof, bodyIndexA, bodyIndexB, linkIndexA, linkIndexB. Function returns int loggingUniqueId"}, + "fileName, optional objectUniqueId, maxLogDof, bodyUniqueIdA, bodyUniqueIdB, linkIndexA, linkIndexB. Function returns int loggingUniqueId"}, {"stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS, "Stop logging of robot state, given a loggingUniqueId."}, {"rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS,