From a33dcdb63d4c4b2fcabad34a5286d476457aa515 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 1 Mar 2017 23:37:01 -0800 Subject: [PATCH] Modify logging and playback to handle fixed joint. --- .../PhysicsServerCommandProcessor.cpp | 31 ++++++++++--------- examples/pybullet/kuka_with_cube_playback.py | 7 +++-- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b78b3ba1b..6d85bd7cc 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -684,35 +684,36 @@ struct GenericRobotStateLogger : public InternalStateLogger int numDofs = mb->getNumDofs(); logData.m_values.push_back(numDofs); + int numJoints = mb->getNumLinks(); - for (int j = 0; j < 12; ++j) + for (int j = 0; j < numJoints; ++j) { - if (j < numDofs) + if (mb->getLink(j).m_jointType == 0 || mb->getLink(j).m_jointType == 1) { float q = mb->getJointPos(j); logData.m_values.push_back(q); } - else - { - float q = 0.0; - logData.m_values.push_back(q); - } + } + for (int j = numDofs; j < 12; ++j) + { + float q = 0.0; + logData.m_values.push_back(q); } - for (int j = 0; j < 12; ++j) + for (int j = 0; j < numJoints; ++j) { - if (j < numDofs) + if (mb->getLink(j).m_jointType == 0 || mb->getLink(j).m_jointType == 1) { float u = mb->getJointVel(j); logData.m_values.push_back(u); } - else - { - float u = 0.0; - logData.m_values.push_back(u); - } } - + for (int j = numDofs; j < 12; ++j) + { + float u = 0.0; + logData.m_values.push_back(u); + } + //at the moment, appendMinitaurLogData will directly write to disk (potential delay) //better to fill a huge memory buffer and once in a while write it to disk appendMinitaurLogData(m_logFileHandle, m_structTypes, logData); diff --git a/examples/pybullet/kuka_with_cube_playback.py b/examples/pybullet/kuka_with_cube_playback.py index 5b3aade9b..c3a5ad445 100644 --- a/examples/pybullet/kuka_with_cube_playback.py +++ b/examples/pybullet/kuka_with_cube_playback.py @@ -71,7 +71,10 @@ for record in log: pos = [record[2],record[3],record[4]] orn = [record[5],record[6],record[7],record[8]] p.resetBasePositionAndOrientation(Id,pos,orn) - numJoints = record[15] + numJoints = p.getNumJoints(Id) for i in range (numJoints): - p.resetJointState(Id,i,record[i+16]) + jointInfo = p.getJointInfo(Id,i) + qIndex = jointInfo[3] + if qIndex > -1: + p.resetJointState(Id,i,record[qIndex-7+16]) sleep(0.0005) \ No newline at end of file