From 6e3cd26f11f5751eea4953acf5e63d2e9c9d5399 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Sat, 4 Mar 2017 13:19:43 -0800 Subject: [PATCH] Log time stamp and step count. --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 8 ++++++-- examples/pybullet/kuka_with_cube_playback.py | 8 ++++---- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 312691872..94d226be4 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -578,6 +578,7 @@ struct GenericRobotStateLogger : public InternalStateLogger m_loggingType = STATE_LOGGING_GENERIC_ROBOT; btAlignedObjectArray structNames; + structNames.push_back("stepCount"); structNames.push_back("timeStamp"); structNames.push_back("objectId"); structNames.push_back("posX"); @@ -619,7 +620,7 @@ struct GenericRobotStateLogger : public InternalStateLogger structNames.push_back("u10"); structNames.push_back("u11"); - m_structTypes = "fIfffffffffffffIffffffffffffffffffffffff"; + m_structTypes = "IfIfffffffffffffIffffffffffffffffffffffff"; const char* fileNameC = fileName.c_str(); m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes); @@ -647,7 +648,10 @@ struct GenericRobotStateLogger : public InternalStateLogger } MinitaurLogRecord logData; - logData.m_values.push_back(m_loggingTimeStamp); + int stepCount = m_loggingTimeStamp; + float timeStamp = m_loggingTimeStamp*m_dynamicsWorld->getSolverInfo().m_timeStep; + logData.m_values.push_back(stepCount); + logData.m_values.push_back(timeStamp); btVector3 pos = mb->getBasePos(); btQuaternion ori = mb->getWorldToBaseRot().inverse(); diff --git a/examples/pybullet/kuka_with_cube_playback.py b/examples/pybullet/kuka_with_cube_playback.py index c3a5ad445..64b9a52c7 100644 --- a/examples/pybullet/kuka_with_cube_playback.py +++ b/examples/pybullet/kuka_with_cube_playback.py @@ -67,14 +67,14 @@ print('item num:'), print(itemNum) for record in log: - Id = record[1] - pos = [record[2],record[3],record[4]] - orn = [record[5],record[6],record[7],record[8]] + Id = record[2] + pos = [record[3],record[4],record[5]] + orn = [record[6],record[7],record[8],record[9]] p.resetBasePositionAndOrientation(Id,pos,orn) numJoints = p.getNumJoints(Id) for i in range (numJoints): jointInfo = p.getJointInfo(Id,i) qIndex = jointInfo[3] if qIndex > -1: - p.resetJointState(Id,i,record[qIndex-7+16]) + p.resetJointState(Id,i,record[qIndex-7+17]) sleep(0.0005) \ No newline at end of file