mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-05 15:21:06 +00:00
80 lines
1.8 KiB
Python
80 lines
1.8 KiB
Python
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().rstrip('\n').split(',')
|
|
fmt = f.readline().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('\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.loadURDF("plane.urdf",[0,0,-0.3])
|
|
p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
|
|
p.loadURDF("cube.urdf",[2,2,5])
|
|
p.loadURDF("cube.urdf",[-2,-2,5])
|
|
p.loadURDF("cube.urdf",[2,-2,5])
|
|
|
|
log = readLogFile("LOG0001.txt")
|
|
|
|
recordNum = len(log)
|
|
itemNum = len(log[0])
|
|
print('record num:'),
|
|
print(recordNum)
|
|
print('item num:'),
|
|
print(itemNum)
|
|
|
|
for record in log:
|
|
Id = record[1]
|
|
pos = [record[2],record[3],record[4]]
|
|
orn = [record[5],record[6],record[7],record[8]]
|
|
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+16])
|
|
sleep(0.0005) |