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

Modify logging and playback to handle fixed joint.

Browse Source
This commit is contained in:
yunfeibai
2017-03-01 23:37:01 -08:00
parent 413d31e77e
commit a33dcdb63d
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();
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);

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