diff --git a/examples/pybullet/examples/kuka_grasp_block_playback.py b/examples/pybullet/examples/kuka_grasp_block_playback.py new file mode 100644 index 000000000..d22335975 --- /dev/null +++ b/examples/pybullet/examples/kuka_grasp_block_playback.py @@ -0,0 +1,91 @@ +import pybullet as p +import time +import math +from datetime import datetime +from numpy import * +from pylab import * +import struct +import sys +import os, fnmatch +import argparse +from time import sleep + +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) +