Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1053 from YunfeiBai/master

Browse Source 
Add logging for contact information.
This commit is contained in:
erwincoumans
2017-04-04 23:34:33 +00:00
committed by GitHub
parent 0c6b0d52c6 fedf718d9e
commit 814e901a20
7 changed files with 287 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
examples/SharedMemory/PhysicsClientC_API.cpp
View File

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

4
examples/SharedMemory/PhysicsClientC_API.h
View File

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

191
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

14
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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

1
examples/SharedMemory/SharedMemoryPublic.h
View File

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

3
examples/pybullet/examples/kuka_with_cube.py
View File

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

32
examples/pybullet/pybullet.c
View File

@@ -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);
@@ -2719,6 +2723,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);
if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
@@ -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);