move pybullet examples to Bullet/examples/pybullet/examples

2017-03-29 09:40:56 -07:00
parent 0750d502a8
commit eb8c31ae82
29 changed files with 0 additions and 0 deletions
--- a/examples/pybullet/examples/constraint.py
+++ b/examples/pybullet/examples/constraint.py
@@ -0,0 +1,24 @@
+import pybullet as p
+import time
+import math
+
+p.connect(p.GUI)
+
+p.loadURDF("plane.urdf")
+cubeId = p.loadURDF("cube_small.urdf",0,0,1)
+p.setGravity(0,0,-10)
+p.setRealTimeSimulation(1)
+cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1])
+prev=[0,0,1]
+a=-math.pi
+while 1:
+	a=a+0.01
+	if (a>math.pi):
+		a=-math.pi
+	time.sleep(.01)
+	p.setGravity(0,0,-10)
+	pivot=[a,0,1]
+	orn = p.getQuaternionFromEuler([a,0,0])
+	p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50)
+
+p.removeConstraint(cid)
--- a/examples/pybullet/examples/forcetorquesensor.py
+++ b/examples/pybullet/examples/forcetorquesensor.py
@@ -0,0 +1,26 @@
+import pybullet as p
+p.connect(p.DIRECT)
+hinge = p.loadURDF("hinge.urdf")
+print("mass of linkA = 1kg, linkB = 1kg, total mass = 2kg")
+
+hingeJointIndex = 0
+
+#by default, joint motors are enabled, maintaining zero velocity
+p.setJointMotorControl2(hinge,hingeJointIndex,p.VELOCITY_CONTROL,0,0,0)
+
+p.setGravity(0,0,-10)
+p.stepSimulation()
+print("joint state without sensor")
+
+print(p.getJointState(0,0))
+p.enableJointForceTorqueSensor(hinge,hingeJointIndex)
+p.stepSimulation()
+print("joint state with force/torque sensor, gravity [0,0,-10]")
+print(p.getJointState(0,0))
+p.setGravity(0,0,0)
+p.stepSimulation()
+print("joint state with force/torque sensor, no gravity")
+print(p.getJointState(0,0))
+
+p.disconnect()
+
--- a/examples/pybullet/examples/hand.ino
+++ b/examples/pybullet/examples/hand.ino
@@ -0,0 +1,32 @@
+//arduino script for vr glove, sending analogue 'finger' readings
+//to be used with pybullet and hand.py
+
+
+int sensorPin0 = A0;
+int sensorPin1 = A1;
+int sensorPin2 = A2;
+int sensorPin3 = A3;
+
+void setup() {
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  int sensorValue0 = analogRead(sensorPin0);
+  int sensorValue1 = analogRead(sensorPin1);
+  int sensorValue2 = analogRead(sensorPin2);
+  int sensorValue3 = analogRead(sensorPin3);
+
+  Serial.print(",");
+  Serial.print(sensorValue0);
+  Serial.print(",");
+  Serial.print(sensorValue1);
+  Serial.print(",");
+  Serial.print(sensorValue2);
+  Serial.print(",");
+  Serial.print(sensorValue3);  
+  Serial.println(",");
+  delay(10);
+}
--- a/examples/pybullet/examples/hand.py
+++ b/examples/pybullet/examples/hand.py
@@ -0,0 +1,78 @@
+#script to control a simulated robot hand using a VR glove
+#see https://twitter.com/erwincoumans/status/821953216271106048
+#and https://www.youtube.com/watch?v=I6s37aBXbV8
+#vr glove was custom build using Spectra Symbolflex sensors (4.5")
+#inside a Under Armour Batting Glove, using DFRobot Bluno BLE/Beetle
+#with BLE Link to receive serial (for wireless bluetooth serial)
+
+import serial
+import time
+import pybullet as p
+
+#first try to connect to shared memory (VR), if it fails use local GUI
+c = p.connect(p.SHARED_MEMORY)
+print(c)
+if (c<0):
+		p.connect(p.GUI)
+		
+#load the MuJoCo MJCF hand
+objects = p.loadMJCF("MPL/MPL.xml")
+
+hand=objects[0]
+#clamp in range 400-600
+minV = 400
+maxV = 600
+
+p.setRealTimeSimulation(1)
+
+def convertSensor(x):
+	v = minV
+	try:
+		v = float(x)
+	except ValueError:
+		v = minV    
+	if (v<minV):
+		v=minV
+	if (v>maxV):
+		v=maxV
+	b = (v-minV)/float(maxV-minV)
+	return (1.0-b)
+	
+ser = serial.Serial(port='COM13',baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
+if (ser.isOpen()):
+	while True:
+		while ser.inWaiting() > 0:
+			line = str(ser.readline())
+			words = line.split(",")
+			if (len(words)==6):
+				middle = convertSensor(words[1])
+				pink = convertSensor(words[2])
+				index = convertSensor(words[3])
+				thumb = convertSensor(words[4])
+
+				p.setJointMotorControl2(hand,7,p.POSITION_CONTROL,thumb)
+				p.setJointMotorControl2(hand,9,p.POSITION_CONTROL,thumb)
+				p.setJointMotorControl2(hand,11,p.POSITION_CONTROL,thumb)
+				p.setJointMotorControl2(hand,13,p.POSITION_CONTROL,thumb)
+				
+				p.setJointMotorControl2(hand,17,p.POSITION_CONTROL,index)
+				p.setJointMotorControl2(hand,19,p.POSITION_CONTROL,index)
+				p.setJointMotorControl2(hand,21,p.POSITION_CONTROL,index)
+
+				p.setJointMotorControl2(hand,24,p.POSITION_CONTROL,middle)
+				p.setJointMotorControl2(hand,26,p.POSITION_CONTROL,middle)
+				p.setJointMotorControl2(hand,28,p.POSITION_CONTROL,middle)
+				
+				p.setJointMotorControl2(hand,40,p.POSITION_CONTROL,pink)
+				p.setJointMotorControl2(hand,42,p.POSITION_CONTROL,pink)
+				p.setJointMotorControl2(hand,44,p.POSITION_CONTROL,pink)
+
+				ringpos = 0.5*(pink+middle)
+				p.setJointMotorControl2(hand,32,p.POSITION_CONTROL,ringpos)
+				p.setJointMotorControl2(hand,34,p.POSITION_CONTROL,ringpos)
+				p.setJointMotorControl2(hand,36,p.POSITION_CONTROL,ringpos)
+								
+				#print(middle)
+				#print(pink)
+				#print(index)
+				#print(thumb)
--- a/examples/pybullet/examples/hello_pybullet.py
+++ b/examples/pybullet/examples/hello_pybullet.py
@@ -0,0 +1,23 @@
+import pybullet as p
+from time import sleep
+
+physicsClient = p.connect(p.GUI)
+
+p.setGravity(0,0,-10)
+planeId = p.loadURDF("plane.urdf")
+cubeStartPos = [0,0,1]
+cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
+boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
+cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
+
+useRealTimeSimulation = 0
+
+if (useRealTimeSimulation):
+	p.setRealTimeSimulation(1)
+
+while 1:
+	if (useRealTimeSimulation):
+		p.setGravity(0,0,-10)
+		sleep(0.01) # Time in seconds.
+	else:
+		p.stepSimulation()
--- a/examples/pybullet/examples/inverse_kinematics.py
+++ b/examples/pybullet/examples/inverse_kinematics.py
@@ -0,0 +1,87 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+
+#clid = p.connect(p.SHARED_MEMORY)
+p.connect(p.GUI)
+p.loadURDF("plane.urdf",[0,0,-0.3])
+kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
+p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
+kukaEndEffectorIndex = 6
+numJoints = p.getNumJoints(kukaId)
+if (numJoints!=7):
+	exit()
+	
+
+#lower limits for null space
+ll=[-.967,-2	,-2.96,0.19,-2.96,-2.09,-3.05]
+#upper limits for null space
+ul=[.967,2	,2.96,2.29,2.96,2.09,3.05]
+#joint ranges for null space
+jr=[5.8,4,5.8,4,5.8,4,6]
+#restposes for null space
+rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0]
+#joint damping coefficents
+jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1]
+
+for i in range (numJoints):
+	p.resetJointState(kukaId,i,rp[i])
+
+p.setGravity(0,0,0)
+t=0.
+prevPose=[0,0,0]
+prevPose1=[0,0,0]
+hasPrevPose = 0
+useNullSpace = 0
+
+useOrientation = 1
+useSimulation = 1
+useRealTimeSimulation = 1
+p.setRealTimeSimulation(useRealTimeSimulation)
+#trailDuration is duration (in seconds) after debug lines will be removed automatically
+#use 0 for no-removal
+trailDuration = 15
+
+	
+while 1:
+	if (useRealTimeSimulation):
+		dt = datetime.now()
+		t = (dt.second/60.)*2.*math.pi
+	else:
+		t=t+0.1
+	
+	if (useSimulation and useRealTimeSimulation==0):
+		p.stepSimulation()
+	
+	for i in range (1):
+		pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)]
+		#end effector points down, not up (in case useOrientation==1)
+		orn = p.getQuaternionFromEuler([0,-math.pi,0])
+		
+		if (useNullSpace==1):
+			if (useOrientation==1):
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp)
+			else:
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp)
+		else:
+			if (useOrientation==1):
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
+			else:
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos)
+	
+		if (useSimulation):
+			for i in range (numJoints):
+				p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
+		else:
+			#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
+			for i in range (numJoints):
+				p.resetJointState(kukaId,i,jointPoses[i])
+
+	ls = p.getLinkState(kukaId,kukaEndEffectorIndex)	
+	if (hasPrevPose):
+		p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration)
+		p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration)
+	prevPose=pos
+	prevPose1=ls[4]
+	hasPrevPose = 1		
--- a/examples/pybullet/examples/kuka_with_cube.py
+++ b/examples/pybullet/examples/kuka_with_cube.py
@@ -0,0 +1,92 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+
+#clid = p.connect(p.SHARED_MEMORY)
+p.connect(p.GUI)
+p.loadURDF("plane.urdf",[0,0,-0.3],useFixedBase=True)
+kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0],useFixedBase=True)
+p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
+kukaEndEffectorIndex = 6
+numJoints = p.getNumJoints(kukaId)
+if (numJoints!=7):
+	exit()
+
+p.loadURDF("cube.urdf",[2,2,5])
+p.loadURDF("cube.urdf",[-2,-2,5])
+p.loadURDF("cube.urdf",[2,-2,5])
+
+#lower limits for null space
+ll=[-.967,-2	,-2.96,0.19,-2.96,-2.09,-3.05]
+#upper limits for null space
+ul=[.967,2	,2.96,2.29,2.96,2.09,3.05]
+#joint ranges for null space
+jr=[5.8,4,5.8,4,5.8,4,6]
+#restposes for null space
+rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0]
+#joint damping coefficents
+jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1]
+
+for i in range (numJoints):
+	p.resetJointState(kukaId,i,rp[i])
+
+p.setGravity(0,0,-10)
+t=0.
+prevPose=[0,0,0]
+prevPose1=[0,0,0]
+hasPrevPose = 0
+useNullSpace = 0
+
+count = 0
+useOrientation = 1
+useSimulation = 1
+useRealTimeSimulation = 1
+p.setRealTimeSimulation(useRealTimeSimulation)
+#trailDuration is duration (in seconds) after debug lines will be removed automatically
+#use 0 for no-removal
+trailDuration = 15
+
+logId = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2])
+	
+while 1:
+	if (useRealTimeSimulation):
+		dt = datetime.now()
+		t = (dt.second/60.)*2.*math.pi
+	else:
+		t=t+0.1
+	
+	if (useSimulation and useRealTimeSimulation==0):
+		p.stepSimulation()
+	
+	for i in range (1):
+		pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)]
+		#end effector points down, not up (in case useOrientation==1)
+		orn = p.getQuaternionFromEuler([0,-math.pi,0])
+		
+		if (useNullSpace==1):
+			if (useOrientation==1):
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp)
+			else:
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp)
+		else:
+			if (useOrientation==1):
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
+			else:
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos)
+	
+		if (useSimulation):
+			for i in range (numJoints):
+				p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
+		else:
+			#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
+			for i in range (numJoints):
+				p.resetJointState(kukaId,i,jointPoses[i])
+
+	ls = p.getLinkState(kukaId,kukaEndEffectorIndex)	
+	if (hasPrevPose):
+		p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration)
+		p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration)
+	prevPose=pos
+	prevPose1=ls[4]
+	hasPrevPose = 1
--- a/examples/pybullet/examples/kuka_with_cube_playback.py
+++ b/examples/pybullet/examples/kuka_with_cube_playback.py
@@ -0,0 +1,80 @@
+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.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[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])
+    sleep(0.0005)
--- a/examples/pybullet/examples/logMinitaur.py
+++ b/examples/pybullet/examples/logMinitaur.py
@@ -0,0 +1,15 @@
+import pybullet as p
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0) :
+	p.connect(p.GUI)
+p.loadURDF("plane.urdf")
+
+quadruped = p.loadURDF("quadruped/quadruped.urdf")
+logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"LOG00048.TXT",[quadruped])
+p.stepSimulation()
+p.stepSimulation()
+p.stepSimulation()
+p.stepSimulation()
+p.stepSimulation()
+
+p.stopStateLogging(logId)
--- a/examples/pybullet/examples/manyspheres.py
+++ b/examples/pybullet/examples/manyspheres.py
@@ -0,0 +1,25 @@
+import pybullet as p
+import time
+
+p.connect(p.GUI)
+p.loadURDF("plane.urdf",useMaximalCoordinates=True)
+p.loadURDF("tray/traybox.urdf",useMaximalCoordinates=True)
+
+gravXid = p.addUserDebugParameter("gravityX",-10,10,0)
+gravYid = p.addUserDebugParameter("gravityY",-10,10,0)
+gravZid = p.addUserDebugParameter("gravityZ",-10,10,-10)
+p.setPhysicsEngineParameter(numSolverIterations=10)
+p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
+for i in range (10):
+    for j in range (10):
+        for k in range (5):
+            ob = p.loadURDF("sphere_1cm.urdf",[0.02*i,0.02*j,0.2+0.02*k],useMaximalCoordinates=True)
+p.setGravity(0,0,-10)
+p.setRealTimeSimulation(1)
+while True:
+    gravX = p.readUserDebugParameter(gravXid)
+    gravY = p.readUserDebugParameter(gravYid)
+    gravZ = p.readUserDebugParameter(gravZid)
+    p.setGravity(gravX,gravY,gravZ)
+    time.sleep(0.01)
+
--- a/examples/pybullet/examples/minitaur.py
+++ b/examples/pybullet/examples/minitaur.py
@@ -0,0 +1,135 @@
+import pybullet as p
+import numpy as np
+
+class Minitaur:
+  def __init__(self, urdfRootPath=''):
+    self.urdfRootPath = urdfRootPath
+    self.reset()
+
+  def buildJointNameToIdDict(self):
+    nJoints = p.getNumJoints(self.quadruped)
+    self.jointNameToId = {}
+    for i in range(nJoints):
+      jointInfo = p.getJointInfo(self.quadruped, i)
+      self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
+    self.resetPose()
+    for i in range(100):
+      p.stepSimulation()
+
+  def buildMotorIdList(self):
+    self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint'])
+
+
+  def reset(self):
+    self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath,0,0,.2)
+    self.kp = 1
+    self.kd = 0.1
+    self.maxForce = 3.5
+    self.nMotors = 8
+    self.motorIdList = []
+    self.motorDir = [1, 1, 1, 1, 1, 1, 1, 1]
+    self.buildJointNameToIdDict()
+    self.buildMotorIdList()
+
+
+
+  def setMotorAngleById(self, motorId, desiredAngle):
+    p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
+
+  def setMotorAngleByName(self, motorName, desiredAngle):
+    self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle)
+
+  def resetPose(self):
+    kneeFrictionForce = 0
+    halfpi = 1.57079632679
+    kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)
+
+    #left front leg
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle)
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle)
+    p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
+    self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi)
+    self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi)
+    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
+    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
+
+    #left back leg
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle)
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle)
+    p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
+    self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi)
+    self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi)
+    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
+    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
+ 
+    #right front leg
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle)
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle)
+    p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
+    self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi)
+    self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi)
+    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
+    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
+ 
+
+    #right back leg
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle)
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle)
+    p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
+    self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi)
+    self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi)
+    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
+    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
+
+
+  def getBasePosition(self):
+    position, orientation = p.getBasePositionAndOrientation(self.quadruped)
+    return position
+
+  def getBaseOrientation(self):
+    position, orientation = p.getBasePositionAndOrientation(self.quadruped)
+    return orientation
+
+  def applyAction(self, motorCommands):
+    motorCommandsWithDir = np.multiply(motorCommands, self.motorDir)
+    for i in range(self.nMotors):
+      self.setMotorAngleById(self.motorIdList[i], motorCommandsWithDir[i])
+
+  def getMotorAngles(self):
+    motorAngles = []
+    for i in range(self.nMotors):
+      jointState = p.getJointState(self.quadruped, self.motorIdList[i])
+      motorAngles.append(jointState[0])
+    motorAngles = np.multiply(motorAngles, self.motorDir)
+    return motorAngles
+
+  def getMotorVelocities(self):
+    motorVelocities = []
+    for i in range(self.nMotors):
+      jointState = p.getJointState(self.quadruped, self.motorIdList[i])
+      motorVelocities.append(jointState[1])
+    motorVelocities = np.multiply(motorVelocities, self.motorDir)
+    return motorVelocities
+
+  def getMotorTorques(self):
+    motorTorques = []
+    for i in range(self.nMotors):
+      jointState = p.getJointState(self.quadruped, self.motorIdList[i])
+      motorTorques.append(jointState[3])
+    motorTorques = np.multiply(motorTorques, self.motorDir)
+    return motorTorques
--- a/examples/pybullet/examples/minitaur_evaluate.py
+++ b/examples/pybullet/examples/minitaur_evaluate.py
@@ -0,0 +1,99 @@
+from minitaur import Minitaur
+import pybullet as p
+import numpy as np
+import time
+import sys
+import math
+
+minitaur = None
+
+evaluate_func_map = dict()
+
+
+def current_position():
+  global minitaur
+  position = minitaur.getBasePosition()
+  return np.asarray(position)
+
+def is_fallen():
+  global minitaur
+  orientation = minitaur.getBaseOrientation()
+  rotMat = p.getMatrixFromQuaternion(orientation)
+  localUp = rotMat[6:]
+  return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0
+
+def evaluate_desired_motorAngle_8Amplitude8Phase(i, params):
+  nMotors = 8
+  speed = 0.35
+  for jthMotor in range(nMotors):
+    joint_values[jthMotor] = math.sin(i*speed + params[nMotors + jthMotor])*params[jthMotor]*+1.57
+  return joint_values
+
+def evaluate_desired_motorAngle_2Amplitude4Phase(i, params):
+  speed = 0.35
+  phaseDiff = params[2]
+  a0 = math.sin(i * speed) * params[0] + 1.57
+  a1 = math.sin(i * speed + phaseDiff) * params[1] + 1.57
+  a2 = math.sin(i * speed + params[3]) * params[0] + 1.57
+  a3 = math.sin(i * speed + params[3] + phaseDiff) * params[1] + 1.57
+  a4 = math.sin(i * speed + params[4] + phaseDiff) * params[1] + 1.57
+  a5 = math.sin(i * speed + params[4]) * params[0] + 1.57
+  a6 = math.sin(i * speed + params[5] + phaseDiff) * params[1] + 1.57
+  a7 = math.sin(i * speed + params[5]) * params[0] + 1.57
+  joint_values = [a0, a1, a2, a3, a4, a5, a6, a7]
+  return joint_values
+
+def evaluate_desired_motorAngle_hop(i, params):
+  amplitude = params[0]
+  speed = params[1]
+  a1 = math.sin(i*speed)*amplitude+1.57
+  a2 = math.sin(i*speed+3.14)*amplitude+1.57
+  joint_values = [a1, 1.57, a2, 1.57, 1.57, a1, 1.57, a2]
+  return joint_values
+
+
+evaluate_func_map['evaluate_desired_motorAngle_8Amplitude8Phase'] = evaluate_desired_motorAngle_8Amplitude8Phase
+evaluate_func_map['evaluate_desired_motorAngle_2Amplitude4Phase'] = evaluate_desired_motorAngle_2Amplitude4Phase
+evaluate_func_map['evaluate_desired_motorAngle_hop'] = evaluate_desired_motorAngle_hop
+
+
+
+def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=10000, sleepTime=0):
+  print('start evaluation')
+  beforeTime = time.time()
+  p.resetSimulation()
+
+  p.setTimeStep(timeStep)
+  p.loadURDF("%s/plane.urdf" % urdfRoot)
+  p.setGravity(0,0,-10)
+
+  global minitaur
+  minitaur = Minitaur(urdfRoot)
+  start_position = current_position()
+  last_position = None  # for tracing line
+  total_energy = 0
+
+  for i in range(maxNumSteps):
+    torques = minitaur.getMotorTorques()
+    velocities = minitaur.getMotorVelocities()
+    total_energy += np.dot(np.fabs(torques), np.fabs(velocities)) * timeStep
+
+    joint_values = evaluate_func_map[evaluateFunc](i, params)
+    minitaur.applyAction(joint_values)
+    p.stepSimulation()
+    if (is_fallen()):
+      break
+
+    if i % 100 == 0:
+      sys.stdout.write('.')
+      sys.stdout.flush()
+    time.sleep(sleepTime)
+
+  print(' ')
+
+  alpha = objectiveParams[0]
+  final_distance = np.linalg.norm(start_position - current_position())
+  finalReturn = final_distance - alpha * total_energy
+  elapsedTime = time.time() - beforeTime
+  print ("trial for ", params, " final_distance", final_distance, "total_energy", total_energy, "finalReturn", finalReturn, "elapsed_time", elapsedTime)
+  return finalReturn
--- a/examples/pybullet/examples/minitaur_test.py
+++ b/examples/pybullet/examples/minitaur_test.py
@@ -0,0 +1,27 @@
+import sys
+#some python interpreters need '.' added
+sys.path.append(".")
+
+import pybullet as p
+from minitaur import Minitaur
+from minitaur_evaluate import *
+
+import time
+import math
+import numpy as np
+
+def main(unused_args):
+  timeStep = 0.01
+  c = p.connect(p.SHARED_MEMORY)
+  if (c<0):
+      c = p.connect(p.GUI)
+
+  params = [0.1903581461951056, 0.0006732219568880068, 0.05018085615283363, 3.219916795483583, 6.2406418167980595, 4.189869754607539]
+  evaluate_func = 'evaluate_desired_motorAngle_2Amplitude4Phase'
+  energy_weight = 0.01
+
+  finalReturn = evaluate_params(evaluateFunc = evaluate_func, params=params, objectiveParams=[energy_weight], timeStep=timeStep, sleepTime=timeStep)
+
+  print(finalReturn)
+
+main(0)
--- a/examples/pybullet/examples/mylittleminitaur.py
+++ b/examples/pybullet/examples/mylittleminitaur.py
@@ -0,0 +1,21 @@
+import pybullet as p
+import time
+import math
+import sys
+sys.path.append(".")
+
+from minitaur import Minitaur
+p.connect(p.GUI)
+p.setTimeOut(5)
+#p.setPhysicsEngineParameter(numSolverIterations=50)
+p.setGravity(0,0,-10)
+p.setTimeStep(0.01)
+
+urdfRoot = ''
+p.loadURDF("%s/plane.urdf" % urdfRoot)
+minitaur = Minitaur(urdfRoot)
+
+while (True):
+	p.stepSimulation()
+	time.sleep(0.01)
+  
--- a/examples/pybullet/examples/quadruped.py
+++ b/examples/pybullet/examples/quadruped.py
@@ -0,0 +1,179 @@
+import pybullet as p
+import time
+import math
+
+useRealTime = 0
+fixedTimeStep = 0.01
+speed = 10
+amplitude = 0.8
+jump_amp = 0.5
+maxForce = 3.5
+kneeFrictionForce = 0.00
+kp = 1
+kd = .1
+
+
+physId = p.connect(p.SHARED_MEMORY)
+if (physId<0):
+	p.connect(p.GUI)
+#p.resetSimulation()
+p.loadURDF("plane.urdf",0,0,0)
+p.setPhysicsEngineParameter(numSolverIterations=50)
+
+p.setTimeOut(4)
+
+p.setGravity(0,0,0)
+p.setTimeStep(fixedTimeStep)
+
+orn = p.getQuaternionFromEuler([0,0,0.4])
+p.setRealTimeSimulation(0)
+quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,0,0.2],orn,useFixedBase=False)
+nJoints = p.getNumJoints(quadruped)
+
+jointNameToId = {}
+for i in range(nJoints):
+	jointInfo = p.getJointInfo(quadruped, i)
+	jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
+
+
+motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
+hip_front_rightR_link = jointNameToId['hip_front_rightR_link']
+knee_front_rightR_link = jointNameToId['knee_front_rightR_link']
+motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
+motor_front_rightL_link = jointNameToId['motor_front_rightL_link']
+knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
+motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
+hip_front_leftR_link = jointNameToId['hip_front_leftR_link']
+knee_front_leftR_link = jointNameToId['knee_front_leftR_link']
+motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
+motor_front_leftL_link = jointNameToId['motor_front_leftL_link']
+knee_front_leftL_link = jointNameToId['knee_front_leftL_link']
+motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint']
+hip_rightR_link = jointNameToId['hip_rightR_link']
+knee_back_rightR_link = jointNameToId['knee_back_rightR_link']
+motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
+motor_back_rightL_link = jointNameToId['motor_back_rightL_link']
+knee_back_rightL_link = jointNameToId['knee_back_rightL_link']
+motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint']
+hip_leftR_link = jointNameToId['hip_leftR_link']
+knee_back_leftR_link = jointNameToId['knee_back_leftR_link']
+motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
+motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
+knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
+
+motordir=[-1,-1,-1,-1,1,1,1,1]
+halfpi = 1.57079632679
+kneeangle = -2.1834
+p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi)
+p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle)
+p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi)
+p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle)
+cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
+p.changeConstraint(cid,maxForce=10000)
+
+p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
+p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
+
+p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi)
+p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle)
+p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi)
+p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle)
+cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
+p.changeConstraint(cid,maxForce=10000)
+p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
+p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
+
+
+#p.getNumJoints(1)
+
+
+p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi)
+p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle)
+p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi)
+p.resetJointState(quadruped,knee_front_rightR_link,motordir[5]*kneeangle)
+cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
+p.changeConstraint(cid,maxForce=10000)
+
+p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
+p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
+
+p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi)
+p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle)
+p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi)
+p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle)
+cid = p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
+p.changeConstraint(cid,maxForce=10000)
+
+p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
+p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
+
+p.setGravity(0,0,-10)
+
+
+
+legnumbering=[
+	motor_front_leftL_joint,
+	motor_front_leftR_joint,
+	motor_back_leftL_joint,
+	motor_back_leftR_joint,
+	motor_front_rightL_joint,
+	motor_front_rightR_joint,
+	motor_back_rightL_joint,
+	motor_back_rightR_joint]
+
+for i in range (8):
+	print (legnumbering[i])
+#use the Minitaur leg numbering
+p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
+p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
+#stand still
+p.setRealTimeSimulation(useRealTime)
+
+#while (True):
+#	time.sleep(0.01)
+#p.stepSimulation()
+
+
+print("quadruped Id = ")
+print(quadruped)
+p.saveWorld("quadru.py")
+logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.txt",[quadruped])
+
+
+
+
+
+
+#jump
+t = 0.0
+t_end = t + 100
+i=0
+ref_time = time.time()
+
+while t < t_end:
+	if (useRealTime):
+		t = time.time()-ref_time
+	else:
+		t = t+fixedTimeStep
+	if (True):
+	
+		target = math.sin(t*speed)*jump_amp+1.57;
+		p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*target,positionGain=kp, velocityGain=kd, force=maxForce)
+		p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*target,positionGain=kp, velocityGain=kd, force=maxForce)
+		p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*target,positionGain=kp, velocityGain=kd, force=maxForce)
+		p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*target,positionGain=kp, velocityGain=kd, force=maxForce)
+		p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*target,positionGain=kp, velocityGain=kd, force=maxForce)
+		p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*target,positionGain=kp, velocityGain=kd, force=maxForce)
+		p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*target,positionGain=kp, velocityGain=kd, force=maxForce)
+		p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*target,positionGain=kp, velocityGain=kd, force=maxForce)
+		
+	if (useRealTime==0):	
+		p.stepSimulation()
+		time.sleep(fixedTimeStep)
+
--- a/examples/pybullet/examples/quadruped_playback.py
+++ b/examples/pybullet/examples/quadruped_playback.py
@@ -0,0 +1,126 @@
+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')
+  print ("num chunks")
+  print (len(chunks))
+  
+  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)
+
+log = readLogFile("LOG00076.TXT");
+
+recordNum = len(log)
+print('record num:'),
+print(recordNum)
+itemNum = len(log[0])
+print('item num:'),
+print(itemNum)
+
+useRealTime = 0
+fixedTimeStep = 0.001
+speed = 10
+amplitude = 0.8
+jump_amp = 0.5
+maxForce = 3.5
+kp = .05
+kd = .5
+
+
+
+quadruped = 1
+nJoints = p.getNumJoints(quadruped)
+jointNameToId = {}
+for i in range(nJoints):
+  jointInfo = p.getJointInfo(quadruped, i)
+  jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
+
+motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
+hip_front_rightR_link = jointNameToId['hip_front_rightR_link']
+knee_front_rightR_link = jointNameToId['knee_front_rightR_link']
+motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
+motor_front_rightL_link = jointNameToId['motor_front_rightL_link']
+knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
+motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
+hip_front_leftR_link = jointNameToId['hip_front_leftR_link']
+knee_front_leftR_link = jointNameToId['knee_front_leftR_link']
+motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
+motor_front_leftL_link = jointNameToId['motor_front_leftL_link']
+knee_front_leftL_link = jointNameToId['knee_front_leftL_link']
+motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint']
+hip_rightR_link = jointNameToId['hip_rightR_link']
+knee_back_rightR_link = jointNameToId['knee_back_rightR_link']
+motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
+motor_back_rightL_link = jointNameToId['motor_back_rightL_link']
+knee_back_rightL_link = jointNameToId['knee_back_rightL_link']
+motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint']
+hip_leftR_link = jointNameToId['hip_leftR_link']
+knee_back_leftR_link = jointNameToId['knee_back_leftR_link']
+motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
+motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
+knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
+
+motorDir = [1, 1, 1, 1, 1, 1, 1, 1];
+legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint]
+
+
+for record in log:
+  p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[0], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[0]*record[7], positionGain=kp, velocityGain=kd, force=maxForce)
+  p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[1], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[1]*record[8], positionGain=kp, velocityGain=kd, force=maxForce)
+  p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[2], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[2]*record[9], positionGain=kp, velocityGain=kd, force=maxForce)
+  p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[3], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[3]*record[10], positionGain=kp, velocityGain=kd, force=maxForce)
+  p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[4], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[4]*record[11], positionGain=kp, velocityGain=kd, force=maxForce)
+  p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[5], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[5]*record[12], positionGain=kp, velocityGain=kd, force=maxForce)
+  p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[6], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[6]*record[13], positionGain=kp, velocityGain=kd, force=maxForce)
+  p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[7], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[7]*record[14], positionGain=kp, velocityGain=kd, force=maxForce)
+  p.setGravity(0.000000,0.000000,-10.000000)
+  p.stepSimulation()
+  p.stepSimulation()
+  sleep(0.01)
+  
--- a/examples/pybullet/examples/quadruped_setup_playback.py
+++ b/examples/pybullet/examples/quadruped_setup_playback.py
@@ -0,0 +1,21 @@
+import pybullet as p
+p.connect(p.SHARED_MEMORY)
+objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,-.300000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("quadruped/minitaur.urdf", [-0.000046,-0.000068,0.200774],[-0.000701,0.000387,-0.000252,1.000000],useFixedBase=False)]
+ob = objects[0]
+jointPositions=[ 0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000, -2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193, 0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517 ]
+for ji in range (p.getNumJoints(ob)):
+	p.resetJointState(ob,ji,jointPositions[ji])
+	p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0)
+
+cid0 = p.createConstraint(1,3,1,6,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
+p.changeConstraint(cid0,maxForce=500.000000)
+cid1 = p.createConstraint(1,16,1,19,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
+p.changeConstraint(cid1,maxForce=500.000000)
+cid2 = p.createConstraint(1,9,1,12,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
+p.changeConstraint(cid2,maxForce=500.000000)
+cid3 = p.createConstraint(1,22,1,25,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
+p.changeConstraint(cid3,maxForce=500.000000)
+p.setGravity(0.000000,0.000000,0.000000)
+p.stepSimulation()
+p.disconnect()
--- a/examples/pybullet/examples/robotcontrol.py
+++ b/examples/pybullet/examples/robotcontrol.py
@@ -0,0 +1,30 @@
+import pybullet as p
+p.connect(p.GUI) #or p.SHARED_MEMORY or p.DIRECT
+
+p.loadURDF("plane.urdf")
+p.setGravity(0,0,-10)
+huskypos = [0,0,0.1]
+
+husky = p.loadURDF("husky/husky.urdf",huskypos[0],huskypos[1],huskypos[2])
+numJoints = p.getNumJoints(husky)
+for joint in range (numJoints) :
+	print (p.getJointInfo(husky,joint))
+targetVel = 10 #rad/s
+maxForce = 100 #Newton
+
+for joint in range (2,6) :
+	p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce)
+for step in range (300):
+	p.stepSimulation()
+
+targetVel=-10
+for joint in range (2,6) :
+        p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce)
+for step in range (400):
+        p.stepSimulation()
+
+p.getContactPoints(husky)
+
+p.disconnect()
+
+
--- a/examples/pybullet/examples/rollPitchYaw.py
+++ b/examples/pybullet/examples/rollPitchYaw.py
@@ -0,0 +1,26 @@
+import pybullet as p
+import time
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid<0):
+	p.connect(p.GUI)
+q = p.loadURDF("quadruped/quadruped.urdf",useFixedBase=True)
+rollId = p.addUserDebugParameter("roll",-1.5,1.5,0)
+pitchId = p.addUserDebugParameter("pitch",-1.5,1.5,0)
+yawId = p.addUserDebugParameter("yaw",-1.5,1.5,0)
+fwdxId = p.addUserDebugParameter("fwd_x",-1,1,0)
+fwdyId = p.addUserDebugParameter("fwd_y",-1,1,0)
+fwdzId = p.addUserDebugParameter("fwd_z",-1,1,0)
+
+while True:
+	roll = p.readUserDebugParameter(rollId)
+	pitch = p.readUserDebugParameter(pitchId)
+	yaw = p.readUserDebugParameter(yawId)
+	x = p.readUserDebugParameter(fwdxId)
+	y = p.readUserDebugParameter(fwdyId)
+	z = p.readUserDebugParameter(fwdzId)
+
+	orn = p.getQuaternionFromEuler([roll,pitch,yaw])
+	p.resetBasePositionAndOrientation(q,[x,y,z],orn)
+	#p.stepSimulation()#not really necessary for this demo, no physics used
+
--- a/examples/pybullet/examples/saveWorld.py
+++ b/examples/pybullet/examples/saveWorld.py
@@ -0,0 +1,8 @@
+import pybullet as p
+import time
+
+p.connect(p.SHARED_MEMORY)
+timestr = time.strftime("%Y%m%d-%H%M%S")
+filename = "saveWorld" + timestr + ".py"
+p.saveWorld(filename)
+p.disconnect()
--- a/examples/pybullet/examples/test.py
+++ b/examples/pybullet/examples/test.py
@@ -0,0 +1,36 @@
+import pybullet
+import time
+
+#choose connection method: GUI, DIRECT, SHARED_MEMORY
+pybullet.connect(pybullet.GUI)
+pybullet.loadURDF("plane.urdf",0,0,-1)
+#load URDF, given a relative or absolute file+path
+obj = pybullet.loadURDF("r2d2.urdf")
+
+posX=0
+posY=3
+posZ=2
+obj2 = pybullet.loadURDF("kuka_lwr/kuka.urdf",posX,posY,posZ)
+
+#query the number of joints of the object
+numJoints = pybullet.getNumJoints(obj)
+
+print (numJoints)
+
+#set the gravity acceleration
+pybullet.setGravity(0,0,-9.8)
+
+#step the simulation for 5 seconds
+t_end = time.time() + 5
+while time.time() < t_end:
+	pybullet.stepSimulation()
+	posAndOrn = pybullet.getBasePositionAndOrientation(obj)
+	print (posAndOrn)
+
+print ("finished")
+#remove all objects
+pybullet.resetSimulation()
+
+#disconnect from the physics server
+pybullet.disconnect()
+
--- a/examples/pybullet/examples/testrender.py
+++ b/examples/pybullet/examples/testrender.py
@@ -0,0 +1,47 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import pybullet
+
+pybullet.connect(pybullet.GUI)
+pybullet.loadURDF("r2d2.urdf")
+
+camTargetPos = [0.,0.,0.]
+cameraUp = [0,0,1]
+cameraPos = [1,1,1]
+yaw = 40
+pitch = 10.0
+
+roll=0
+upAxisIndex = 2
+camDistance = 4
+pixelWidth = 320
+pixelHeight = 240
+nearPlane = 0.01
+farPlane = 1000
+lightDirection = [0,1,0]
+lightColor = [1,1,1]#optional argument
+fov = 60
+
+#img_arr = pybullet.renderImage(pixelWidth, pixelHeight)
+#renderImage(w, h, view[16], projection[16])
+#img_arr = pybullet.renderImage(pixelWidth, pixelHeight, cameraPos, camTargetPos, cameraUp, nearPlane, farPlane)
+for pitch in range (0,360,10) :
+    viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
+    aspect = pixelWidth / pixelHeight;
+    projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
+    #getCameraImage can also use renderer=pybullet.ER_BULLET_HARDWARE_OPENGL
+    img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection,lightColor,renderer=pybullet.ER_TINY_RENDERER)
+    w=img_arr[0]
+    h=img_arr[1]
+    rgb=img_arr[2]
+    dep=img_arr[3]
+    #print 'width = %d height = %d' % (w,h)
+    # reshape creates np array
+    np_img_arr = np.reshape(rgb, (h, w, 4))
+    np_img_arr = np_img_arr*(1./255.)
+    #show
+    plt.imshow(np_img_arr,interpolation='none')
+    
+    plt.pause(0.01)
+
+pybullet.resetSimulation()
--- a/examples/pybullet/examples/testrender_np.py
+++ b/examples/pybullet/examples/testrender_np.py
@@ -0,0 +1,54 @@
+
+#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled, otherwise use testrender.py (slower but compatible without numpy)
+
+import numpy as np
+import matplotlib.pyplot as plt
+import pybullet
+import time
+
+pybullet.connect(pybullet.GUI)
+pybullet.loadURDF("r2d2.urdf")
+
+camTargetPos = [0,0,0]
+cameraUp = [0,0,1]
+cameraPos = [1,1,1]
+yaw = 40
+pitch = 10.0
+
+roll=0
+upAxisIndex = 2
+camDistance = 4
+pixelWidth = 1024
+pixelHeight = 768
+nearPlane = 0.01
+farPlane = 1000
+
+fov = 60
+
+main_start = time.time()
+for pitch in range (0,360,10) :
+    start = time.time()
+    viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
+    aspect = pixelWidth / pixelHeight;
+    projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
+    img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, [0,1,0],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
+    stop = time.time()
+    print ("renderImage %f" % (stop - start))
+
+    w=img_arr[0] #width of the image, in pixels
+    h=img_arr[1] #height of the image, in pixels
+    rgb=img_arr[2] #color data RGB
+    dep=img_arr[3] #depth data
+
+    print ('width = %d height = %d' % (w,h))
+
+    #note that sending the data to matplotlib is really slow
+
+    plt.imshow(rgb,interpolation='none')
+    plt.pause(0.001)
+
+main_stop = time.time()
+
+print ("Total time %f" % (main_stop - main_start))
+
+pybullet.resetSimulation()
--- a/examples/pybullet/examples/vrEvent.py
+++ b/examples/pybullet/examples/vrEvent.py
@@ -0,0 +1,62 @@
+# See pybullet quickstart guide here:
+# https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#
+# Create a Tiltbrush-like app, drawing lines using any controller
+# Line width can be changed
+
+import pybullet as p
+
+CONTROLLER_ID = 0
+POSITION=1
+ORIENTATION=2
+BUTTONS=6
+
+#assume that the VR physics server is already started before
+c = p.connect(p.SHARED_MEMORY)
+print(c)
+if (c<0):
+		p.connect(p.GUI)
+
+p.setInternalSimFlags(0)#don't load default robot assets etc
+p.resetSimulation()
+p.loadURDF("plane.urdf")
+
+prevPosition=[None]*p.VR_MAX_CONTROLLERS
+colors=[0.,0.5,0.5]*p.VR_MAX_CONTROLLERS
+widths = [3]*p.VR_MAX_CONTROLLERS
+
+#use a few default colors
+colors[0] = [0,0,0]
+colors[1] = [0.5,0,0]
+colors[2] = [0,0.5,0]
+colors[3] = [0,0,0.5]
+colors[4] = [0.5,0.5,0.]
+colors[5] = [.5,.5,.5]
+
+while True:
+	events = p.getVREvents()
+
+	for e in (events):
+		if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED):
+			prevPosition[e[CONTROLLER_ID]] = e[POSITION]
+		if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED):
+			widths[e[CONTROLLER_ID]]=widths[e[0]]+1
+			if (widths[e[CONTROLLER_ID]]>20):
+				widths[e[CONTROLLER_ID]] = 1
+		if (e[BUTTONS][1]&p.VR_BUTTON_WAS_TRIGGERED):
+			p.resetSimulation()
+			#p.setGravity(0,0,-10)
+			p.removeAllUserDebugItems()
+			p.loadURDF("plane.urdf")
+		if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN):
+			pt = prevPosition[e[CONTROLLER_ID]]
+			
+			#print(prevPosition[e[0]])
+			#print(e[1])
+			diff = [pt[0]-e[POSITION][0],pt[1]-e[POSITION][1],pt[2]-e[POSITION][2]]
+			lenSqr =	diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2]
+			ptDistThreshold = 0.01
+			if (lenSqr>(ptDistThreshold*ptDistThreshold)):
+				p.addUserDebugLine(e[POSITION],prevPosition[e[CONTROLLER_ID]],colors[e[CONTROLLER_ID]],widths[e[CONTROLLER_ID]])
+				#p.loadURDF("cube_small.urdf",e[1])
+				colors[e[CONTROLLER_ID]] = [1-colors[e[CONTROLLER_ID]][0],1-colors[e[CONTROLLER_ID]][1],1-colors[e[CONTROLLER_ID]][2]]
+				prevPosition[e[CONTROLLER_ID]] = e[POSITION]			
--- a/examples/pybullet/examples/vr_kuka_control.py
+++ b/examples/pybullet/examples/vr_kuka_control.py
@@ -0,0 +1,99 @@
+## Assume you have run vr_kuka_setup and have default scene set up
+# Require p.setInternalSimFlags(0) in kuka_setup
+import pybullet as p
+import math
+# import numpy as np
+
+p.connect(p.SHARED_MEMORY)
+
+kuka = 3
+kuka_gripper = 7
+POSITION = 1
+ORIENTATION = 2
+BUTTONS = 6
+
+THRESHOLD = 1.3
+LOWER_LIMITS = [-.967, -2.0, -2.96, 0.19, -2.96, -2.09, -3.05]
+UPPER_LIMITS = [.96, 2.0, 2.96, 2.29, 2.96, 2.09, 3.05]
+JOINT_RANGE = [5.8, 4, 5.8, 4, 5.8, 4, 6]
+REST_POSE = [0, 0, 0, math.pi / 2, 0, -math.pi * 0.66, 0]
+JOINT_DAMP = [.1, .1, .1, .1, .1, .1, .1]
+REST_JOINT_POS = [-0., -0., 0., 1.570793, 0., -1.036725, 0.000001]
+MAX_FORCE = 500
+KUKA_GRIPPER_REST_POS = [0., -0.011130, -0.206421, 0.205143, -0.009999, 0., -0.010055, 0.]
+KUKA_GRIPPER_CLOZ_POS = [0.0, -0.047564246423083795, 0.6855956234759611, -0.7479294372303137, 0.05054599996976922, 0.0, 0.049838105678835724, 0.0]
+
+def euc_dist(posA, posB):
+	dist = 0.
+	for i in range(len(posA)):
+		dist += (posA[i] - posB[i]) ** 2
+	return dist
+
+p.setRealTimeSimulation(1)
+
+controllers = [e[0] for e in p.getVREvents()]
+
+while True:
+
+	events = p.getVREvents()
+	for e in (events):
+
+		# Only use one controller
+		###########################################
+		# This is important: make sure there's only one VR Controller!
+		if e[0] == controllers[0]:
+			break
+
+		sq_len = euc_dist(p.getLinkState(kuka, 6)[0], e[POSITION])
+
+		# A simplistic version of gripper control
+		#@TO-DO: Add slider for the gripper
+		if e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED:
+			# avg = 0.
+			for i in range(p.getNumJoints(kuka_gripper)):
+				p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=KUKA_GRIPPER_CLOZ_POS[i], force=50)
+
+		if e[BUTTONS][33] & p.VR_BUTTON_WAS_RELEASED:	
+			for i in range(p.getNumJoints(kuka_gripper)):
+				p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=KUKA_GRIPPER_REST_POS[i], force=50)
+
+		if sq_len < THRESHOLD * THRESHOLD:
+			eef_pos = e[POSITION]
+
+			joint_pos = p.calculateInverseKinematics(kuka, 6, eef_pos, 
+				lowerLimits=LOWER_LIMITS, upperLimits=UPPER_LIMITS, 
+				jointRanges=JOINT_RANGE, restPoses=REST_POSE, jointDamping=JOINT_DAMP)
+
+			# Only need links 1- 4, no need for joint 5-6 with pure position IK
+			for i in range(len(joint_pos) - 2):
+				p.setJointMotorControl2(kuka, i, p.POSITION_CONTROL, 
+					targetPosition=joint_pos[i], targetVelocity=0, positionGain=0.05, 
+					velocityGain=1.0, force=MAX_FORCE)
+	
+			# Rotate the end effector
+			targetOrn = e[ORIENTATION]
+
+			_, _, z = p.getEulerFromQuaternion(targetOrn)
+			# End effector needs protection, done by using triangular tricks
+
+			if LOWER_LIMITS[6] < z < UPPER_LIMITS[6]:
+				p.setJointMotorControl2(kuka, 6, p.POSITION_CONTROL, 
+					targetPosition=z, targetVelocity=0, positionGain=0.03, velocityGain=1.0, force=MAX_FORCE)
+
+			else:
+				p.setJointMotorControl2(kuka, 6, p.POSITION_CONTROL, 
+					targetPosition=joint_pos[6], targetVelocity=0, positionGain=0.05, 
+					velocityGain=1.0, force=MAX_FORCE)
+
+			p.setJointMotorControl2(kuka, 5, p.POSITION_CONTROL, 
+				targetPosition=-1.57, targetVelocity=0, 
+				positionGain=0.03, velocityGain=1.0, force=MAX_FORCE)
+
+		else:
+			# Set back to original rest pose
+			for jointIndex in range(p.getNumJoints(kuka)):
+				p.setJointMotorControl2(kuka, jointIndex, p.POSITION_CONTROL, 
+					REST_JOINT_POS[jointIndex], 0)
+
+
+
--- a/examples/pybullet/examples/vr_kuka_pr2_move.py
+++ b/examples/pybullet/examples/vr_kuka_pr2_move.py
@@ -0,0 +1,22 @@
+#python script with hardcoded values, assumes that you run the vr_kuka_setup.py first
+
+import pybullet as p
+p.connect(p.SHARED_MEMORY)
+
+pr2_gripper = 2
+pr2_cid = 1
+
+CONTROLLER_ID = 0
+POSITION=1
+ORIENTATION=2
+ANALOG=3
+BUTTONS=6
+
+gripper_max_joint = 0.550569
+while True:
+	events = p.getVREvents()
+	for e in (events):
+		if e[CONTROLLER_ID] == 3:  # To make sure we only get the value for one of the remotes
+			p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500)
+			p.setJointMotorControl2(pr2_gripper, 0, controlMode=p.POSITION_CONTROL,targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,force=1.0)
+			p.setJointMotorControl2(pr2_gripper, 2, controlMode=p.POSITION_CONTROL,targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,force=1.1)
--- a/examples/pybullet/examples/vr_kuka_setup.py
+++ b/examples/pybullet/examples/vr_kuka_setup.py
@@ -0,0 +1,79 @@
+import pybullet as p
+#p.connect(p.UDP,"192.168.86.100")
+p.connect(p.SHARED_MEMORY)
+p.resetSimulation()
+
+objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)]
+pr2_gripper = objects[0]
+print ("pr2_gripper=")
+print (pr2_gripper)
+
+jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
+for jointIndex in range (p.getNumJoints(pr2_gripper)):
+	p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
+
+pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
+print ("pr2_cid")
+print (pr2_cid)
+
+objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
+kuka = objects[0]
+jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
+for jointIndex in range (p.getNumJoints(kuka)):
+	p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
+	p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
+
+objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)]
+objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
+kuka_gripper = objects[0]
+print ("kuka gripper=")
+print(kuka_gripper)
+
+p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970])
+jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ]
+for jointIndex in range (p.getNumJoints(kuka_gripper)):
+	p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex])
+	p.setJointMotorControl2(kuka_gripper,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
+
+
+kuka_cid = p.createConstraint(kuka,   6,  kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0])
+
+objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
+objects = [p.loadURDF("teddy_vhacd.urdf", 1.050000,-0.500000,0.700000,0.000000,0.000000,0.707107,0.707107)]
+objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
+objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)]
+objects = [p.loadURDF("duck_vhacd.urdf", 0.850000,-0.400000,0.900000,0.000000,0.000000,0.707107,0.707107)]
+objects = p.loadSDF("kiva_shelf/model.sdf")
+ob = objects[0]
+p.resetBasePositionAndOrientation(ob,[0.000000,1.000000,1.204500],[0.000000,0.000000,0.000000,1.000000])
+objects = [p.loadURDF("teddy_vhacd.urdf", -0.100000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("sphere_small.urdf", -0.100000,0.955006,1.169706,0.633232,-0.000000,-0.000000,0.773962)]
+objects = [p.loadURDF("cube_small.urdf", 0.300000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
+objects = [p.loadURDF("table_square/table_square.urdf", -1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
+ob = objects[0]
+jointPositions=[ 0.000000 ]
+for jointIndex in range (p.getNumJoints(ob)):
+	p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
+
+objects = [p.loadURDF("husky/husky.urdf", 2.000000,-5.000000,1.000000,0.000000,0.000000,0.000000,1.000000)]
+ob = objects[0]
+jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000 ]
+for jointIndex in range (p.getNumJoints(ob)):
+	p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
+
+p.setGravity(0.000000,0.000000,0.000000)
+p.setGravity(0,0,-10)
+
+p.stepSimulation()
+
+p.disconnect()
--- a/examples/pybullet/examples/vrhand.py
+++ b/examples/pybullet/examples/vrhand.py
@@ -0,0 +1,111 @@
+#script to control a simulated robot hand using a VR glove
+#see https://twitter.com/erwincoumans/status/821953216271106048
+#and https://www.youtube.com/watch?v=I6s37aBXbV8
+#vr glove was custom build using Spectra Symbolflex sensors (4.5")
+#inside a Under Armour Batting Glove, using DFRobot Bluno BLE/Beetle
+#with BLE Link to receive serial (for wireless bluetooth serial)
+
+import serial
+import time
+import pybullet as p
+
+#first try to connect to shared memory (VR), if it fails use local GUI
+c = p.connect(p.SHARED_MEMORY)
+if (c<0):
+	c = p.connect(p.GUI)
+#p.resetSimulation()
+p.setGravity(0,0,-10)
+print(c)
+if (c<0):
+		p.connect(p.GUI)
+		
+#load the MuJoCo MJCF hand
+objects = p.loadMJCF("MPL/mpl2.xml")
+
+hand=objects[0]
+ho = p.getQuaternionFromEuler([0,3.14,0])
+hand_cid = p.createConstraint(hand,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.1,0,0],[0.500000,0.300006,0.700000],ho)
+print ("hand_cid")
+print (hand_cid)
+for i in range (p.getNumJoints(hand)):
+	p.setJointMotorControl2(hand,i,p.POSITION_CONTROL,0,0)
+
+#clamp in range 400-600
+minV = 400
+maxV = 600
+
+POSITION=1
+ORIENTATION=2
+BUTTONS=6
+
+p.setRealTimeSimulation(1)
+
+def convertSensor(x):
+	v = minV
+	try:
+		v = float(x)
+	except ValueError:
+		v = minV    
+	if (v<minV):
+		v=minV
+	if (v>maxV):
+		v=maxV
+	b = (v-minV)/float(maxV-minV)
+	return (1.0-b)
+	
+controllerId = -1
+
+serialSteps = 0
+serialStepsUntilCheckVREvents = 3
+
+
+ser = serial.Serial(port='COM9',baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
+if (ser.isOpen()):
+	while True:
+		events = p.getVREvents()
+		for e in (events):
+			if (e[BUTTONS][33]&p.VR_BUTTON_IS_DOWN):
+				controllerId = e[0]
+			if (e[0] == controllerId):
+				p.changeConstraint(hand_cid,e[POSITION],e[ORIENTATION], maxForce=50)
+			
+		serialSteps = 0
+		while ser.inWaiting() > 0:
+			serialSteps=serialSteps+1
+			if (serialSteps>serialStepsUntilCheckVREvents):
+				ser.flushInput()
+				break
+			line = str(ser.readline())
+			words = line.split(",")
+			if (len(words)==6):
+				middle = convertSensor(words[1])
+				pink = convertSensor(words[2])
+				index = convertSensor(words[3])
+				thumb = convertSensor(words[4])+0.2
+
+				p.setJointMotorControl2(hand,5,p.POSITION_CONTROL,1.3)
+				p.setJointMotorControl2(hand,7,p.POSITION_CONTROL,thumb)
+				p.setJointMotorControl2(hand,9,p.POSITION_CONTROL,thumb)
+				p.setJointMotorControl2(hand,11,p.POSITION_CONTROL,thumb)
+				
+				p.setJointMotorControl2(hand,15,p.POSITION_CONTROL,index)
+				p.setJointMotorControl2(hand,17,p.POSITION_CONTROL,index)
+				p.setJointMotorControl2(hand,19,p.POSITION_CONTROL,index)
+
+				p.setJointMotorControl2(hand,22,p.POSITION_CONTROL,middle)
+				p.setJointMotorControl2(hand,24,p.POSITION_CONTROL,middle)
+				p.setJointMotorControl2(hand,26,p.POSITION_CONTROL,middle)
+				
+				p.setJointMotorControl2(hand,38,p.POSITION_CONTROL,pink)
+				p.setJointMotorControl2(hand,40,p.POSITION_CONTROL,pink)
+				p.setJointMotorControl2(hand,42,p.POSITION_CONTROL,pink)
+
+				ringpos = 0.5*(pink+middle)
+				p.setJointMotorControl2(hand,30,p.POSITION_CONTROL,ringpos)
+				p.setJointMotorControl2(hand,32,p.POSITION_CONTROL,ringpos)
+				p.setJointMotorControl2(hand,34,p.POSITION_CONTROL,ringpos)
+								
+				#print(middle)
+				#print(pink)
+				#print(index)
+				#print(thumb)
--- a/examples/pybullet/examples/vrminitaur.py
+++ b/examples/pybullet/examples/vrminitaur.py
@@ -0,0 +1,49 @@
+#script to track a robot with a VR controller attached to it.
+
+import time
+import pybullet as p
+
+#first try to connect to shared memory (VR), if it fails use local GUI
+c = p.connect(p.SHARED_MEMORY)
+if (c<0):
+	c = p.connect(p.GUI)
+p.resetSimulation()
+p.setGravity(0,0,0)
+print(c)
+if (c<0):
+		p.connect(p.GUI)
+		
+#load the MuJoCo MJCF hand
+minitaur = p.loadURDF("quadruped/minitaur.urdf")
+robot_cid = -1
+
+POSITION=1
+ORIENTATION=2
+BUTTONS=6
+
+p.setRealTimeSimulation(1)
+
+controllerId = -1
+
+while True:
+	events = p.getVREvents()
+	for e in (events):
+		#print (e[BUTTONS])
+		if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED ):
+			if (controllerId >= 0):
+				controllerId = -1
+			else:
+				controllerId = e[0]
+		if (e[0] == controllerId):
+			if (robot_cid>=0):
+				p.changeConstraint(robot_cid,e[POSITION],e[ORIENTATION], maxForce=5000)
+		if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED ):
+			if (robot_cid>=0):
+				p.removeConstraint(robot_cid)
+
+			#q = [0,0,0,1]
+			euler = p.getEulerFromQuaternion(e[ORIENTATION])
+			q = p.getQuaternionFromEuler([euler[0],euler[1],0]) #-euler[0],-euler[1],-euler[2]])
+			robot_cid = p.createConstraint(minitaur,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.1,0,0],e[POSITION],q,e[ORIENTATION])
+
+