Merge remote-tracking branch 'bp/master'

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

examples/pybullet/examples/reset_dynamic_info.py

@@ -6,15 +6,15 @@ p.connect(p.GUI)
 planeId = p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593])
 p.loadURDF(fileName="cube.urdf",baseOrientation=[0.25882,0,0,0.96593],basePosition=[0,0,2])
 cubeId = p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4])
-p.changeDynamicsInfo(bodyUniqueId=2,linkIndex=-1,mass=0.1)
-#p.changeDynamicsInfo(bodyUniqueId=2,linkIndex=-1,mass=100.0)
+#p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=0.1)
+p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=100.0)
 p.setGravity(0,0,-10)
 p.setRealTimeSimulation(0)
 t=0
 while 1:
 	t=t+1
 	if t > 400:
-		p.changeDynamicsInfo(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01)
+		p.changeDynamics(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01)
 	mass1,frictionCoeff1=p.getDynamicsInfo(bodyUniqueId=planeId,linkIndex=-1)
 	mass2,frictionCoeff2=p.getDynamicsInfo(bodyUniqueId=cubeId,linkIndex=-1)
 	print mass1,frictionCoeff1