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

Merge pull request #980 from YunfeiBai/master

Browse Source 
Modify logging and playback to handle fixed joint.
This commit is contained in:
erwincoumans
2017-03-02 08:17:59 -08:00
committed by GitHub
parent 413d31e77e a33dcdb63d
commit 1ba22c7c45
2 changed files with 21 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -684,35 +684,36 @@ struct GenericRobotStateLogger : public InternalStateLogger
int numDofs = mb->getNumDofs(); int numDofs = mb->getNumDofs();
logData.m_values.push_back(numDofs); 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); float q = mb->getJointPos(j);
logData.m_values.push_back(q); logData.m_values.push_back(q);
} }
else }
{ for (int j = numDofs; j < 12; ++j)
float q = 0.0; {
logData.m_values.push_back(q); 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); float u = mb->getJointVel(j);
logData.m_values.push_back(u); 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) //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 //better to fill a huge memory buffer and once in a while write it to disk
appendMinitaurLogData(m_logFileHandle, m_structTypes, logData); appendMinitaurLogData(m_logFileHandle, m_structTypes, logData);

7
examples/pybullet/kuka_with_cube_playback.py
View File

@@ -71,7 +71,10 @@ for record in log:
pos = [record[2],record[3],record[4]] pos = [record[2],record[3],record[4]]
orn = [record[5],record[6],record[7],record[8]] orn = [record[5],record[6],record[7],record[8]]
p.resetBasePositionAndOrientation(Id,pos,orn) p.resetBasePositionAndOrientation(Id,pos,orn)
numJoints = record[15] numJoints = p.getNumJoints(Id)
for i in range (numJoints): 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) sleep(0.0005)