diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b0695cd20..ac5258f2f 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 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_UNIQUE_ID_A; + command->m_stateLoggingArguments.m_bodyUniqueIdA = bodyAUniqueId; + } + return 0; +} + +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_UNIQUE_ID_B; + command->m_stateLoggingArguments.m_bodyUniqueIdB = bodyBUniqueId; + } + 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..202240564 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(b3SharedMemoryCommandHandle commandHandle, int linkIndexA); +int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB); +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 4ed8ea901..318c3c630 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_bodyUniqueIdA; + int m_bodyUniqueIdB; + + 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_bodyUniqueIdA(-1), + m_bodyUniqueIdB(-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_bodyUniqueIdA >= 0) + { + if ((m_bodyUniqueIdA != objectIndexA) && + (m_bodyUniqueIdA != objectIndexB)) + continue; + } + + //apply the second object filter, if the user provides it + if (m_bodyUniqueIdB >= 0) + { + if ((m_bodyUniqueIdB != objectIndexA) && + (m_bodyUniqueIdB != 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 @@ -2112,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_UNIQUE_ID_A) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA > -1) + { + logger->m_bodyUniqueIdA = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB > -1) + { + logger->m_bodyUniqueIdB = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB; + } + 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..f91426eb7 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_UNIQUE_ID_A=64, + STATE_LOGGING_FILTER_BODY_UNIQUE_ID_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_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/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, }; diff --git a/examples/pybullet/examples/kuka_with_cube.py b/examples/pybullet/examples/kuka_with_cube.py index aad32f489..a4668dd49 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",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 b041f88d8..7eb6a8cd1 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 bodyUniqueIdA = -1; + int bodyUniqueIdB = -1; + int linkIndexA = -2; + int linkIndexB = -2; - static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "physicsClientId", NULL}; + static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyUniqueIdA", "bodyUniqueIdB", "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, &bodyUniqueIdA, &bodyUniqueIdB, &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 (bodyUniqueIdA > -1) + { + b3StateLoggingSetBodyAUniqueId(commandHandle, bodyUniqueIdA); + } + if (bodyUniqueIdB > -1) + { + b3StateLoggingSetBodyBUniqueId(commandHandle, bodyUniqueIdB); + } + 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, 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, @@ -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);