Merge pull request #1053 from YunfeiBai/master

Add logging for contact information.

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;

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);

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<std::string> 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;

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_logType;//Minitaur, generic robot, VR states, contact points
-	int m_numBodyUniqueIds;////only if ROBOT_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set
+	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

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,
 };
 
 

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])

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,
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiii", kwlist,
-									 &loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &physicsClientId))
+									 &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), "
+	 "Specify loggingType (STATE_LOGGING_MINITAUR, STATE_LOGGING_GENERIC_ROBOT, STATE_LOGGING_VR_CONTROLLERS, STATE_LOGGING_CONTACT_POINTS, etc), "
-	 "fileName, optional objectUniqueId. 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,
@@ -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);