85 lines
1.9 KiB
Python
85 lines
1.9 KiB
Python
import pybullet as p
|
|
import struct
|
|
|
|
|
|
def readLogFile(filename, verbose=True):
|
|
f = open(filename, 'rb')
|
|
|
|
print('Opened'),
|
|
print(filename)
|
|
|
|
keys = f.readline().decode('utf8').rstrip('\n').split(',')
|
|
fmt = f.readline().decode('utf8').rstrip('\n')
|
|
|
|
# The byte number of one record
|
|
sz = struct.calcsize(fmt)
|
|
# The type number of one record
|
|
ncols = len(fmt)
|
|
|
|
if verbose:
|
|
print('Keys:'),
|
|
print(keys)
|
|
print('Format:'),
|
|
print(fmt)
|
|
print('Size:'),
|
|
print(sz)
|
|
print('Columns:'),
|
|
print(ncols)
|
|
|
|
# Read data
|
|
wholeFile = f.read()
|
|
# split by alignment word
|
|
chunks = wholeFile.split(b'\xaa\xbb')
|
|
log = list()
|
|
for chunk in chunks:
|
|
if len(chunk) == sz:
|
|
values = struct.unpack(fmt, chunk)
|
|
record = list()
|
|
for i in range(ncols):
|
|
record.append(values[i])
|
|
log.append(record)
|
|
|
|
return log
|
|
|
|
|
|
#clid = p.connect(p.SHARED_MEMORY)
|
|
p.connect(p.GUI)
|
|
p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf")
|
|
p.loadURDF("tray/tray.urdf", [0, 0, 0])
|
|
p.loadURDF("block.urdf", [0, 0, 2])
|
|
|
|
log = readLogFile("data/block_grasp_log.bin")
|
|
|
|
recordNum = len(log)
|
|
itemNum = len(log[0])
|
|
objectNum = p.getNumBodies()
|
|
|
|
print('record num:'),
|
|
print(recordNum)
|
|
print('item num:'),
|
|
print(itemNum)
|
|
|
|
|
|
def Step(stepIndex):
|
|
for objectId in range(objectNum):
|
|
record = log[stepIndex * objectNum + objectId]
|
|
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 + 17])
|
|
|
|
|
|
stepIndexId = p.addUserDebugParameter("stepIndex", 0, recordNum / objectNum - 1, 0)
|
|
|
|
while True:
|
|
stepIndex = int(p.readUserDebugParameter(stepIndexId))
|
|
Step(stepIndex)
|
|
p.stepSimulation()
|
|
Step(stepIndex)
|