* Adjustment: Update Bullet version to 3.24.

Engine/lib/bullet/examples/pybullet/CMakeLists.txt

@@ -0,0 +1,98 @@
+
+INCLUDE_DIRECTORIES( 
+		${BULLET_PHYSICS_SOURCE_DIR}/src
+		${BULLET_PHYSICS_SOURCE_DIR}/examples
+		${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
+		${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include
+                ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src
+		${PYTHON_INCLUDE_DIRS}
+  )
+IF(BUILD_PYBULLET_NUMPY)
+	INCLUDE_DIRECTORIES(
+		${PYTHON_NUMPY_INCLUDE_DIR}
+	)
+ENDIF()
+
+ADD_DEFINITIONS(-DSTATIC_LINK_SPD_PLUGIN )
+
+IF(ENABLE_VHACD)
+ADD_DEFINITIONS(-DBT_ENABLE_VHACD)
+
+INCLUDE_DIRECTORIES( 
+  ../../Extras/VHACD/inc
+  ../../Extras/VHACD/public
+)
+ENDIF(ENABLE_VHACD)
+	
+SET(pybullet_SRCS
+	pybullet.c
+)
+
+IF(BUILD_CLSOCKET)
+ ADD_DEFINITIONS(-DBT_ENABLE_CLSOCKET)
+ENDIF()
+
+IF(WIN32)
+	LINK_LIBRARIES(
+		${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
+	)
+	IF(BUILD_ENET)
+		ADD_DEFINITIONS(-DWIN32 -DBT_ENABLE_ENET)
+	ENDIF()
+	IF(BUILD_CLSOCKET)
+		ADD_DEFINITIONS(-DWIN32)
+	ENDIF()
+
+ELSE(WIN32)
+	IF(BUILD_ENET)
+		ADD_DEFINITIONS(-DHAS_SOCKLEN_T -DBT_ENABLE_ENET)
+	ENDIF()
+
+	IF(BUILD_CLSOCKET)	
+		ADD_DEFINITIONS(${OSDEF})
+	ENDIF()
+ENDIF(WIN32)
+
+
+
+ADD_LIBRARY(pybullet SHARED ${pybullet_SRCS})
+
+SET_TARGET_PROPERTIES(pybullet PROPERTIES PREFIX "")
+SET_TARGET_PROPERTIES(pybullet PROPERTIES POSTFIX "")
+
+SET_TARGET_PROPERTIES(pybullet PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(pybullet PROPERTIES SOVERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(pybullet PROPERTIES DEBUG_POSTFIX "_d")
+
+
+IF(WIN32)
+	IF(BUILD_ENET OR BUILD_CLSOCKET)
+		TARGET_LINK_LIBRARIES(pybullet ws2_32 )
+	ENDIF()
+
+	SET_TARGET_PROPERTIES(pybullet PROPERTIES SUFFIX ".pyd" )
+ENDIF(WIN32)
+
+IF (APPLE)
+  SET_TARGET_PROPERTIES(pybullet PROPERTIES SUFFIX ".so" )
+ENDIF()
+
+
+
+TARGET_LINK_LIBRARIES(pybullet BulletRoboticsGUI BulletExampleBrowserLib BulletRobotics BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common)
+
+IF (WIN32)
+  TARGET_LINK_LIBRARIES(pybullet ${PYTHON_LIBRARIES})
+ELSEIF (APPLE)
+  SET_TARGET_PROPERTIES(pybullet PROPERTIES LINK_FLAGS "-undefined dynamic_lookup")
+ENDIF ()
+# else Linux: dont link
+
+IF(WIN32)
+  SET(PYTHON_SITE_PACKAGES Lib/site-packages CACHE PATH "Python install path")
+ELSE()
+  SET(PYTHON_SITE_PACKAGES lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages CACHE PATH "Python install path")
+ENDIF()
+
+INSTALL(TARGETS pybullet DESTINATION ${PYTHON_SITE_PACKAGES})
+

Engine/lib/bullet/examples/pybullet/examples/__init__.py

@@ -0,0 +1 @@
+

Engine/lib/bullet/examples/pybullet/examples/addPlanarReflection.py

@@ -0,0 +1,106 @@
+import pybullet as p
+import time
+import math
+import pybullet_data
+
+
+
+def getRayFromTo(mouseX, mouseY):
+  width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
+  )
+  camPos = [
+      camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
+      camTarget[2] - dist * camForward[2]
+  ]
+  farPlane = 10000
+  rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
+  invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
+                                      rayForward[1] + rayForward[2] * rayForward[2]))
+  rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
+  rayFrom = camPos
+  oneOverWidth = float(1) / float(width)
+  oneOverHeight = float(1) / float(height)
+  dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
+  dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
+  rayToCenter = [
+      rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
+  ]
+  rayTo = [
+      rayFrom[0] + rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] -
+      float(mouseY) * dVer[0], rayFrom[1] + rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1] +
+      float(mouseX) * dHor[1] - float(mouseY) * dVer[1], rayFrom[2] + rayForward[2] -
+      0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
+  ]
+  return rayFrom, rayTo
+
+
+#cid = p.connect(p.SHARED_MEMORY_GUI)
+cid = p.connect(p.GUI)
+if (cid < 0):
+  p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setPhysicsEngineParameter(numSolverIterations=10)
+p.setTimeStep(1. / 120.)
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
+#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
+p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
+#disable rendering during creation.
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION, 1)
+
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+#disable tinyrenderer, software (CPU) renderer, we don't use it here
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
+
+shift = [0, -0.02, 0]
+meshScale = [0.1, 0.1, 0.1]
+#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
+visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
+                                    fileName="duck.obj",
+                                    rgbaColor=[1, 1, 1, 1],
+                                    specularColor=[0.4, .4, 0],
+                                    visualFramePosition=shift,
+                                    meshScale=meshScale)
+collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
+                                          fileName="duck_vhacd.obj",
+                                          collisionFramePosition=shift,
+                                          meshScale=meshScale)
+
+rangex = 3
+rangey = 3
+for i in range(rangex):
+  for j in range(rangey):
+    p.createMultiBody(baseMass=1,
+                      baseInertialFramePosition=[0, 0, 0],
+                      baseCollisionShapeIndex=collisionShapeId,
+                      baseVisualShapeIndex=visualShapeId,
+                      basePosition=[((-rangex / 2) + i) * meshScale[0] * 2,
+                                    (-rangey / 2 + j) * meshScale[1] * 2, 1],
+                      useMaximalCoordinates=True)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+p.stopStateLogging(logId)
+p.setGravity(0, 0, -10)
+p.setRealTimeSimulation(1)
+
+colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
+currentColor = 0
+
+while (1):
+  p.getDebugVisualizerCamera()
+  mouseEvents = p.getMouseEvents()
+  for e in mouseEvents:
+    if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
+      mouseX = e[1]
+      mouseY = e[2]
+      rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
+      rayInfo = p.rayTest(rayFrom, rayTo)
+      #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
+      for l in range(len(rayInfo)):
+        hit = rayInfo[l]
+        objectUid = hit[0]
+        if (objectUid >= 1):
+          #p.removeBody(objectUid)
+          p.changeVisualShape(objectUid, -1, rgbaColor=colors[currentColor])
+          currentColor += 1
+          if (currentColor >= len(colors)):
+            currentColor = 0

Engine/lib/bullet/examples/pybullet/examples/batchRayCast.py

@@ -0,0 +1,76 @@
+import pybullet as p
+import time
+import math
+import pybullet_data
+
+useGui = True
+
+if (useGui):
+  p.connect(p.GUI)
+else:
+  p.connect(p.DIRECT)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+
+#p.loadURDF("samurai.urdf")
+p.loadURDF("r2d2.urdf", [3, 3, 1])
+
+rayFrom = []
+rayTo = []
+rayIds = []
+
+numRays = 1024
+
+rayLen = 13
+
+rayHitColor = [1, 0, 0]
+rayMissColor = [0, 1, 0]
+
+replaceLines = True
+
+for i in range(numRays):
+  rayFrom.append([0, 0, 1])
+  rayTo.append([
+      rayLen * math.sin(2. * math.pi * float(i) / numRays),
+      rayLen * math.cos(2. * math.pi * float(i) / numRays), 1
+  ])
+  if (replaceLines):
+    rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor))
+  else:
+    rayIds.append(-1)
+
+if (not useGui):
+  timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "rayCastBench.json")
+
+numSteps = 10
+if (useGui):
+  numSteps = 327680
+
+for i in range(numSteps):
+  p.stepSimulation()
+  for j in range(8):
+    results = p.rayTestBatch(rayFrom, rayTo, j + 1)
+
+  #for i in range (10):
+  #	p.removeAllUserDebugItems()
+
+  if (useGui):
+    if (not replaceLines):
+      p.removeAllUserDebugItems()
+
+    for i in range(numRays):
+      hitObjectUid = results[i][0]
+
+      if (hitObjectUid < 0):
+        hitPosition = [0, 0, 0]
+        p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor, replaceItemUniqueId=rayIds[i])
+      else:
+        hitPosition = results[i][3]
+        p.addUserDebugLine(rayFrom[i], hitPosition, rayHitColor, replaceItemUniqueId=rayIds[i])
+
+  #time.sleep(1./240.)
+
+if (not useGui):
+  p.stopStateLogging(timingLog)

Engine/lib/bullet/examples/pybullet/examples/biped2d_pybullet.py

@@ -0,0 +1,151 @@
+import pybullet as p
+import pybullet_data
+import os
+import time
+
+import pybullet as p
+import pybullet_data as pd
+import math
+import time
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pd.getDataPath())
+
+textureId = -1
+
+useProgrammatic = 0
+useTerrainFromPNG = 1
+useDeepLocoCSV = 2
+updateHeightfield = False
+
+heightfieldSource = useProgrammatic
+import random
+random.seed(10)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+heightPerturbationRange = 0.05
+if heightfieldSource==useProgrammatic:
+  numHeightfieldRows = 256
+  numHeightfieldColumns = 256
+  heightfieldData = [0]*numHeightfieldRows*numHeightfieldColumns 
+  for j in range (int(numHeightfieldColumns/2)):
+    for i in range (int(numHeightfieldRows/2) ):
+      height = random.uniform(0,heightPerturbationRange)
+      heightfieldData[2*i+2*j*numHeightfieldRows]=height
+      heightfieldData[2*i+1+2*j*numHeightfieldRows]=height
+      heightfieldData[2*i+(2*j+1)*numHeightfieldRows]=height
+      heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows]=height
+      
+      
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.05,.05,1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns)
+  terrain  = p.createMultiBody(0, terrainShape)
+  p.resetBasePositionAndOrientation(terrain,[0,0,0], [0,0,0,1])
+
+if heightfieldSource==useDeepLocoCSV:
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.5,.5,2.5],fileName = "heightmaps/ground0.txt", heightfieldTextureScaling=128)
+  terrain  = p.createMultiBody(0, terrainShape)
+  p.resetBasePositionAndOrientation(terrain,[0,0,0], [0,0,0,1])
+
+if heightfieldSource==useTerrainFromPNG:
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.1,.1,24],fileName = "heightmaps/wm_height_out.png")
+  textureId = p.loadTexture("heightmaps/gimp_overlay_out.png")
+  terrain  = p.createMultiBody(0, terrainShape)
+  p.changeVisualShape(terrain, -1, textureUniqueId = textureId)
+ 
+ 
+p.changeVisualShape(terrain, -1, rgbaColor=[1,1,1,1])
+
+
+sphereRadius = 0.05
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+                                  halfExtents=[sphereRadius, sphereRadius, sphereRadius])
+
+mass = 1
+visualShapeId = -1
+
+link_Masses = [1]
+linkCollisionShapeIndices = [colBoxId]
+linkVisualShapeIndices = [-1]
+linkPositions = [[0, 0, 0.11]]
+linkOrientations = [[0, 0, 0, 1]]
+linkInertialFramePositions = [[0, 0, 0]]
+linkInertialFrameOrientations = [[0, 0, 0, 1]]
+indices = [0]
+jointTypes = [p.JOINT_REVOLUTE]
+axis = [[0, 0, 1]]
+
+for i in range(3):
+  for j in range(3):
+    for k in range(3):
+      basePosition = [
+          i * 5 * sphereRadius, j * 5 * sphereRadius, 1 + k * 5 * sphereRadius + 1
+      ]
+      baseOrientation = [0, 0, 0, 1]
+      if (k & 2):
+        sphereUid = p.createMultiBody(mass, colSphereId, visualShapeId, basePosition,
+                                      baseOrientation)
+      else:
+        sphereUid = p.createMultiBody(mass,
+                                      colBoxId,
+                                      visualShapeId,
+                                      basePosition,
+                                      baseOrientation,
+                                      linkMasses=link_Masses,
+                                      linkCollisionShapeIndices=linkCollisionShapeIndices,
+                                      linkVisualShapeIndices=linkVisualShapeIndices,
+                                      linkPositions=linkPositions,
+                                      linkOrientations=linkOrientations,
+                                      linkInertialFramePositions=linkInertialFramePositions,
+                                      linkInertialFrameOrientations=linkInertialFrameOrientations,
+                                      linkParentIndices=indices,
+                                      linkJointTypes=jointTypes,
+                                      linkJointAxis=axis)
+
+      
+      p.changeDynamics(sphereUid,
+                       -1,
+                       spinningFriction=0.001,
+                       rollingFriction=0.001,
+                       linearDamping=0.0)
+      for joint in range(p.getNumJoints(sphereUid)):
+        p.setJointMotorControl2(sphereUid, joint, p.VELOCITY_CONTROL, targetVelocity=1, force=10)
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
+
+GRAVITY = -9.8
+dt = 1e-3
+iters = 2000
+import pybullet_data
+
+#physicsClient = p.connect(p.GUI)
+
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+#p.resetSimulation()
+#p.setRealTimeSimulation(True)
+p.setGravity(0, 0, GRAVITY)
+p.setTimeStep(dt)
+#planeId = p.loadURDF("plane.urdf")
+cubeStartPos = [0, 0, 1.13]
+cubeStartOrientation = p.getQuaternionFromEuler([0., 0, 0])
+botId = p.loadURDF("biped/biped2d_pybullet.urdf", cubeStartPos, cubeStartOrientation)
+
+#disable the default velocity motors
+#and set some position control with small force to emulate joint friction/return to a rest pose
+jointFrictionForce = 1
+for joint in range(p.getNumJoints(botId)):
+  p.setJointMotorControl2(botId, joint, p.POSITION_CONTROL, force=jointFrictionForce)
+
+#for i in range(10000):
+#     p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
+#     p.stepSimulation()
+#import ipdb
+#ipdb.set_trace()
+import time
+p.setRealTimeSimulation(1)
+while (1):
+  #p.stepSimulation()
+  #p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
+  p.setGravity(0, 0, GRAVITY)
+  time.sleep(1 / 240.)
+time.sleep(1000)

Engine/lib/bullet/examples/pybullet/examples/changeDynamicsMass.py

@@ -0,0 +1,17 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+cube2 = p.loadURDF("cube.urdf", [0, 0, 3], useFixedBase=True)
+cube = p.loadURDF("cube.urdf", useFixedBase=True)
+p.setGravity(0, 0, -10)
+timeStep = 1. / 240.
+p.setTimeStep(timeStep)
+p.changeDynamics(cube2, -1, mass=1)
+#now cube2 will have a floating base and move
+
+while (p.isConnected()):
+  p.stepSimulation()
+  time.sleep(timeStep)

Engine/lib/bullet/examples/pybullet/examples/changeTexture.py

@@ -0,0 +1,44 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+planeUidA = p.loadURDF("plane_transparent.urdf", [0, 0, 0])
+planeUid = p.loadURDF("plane_transparent.urdf", [0, 0, -1])
+
+texUid = p.loadTexture("tex256.png")
+
+p.changeVisualShape(planeUidA, -1, rgbaColor=[1, 1, 1, 0.5])
+p.changeVisualShape(planeUid, -1, rgbaColor=[1, 1, 1, 0.5])
+p.changeVisualShape(planeUid, -1, textureUniqueId=texUid)
+
+width = 256
+height = 256
+pixels = [255] * width * height * 3
+colorR = 0
+colorG = 0
+colorB = 0
+
+#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+#p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
+
+blue = 0
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "renderbench.json")
+for i in range(100000):
+  p.stepSimulation()
+  for i in range(width):
+    for j in range(height):
+      pixels[(i + j * width) * 3 + 0] = i
+      pixels[(i + j * width) * 3 + 1] = (j + blue) % 256
+      pixels[(i + j * width) * 3 + 2] = blue
+  blue = blue + 1
+  p.changeTexture(texUid, pixels, width, height)
+  start = time.time()
+  p.getCameraImage(300, 300, renderer=p.ER_BULLET_HARDWARE_OPENGL)
+  end = time.time()
+  print("rendering duration")
+  print(end - start)
+p.stopStateLogging(logId)
+#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
+#p.configureDebugVisualizer(p.COV_ENABLE_GUI,1)

Engine/lib/bullet/examples/pybullet/examples/collisionFilter.py

@@ -0,0 +1,21 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+planeId = p.loadURDF("plane.urdf", useMaximalCoordinates=False)
+cubeId = p.loadURDF("cube_collisionfilter.urdf", [0, 0, 3], useMaximalCoordinates=False)
+
+collisionFilterGroup = 0
+collisionFilterMask = 0
+p.setCollisionFilterGroupMask(cubeId, -1, collisionFilterGroup, collisionFilterMask)
+
+enableCollision = 1
+p.setCollisionFilterPair(planeId, cubeId, -1, -1, enableCollision)
+
+p.setRealTimeSimulation(1)
+p.setGravity(0, 0, -10)
+while (p.isConnected()):
+  time.sleep(1. / 240.)
+  p.setGravity(0, 0, -10)

Engine/lib/bullet/examples/pybullet/examples/commandLogAndPlayback.py

@@ -0,0 +1,16 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+logId = p.startStateLogging(p.STATE_LOGGING_ALL_COMMANDS, "commandLog.bin")
+p.loadURDF("plane.urdf")
+p.loadURDF("r2d2.urdf", [0, 0, 1])
+
+p.stopStateLogging(logId)
+p.resetSimulation()
+
+logId = p.startStateLogging(p.STATE_REPLAY_ALL_COMMANDS, "commandLog.bin")
+while (p.isConnected()):
+  time.sleep(1. / 240.)

Engine/lib/bullet/examples/pybullet/examples/configureDebugVisualizer.py

@@ -0,0 +1,23 @@
+import pybullet as p
+import math
+import time
+dt = 1./240.
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF("r2d2.urdf",[0,0,1])
+p.loadURDF("plane.urdf")
+p.setGravity(0,0,-10)
+
+radius=5
+t = 0
+p.configureDebugVisualizer(shadowMapWorldSize=5)
+p.configureDebugVisualizer(shadowMapResolution=8192)
+
+while (1):
+  t+=dt
+  p.configureDebugVisualizer(lightPosition=[radius*math.sin(t),radius*math.cos(t),3])
+  
+  p.stepSimulation()
+  time.sleep(dt)

Engine/lib/bullet/examples/pybullet/examples/constraint.py

@@ -0,0 +1,27 @@
+import pybullet as p
+import time
+import math
+
+import pybullet_data
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+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])
+print(cid)
+print(p.getConstraintUniqueId(0))
+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)

Engine/lib/bullet/examples/pybullet/examples/contactFriction.py

@@ -0,0 +1,32 @@
+import pybullet as p
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+useMaximalCoordinates = False
+p.loadURDF("plane.urdf", useMaximalCoordinates=useMaximalCoordinates)
+#p.loadURDF("sphere2.urdf",[0,0,1])
+p.loadURDF("cube.urdf", [0, 0, 1], useMaximalCoordinates=useMaximalCoordinates)
+p.setGravity(0, 3, -10)
+while (1):
+  p.stepSimulation()
+  pts = p.getContactPoints()
+
+  print("num pts=", len(pts))
+  totalNormalForce = 0
+  totalFrictionForce = [0, 0, 0]
+  totalLateralFrictionForce = [0, 0, 0]
+  for pt in pts:
+    #print("pt.normal=",pt[7])
+    #print("pt.normalForce=",pt[9])
+    totalNormalForce += pt[9]
+    #print("pt.lateralFrictionA=",pt[10])
+    #print("pt.lateralFrictionADir=",pt[11])
+    #print("pt.lateralFrictionB=",pt[12])
+    #print("pt.lateralFrictionBDir=",pt[13])
+    totalLateralFrictionForce[0] += pt[11][0] * pt[10] + pt[13][0] * pt[12]
+    totalLateralFrictionForce[1] += pt[11][1] * pt[10] + pt[13][1] * pt[12]
+    totalLateralFrictionForce[2] += pt[11][2] * pt[10] + pt[13][2] * pt[12]
+
+  print("totalNormalForce=", totalNormalForce)
+  print("totalLateralFrictionForce=", totalLateralFrictionForce)

Engine/lib/bullet/examples/pybullet/examples/createMesh.py

@@ -0,0 +1,138 @@
+import pybullet as p
+import time
+import math
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+#don't create a ground plane, to allow for gaps etc
+p.resetSimulation()
+#p.createCollisionShape(p.GEOM_PLANE)
+#p.createMultiBody(0,0)
+#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
+p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+
+sphereRadius = 0.05
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
+
+#a few different ways to create a mesh:
+
+vertices = [[-0.246350, -0.246483, -0.000624], [-0.151407, -0.176325, 0.172867],
+            [-0.246350, 0.249205, -0.000624], [-0.151407, 0.129477, 0.172867],
+            [0.249338, -0.246483, -0.000624], [0.154395, -0.176325, 0.172867],
+            [0.249338, 0.249205, -0.000624], [0.154395, 0.129477, 0.172867]]
+indices = [
+    0, 3, 2, 3, 6, 2, 7, 4, 6, 5, 0, 4, 6, 0, 2, 3, 5, 7, 0, 1, 3, 3, 7, 6, 7, 5, 4, 5, 1, 0, 6, 4,
+    0, 3, 1, 5
+]
+#convex mesh from obj
+stoneId = p.createCollisionShape(p.GEOM_MESH, vertices=vertices, indices=indices)
+
+boxHalfLength = 0.5
+boxHalfWidth = 2.5
+boxHalfHeight = 0.1
+segmentLength = 5
+
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+                                  halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
+
+mass = 1
+visualShapeId = -1
+
+segmentStart = 0
+
+for i in range(segmentLength):
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1])
+  segmentStart = segmentStart - 1
+
+for i in range(segmentLength):
+  height = 0
+  if (i % 2):
+    height = 1
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1 + height])
+  segmentStart = segmentStart - 1
+
+baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
+
+for i in range(segmentLength):
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1])
+  segmentStart = segmentStart - 1
+  if (i % 2):
+    p.createMultiBody(baseMass=0,
+                      baseCollisionShapeIndex=colBoxId,
+                      basePosition=[segmentStart, i % 3, -0.1 + height + boxHalfWidth],
+                      baseOrientation=baseOrientation)
+
+for i in range(segmentLength):
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1])
+  width = 4
+  for j in range(width):
+    p.createMultiBody(baseMass=0,
+                      baseCollisionShapeIndex=stoneId,
+                      basePosition=[segmentStart, 0.5 * (i % 2) + j - width / 2., 0])
+  segmentStart = segmentStart - 1
+
+link_Masses = [1]
+linkCollisionShapeIndices = [colBoxId]
+linkVisualShapeIndices = [-1]
+linkPositions = [[0, 0, 0]]
+linkOrientations = [[0, 0, 0, 1]]
+linkInertialFramePositions = [[0, 0, 0]]
+linkInertialFrameOrientations = [[0, 0, 0, 1]]
+indices = [0]
+jointTypes = [p.JOINT_REVOLUTE]
+axis = [[1, 0, 0]]
+
+baseOrientation = [0, 0, 0, 1]
+for i in range(segmentLength):
+  boxId = p.createMultiBody(0,
+                            colSphereId,
+                            -1, [segmentStart, 0, -0.1],
+                            baseOrientation,
+                            linkMasses=link_Masses,
+                            linkCollisionShapeIndices=linkCollisionShapeIndices,
+                            linkVisualShapeIndices=linkVisualShapeIndices,
+                            linkPositions=linkPositions,
+                            linkOrientations=linkOrientations,
+                            linkInertialFramePositions=linkInertialFramePositions,
+                            linkInertialFrameOrientations=linkInertialFrameOrientations,
+                            linkParentIndices=indices,
+                            linkJointTypes=jointTypes,
+                            linkJointAxis=axis)
+  p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
+  print(p.getNumJoints(boxId))
+  for joint in range(p.getNumJoints(boxId)):
+    targetVelocity = 10
+    if (i % 2):
+      targetVelocity = -10
+    p.setJointMotorControl2(boxId,
+                            joint,
+                            p.VELOCITY_CONTROL,
+                            targetVelocity=targetVelocity,
+                            force=100)
+  segmentStart = segmentStart - 1.1
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+while (1):
+  camData = p.getDebugVisualizerCamera()
+  viewMat = camData[2]
+  projMat = camData[3]
+  p.getCameraImage(256,
+                   256,
+                   viewMatrix=viewMat,
+                   projectionMatrix=projMat,
+                   renderer=p.ER_BULLET_HARDWARE_OPENGL)
+  keys = p.getKeyboardEvents()
+  p.stepSimulation()
+  #print(keys)
+  time.sleep(0.01)

Engine/lib/bullet/examples/pybullet/examples/createMultiBodyBatch.py

@@ -0,0 +1,143 @@
+import pybullet as p
+import time
+import math
+import pybullet_data
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+  p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
+p.setAdditionalSearchPath(pybullet_data.getDataPath())  
+
+p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
+p.setTimeStep(1. / 120.)
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json")
+#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
+p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
+#disable rendering during creation.
+p.setPhysicsEngineParameter(contactBreakingThreshold=0.04)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+#disable tinyrenderer, software (CPU) renderer, we don't use it here
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
+
+shift = [0, -0.02, 0]
+meshScale = [0.1, 0.1, 0.1]
+
+vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
+            [1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
+            [-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
+            [1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
+            [-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
+            [-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000],
+            [1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000],
+            [1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
+            [-1.000000, -1.000000, -1.000000], [-1.000000, -1.000000, 1.000000],
+            [1.000000, -1.000000, 1.000000], [1.000000, -1.000000, -1.000000],
+            [-1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, 1.000000],
+            [1.000000, 1.000000, 1.000000], [1.000000, 1.000000, -1.000000]]
+
+normals = [[0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
+           [0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
+           [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
+           [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
+           [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
+           [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
+           [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
+           [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
+           [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
+           [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
+           [0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000],
+           [0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000]]
+
+uvs = [[0.750000, 0.250000], [1.000000, 0.250000], [1.000000, 0.000000], [0.750000, 0.000000],
+       [0.500000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000], [0.500000, 0.000000],
+       [0.500000, 0.000000], [0.750000, 0.000000], [0.750000, 0.250000], [0.500000, 0.250000],
+       [0.250000, 0.500000], [0.250000, 0.250000], [0.000000, 0.250000], [0.000000, 0.500000],
+       [0.250000, 0.500000], [0.250000, 0.250000], [0.500000, 0.250000], [0.500000, 0.500000],
+       [0.000000, 0.000000], [0.000000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000]]
+indices = [
+    0,
+    1,
+    2,
+    0,
+    2,
+    3,  #//ground face
+    6,
+    5,
+    4,
+    7,
+    6,
+    4,  #//top face
+    10,
+    9,
+    8,
+    11,
+    10,
+    8,
+    12,
+    13,
+    14,
+    12,
+    14,
+    15,
+    18,
+    17,
+    16,
+    19,
+    18,
+    16,
+    20,
+    21,
+    22,
+    20,
+    22,
+    23
+]
+
+#p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
+#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
+visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
+                                    rgbaColor=[1, 1, 1, 1],
+                                    specularColor=[0.4, .4, 0],
+                                    visualFramePosition=shift,
+                                    meshScale=meshScale,
+                                    vertices=vertices,
+                                    indices=indices,
+                                    uvs=uvs,
+                                    normals=normals)
+collisionShapeId = p.createCollisionShape(
+    shapeType=p.GEOM_BOX, halfExtents=meshScale
+)  #MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
+
+texUid = p.loadTexture("tex256.png")
+
+batchPositions = []
+
+for x in range(32):
+  for y in range(32):
+    for z in range(10):
+      batchPositions.append(
+          [x * meshScale[0] * 5.5, y * meshScale[1] * 5.5, (0.5 + z) * meshScale[2] * 2.5])
+
+bodyUids = p.createMultiBody(baseMass=0,
+                            baseInertialFramePosition=[0, 0, 0],
+                            baseCollisionShapeIndex=collisionShapeId,
+                            baseVisualShapeIndex=visualShapeId,
+                            basePosition=[0, 0, 2],
+                            batchPositions=batchPositions,
+                            useMaximalCoordinates=True)
+p.changeVisualShape(bodyUids[0], -1, textureUniqueId=texUid)
+
+p.syncBodyInfo()
+print("numBodies=", p.getNumBodies())
+p.stopStateLogging(logId)
+p.setGravity(0, 0, -10)
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+
+colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
+currentColor = 0
+
+while (1):
+  p.stepSimulation()
+  #time.sleep(1./120.)
+  #p.getCameraImage(320,200)

Engine/lib/bullet/examples/pybullet/examples/createMultiBodyLinks.py

@@ -0,0 +1,75 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.createCollisionShape(p.GEOM_PLANE)
+p.createMultiBody(0, 0)
+
+sphereRadius = 0.05
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+                                  halfExtents=[sphereRadius, sphereRadius, sphereRadius])
+
+mass = 1
+visualShapeId = -1
+
+link_Masses = [1]
+linkCollisionShapeIndices = [colBoxId]
+linkVisualShapeIndices = [-1]
+linkPositions = [[0, 0, 0.11]]
+linkOrientations = [[0, 0, 0, 1]]
+linkInertialFramePositions = [[0, 0, 0]]
+linkInertialFrameOrientations = [[0, 0, 0, 1]]
+indices = [0]
+jointTypes = [p.JOINT_REVOLUTE]
+axis = [[0, 0, 1]]
+
+for i in range(3):
+  for j in range(3):
+    for k in range(3):
+      basePosition = [
+          1 + i * 5 * sphereRadius, 1 + j * 5 * sphereRadius, 1 + k * 5 * sphereRadius + 1
+      ]
+      baseOrientation = [0, 0, 0, 1]
+      if (k & 2):
+        sphereUid = p.createMultiBody(mass, colSphereId, visualShapeId, basePosition,
+                                      baseOrientation)
+      else:
+        sphereUid = p.createMultiBody(mass,
+                                      colBoxId,
+                                      visualShapeId,
+                                      basePosition,
+                                      baseOrientation,
+                                      linkMasses=link_Masses,
+                                      linkCollisionShapeIndices=linkCollisionShapeIndices,
+                                      linkVisualShapeIndices=linkVisualShapeIndices,
+                                      linkPositions=linkPositions,
+                                      linkOrientations=linkOrientations,
+                                      linkInertialFramePositions=linkInertialFramePositions,
+                                      linkInertialFrameOrientations=linkInertialFrameOrientations,
+                                      linkParentIndices=indices,
+                                      linkJointTypes=jointTypes,
+                                      linkJointAxis=axis)
+
+      p.changeDynamics(sphereUid,
+                       -1,
+                       spinningFriction=0.001,
+                       rollingFriction=0.001,
+                       linearDamping=0.0)
+      for joint in range(p.getNumJoints(sphereUid)):
+        p.setJointMotorControl2(sphereUid, joint, p.VELOCITY_CONTROL, targetVelocity=1, force=10)
+
+p.setGravity(0, 0, -10)
+p.setRealTimeSimulation(1)
+
+p.getNumJoints(sphereUid)
+for i in range(p.getNumJoints(sphereUid)):
+  p.getJointInfo(sphereUid, i)
+
+while (1):
+  keys = p.getKeyboardEvents()
+  print(keys)
+
+  time.sleep(0.01)

Engine/lib/bullet/examples/pybullet/examples/createObstacleCourse.py

@@ -0,0 +1,130 @@
+import pybullet as p
+import time
+import math
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+#don't create a ground plane, to allow for gaps etc
+p.resetSimulation()
+#p.createCollisionShape(p.GEOM_PLANE)
+#p.createMultiBody(0,0)
+#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
+p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+
+sphereRadius = 0.05
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
+
+#a few different ways to create a mesh:
+
+#convex mesh from obj
+stoneId = p.createCollisionShape(p.GEOM_MESH, fileName="stone.obj")
+
+boxHalfLength = 0.5
+boxHalfWidth = 2.5
+boxHalfHeight = 0.1
+segmentLength = 5
+
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+                                  halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
+
+mass = 1
+visualShapeId = -1
+
+segmentStart = 0
+
+for i in range(segmentLength):
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1])
+  segmentStart = segmentStart - 1
+
+for i in range(segmentLength):
+  height = 0
+  if (i % 2):
+    height = 1
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1 + height])
+  segmentStart = segmentStart - 1
+
+baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
+
+for i in range(segmentLength):
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1])
+  segmentStart = segmentStart - 1
+  if (i % 2):
+    p.createMultiBody(baseMass=0,
+                      baseCollisionShapeIndex=colBoxId,
+                      basePosition=[segmentStart, i % 3, -0.1 + height + boxHalfWidth],
+                      baseOrientation=baseOrientation)
+
+for i in range(segmentLength):
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1])
+  width = 4
+  for j in range(width):
+    p.createMultiBody(baseMass=0,
+                      baseCollisionShapeIndex=stoneId,
+                      basePosition=[segmentStart, 0.5 * (i % 2) + j - width / 2., 0])
+  segmentStart = segmentStart - 1
+
+link_Masses = [1]
+linkCollisionShapeIndices = [colBoxId]
+linkVisualShapeIndices = [-1]
+linkPositions = [[0, 0, 0]]
+linkOrientations = [[0, 0, 0, 1]]
+linkInertialFramePositions = [[0, 0, 0]]
+linkInertialFrameOrientations = [[0, 0, 0, 1]]
+indices = [0]
+jointTypes = [p.JOINT_REVOLUTE]
+axis = [[1, 0, 0]]
+
+baseOrientation = [0, 0, 0, 1]
+for i in range(segmentLength):
+  boxId = p.createMultiBody(0,
+                            colSphereId,
+                            -1, [segmentStart, 0, -0.1],
+                            baseOrientation,
+                            linkMasses=link_Masses,
+                            linkCollisionShapeIndices=linkCollisionShapeIndices,
+                            linkVisualShapeIndices=linkVisualShapeIndices,
+                            linkPositions=linkPositions,
+                            linkOrientations=linkOrientations,
+                            linkInertialFramePositions=linkInertialFramePositions,
+                            linkInertialFrameOrientations=linkInertialFrameOrientations,
+                            linkParentIndices=indices,
+                            linkJointTypes=jointTypes,
+                            linkJointAxis=axis)
+  p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
+  print(p.getNumJoints(boxId))
+  for joint in range(p.getNumJoints(boxId)):
+    targetVelocity = 10
+    if (i % 2):
+      targetVelocity = -10
+    p.setJointMotorControl2(boxId,
+                            joint,
+                            p.VELOCITY_CONTROL,
+                            targetVelocity=targetVelocity,
+                            force=100)
+  segmentStart = segmentStart - 1.1
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+while (1):
+  camData = p.getDebugVisualizerCamera()
+  viewMat = camData[2]
+  projMat = camData[3]
+  p.getCameraImage(256,
+                   256,
+                   viewMatrix=viewMat,
+                   projectionMatrix=projMat,
+                   renderer=p.ER_BULLET_HARDWARE_OPENGL)
+  keys = p.getKeyboardEvents()
+  p.stepSimulation()
+  #print(keys)
+  time.sleep(0.01)

Engine/lib/bullet/examples/pybullet/examples/createSphereMultiBodies.py

@@ -0,0 +1,51 @@
+import pybullet as p
+import time
+
+useMaximalCoordinates = 0
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+#p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
+monastryId = concaveEnv = p.createCollisionShape(p.GEOM_MESH,
+                                                 fileName="samurai_monastry.obj",
+                                                 flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
+orn = p.getQuaternionFromEuler([1.5707963, 0, 0])
+p.createMultiBody(0, monastryId, baseOrientation=orn)
+
+sphereRadius = 0.05
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+                                  halfExtents=[sphereRadius, sphereRadius, sphereRadius])
+
+mass = 1
+visualShapeId = -1
+
+for i in range(5):
+  for j in range(5):
+    for k in range(5):
+      if (k & 2):
+        sphereUid = p.createMultiBody(
+            mass,
+            colSphereId,
+            visualShapeId, [-i * 2 * sphereRadius, j * 2 * sphereRadius, k * 2 * sphereRadius + 1],
+            useMaximalCoordinates=useMaximalCoordinates)
+      else:
+        sphereUid = p.createMultiBody(
+            mass,
+            colBoxId,
+            visualShapeId, [-i * 2 * sphereRadius, j * 2 * sphereRadius, k * 2 * sphereRadius + 1],
+            useMaximalCoordinates=useMaximalCoordinates)
+      p.changeDynamics(sphereUid,
+                       -1,
+                       spinningFriction=0.001,
+                       rollingFriction=0.001,
+                       linearDamping=0.0)
+
+p.setGravity(0, 0, -10)
+p.setRealTimeSimulation(1)
+
+while (1):
+  keys = p.getKeyboardEvents()
+  #print(keys)
+  time.sleep(0.01)

Engine/lib/bullet/examples/pybullet/examples/createTexturedMeshVisualShape.py

@@ -0,0 +1,183 @@
+import pybullet as p
+import time
+import math
+import pybullet_data
+
+
+def getRayFromTo(mouseX, mouseY):
+  width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
+  )
+  camPos = [
+      camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
+      camTarget[2] - dist * camForward[2]
+  ]
+  farPlane = 10000
+  rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
+  invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
+                                      rayForward[1] + rayForward[2] * rayForward[2]))
+  rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
+  rayFrom = camPos
+  oneOverWidth = float(1) / float(width)
+  oneOverHeight = float(1) / float(height)
+  dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
+  dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
+  rayToCenter = [
+      rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
+  ]
+  rayTo = [
+      rayFrom[0] + rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] -
+      float(mouseY) * dVer[0], rayFrom[1] + rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1] +
+      float(mouseX) * dHor[1] - float(mouseY) * dVer[1], rayFrom[2] + rayForward[2] -
+      0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
+  ]
+  return rayFrom, rayTo
+
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+  p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())  
+p.setPhysicsEngineParameter(numSolverIterations=10)
+p.setTimeStep(1. / 120.)
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
+#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
+p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
+#disable rendering during creation.
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+#disable tinyrenderer, software (CPU) renderer, we don't use it here
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
+
+shift = [0, -0.02, 0]
+meshScale = [0.1, 0.1, 0.1]
+
+vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
+            [1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
+            [-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
+            [1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
+            [-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
+            [-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000],
+            [1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000],
+            [1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
+            [-1.000000, -1.000000, -1.000000], [-1.000000, -1.000000, 1.000000],
+            [1.000000, -1.000000, 1.000000], [1.000000, -1.000000, -1.000000],
+            [-1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, 1.000000],
+            [1.000000, 1.000000, 1.000000], [1.000000, 1.000000, -1.000000]]
+
+normals = [[0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
+           [0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
+           [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
+           [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
+           [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
+           [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
+           [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
+           [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
+           [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
+           [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
+           [0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000],
+           [0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000]]
+
+uvs = [[0.750000, 0.250000], [1.000000, 0.250000], [1.000000, 0.000000], [0.750000, 0.000000],
+       [0.500000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000], [0.500000, 0.000000],
+       [0.500000, 0.000000], [0.750000, 0.000000], [0.750000, 0.250000], [0.500000, 0.250000],
+       [0.250000, 0.500000], [0.250000, 0.250000], [0.000000, 0.250000], [0.000000, 0.500000],
+       [0.250000, 0.500000], [0.250000, 0.250000], [0.500000, 0.250000], [0.500000, 0.500000],
+       [0.000000, 0.000000], [0.000000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000]]
+indices = [
+    0,
+    1,
+    2,
+    0,
+    2,
+    3,  #//ground face
+    6,
+    5,
+    4,
+    7,
+    6,
+    4,  #//top face
+    10,
+    9,
+    8,
+    11,
+    10,
+    8,
+    12,
+    13,
+    14,
+    12,
+    14,
+    15,
+    18,
+    17,
+    16,
+    19,
+    18,
+    16,
+    20,
+    21,
+    22,
+    20,
+    22,
+    23
+]
+
+#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
+visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
+                                    rgbaColor=[1, 1, 1, 1],
+                                    specularColor=[0.4, .4, 0],
+                                    visualFramePosition=shift,
+                                    meshScale=meshScale,
+                                    vertices=vertices,
+                                    indices=indices,
+                                    uvs=uvs,
+                                    normals=normals)
+#visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices)
+
+#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj")
+collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
+                                          vertices=vertices,
+                                          collisionFramePosition=shift,
+                                          meshScale=meshScale)
+
+texUid = p.loadTexture("tex256.png")
+
+rangex = 1
+rangey = 1
+for i in range(rangex):
+  for j in range(rangey):
+    bodyUid = p.createMultiBody(baseMass=1,
+                                baseInertialFramePosition=[0, 0, 0],
+                                baseCollisionShapeIndex=collisionShapeId,
+                                baseVisualShapeIndex=visualShapeId,
+                                basePosition=[((-rangex / 2) + i) * meshScale[0] * 2,
+                                              (-rangey / 2 + j) * meshScale[1] * 2, 1],
+                                useMaximalCoordinates=True)
+    p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+p.stopStateLogging(logId)
+p.setGravity(0, 0, -10)
+p.setRealTimeSimulation(1)
+
+colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
+currentColor = 0
+
+while (1):
+  p.getCameraImage(320, 200)
+  mouseEvents = p.getMouseEvents()
+  for e in mouseEvents:
+    if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
+      mouseX = e[1]
+      mouseY = e[2]
+      rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
+      rayInfo = p.rayTest(rayFrom, rayTo)
+      #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
+      for l in range(len(rayInfo)):
+        hit = rayInfo[l]
+        objectUid = hit[0]
+        if (objectUid >= 1):
+          #p.removeBody(objectUid)
+          p.changeVisualShape(objectUid, -1, rgbaColor=colors[currentColor])
+          currentColor += 1
+          if (currentColor >= len(colors)):
+            currentColor = 0

Engine/lib/bullet/examples/pybullet/examples/createVisualShape.py

@@ -0,0 +1,58 @@
+import pybullet as p
+import time
+import math
+import pybullet_data
+
+
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+  p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setPhysicsEngineParameter(numSolverIterations=10)
+p.setTimeStep(1. / 120.)
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
+#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
+p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
+#disable rendering during creation.
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+#disable tinyrenderer, software (CPU) renderer, we don't use it here
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
+
+shift = [0, -0.02, 0]
+meshScale = [0.1, 0.1, 0.1]
+#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
+visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
+                                    fileName="duck.obj",
+                                    rgbaColor=[1, 1, 1, 1],
+                                    specularColor=[0.4, .4, 0],
+                                    visualFramePosition=shift,
+                                    meshScale=meshScale)
+collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
+                                          fileName="duck_vhacd.obj",
+                                          collisionFramePosition=shift,
+                                          meshScale=meshScale)
+
+rangex = 1
+rangey = 1
+for i in range(rangex):
+  for j in range(rangey):
+    p.createMultiBody(baseMass=1,
+                      baseInertialFramePosition=[0, 0, 0],
+                      baseCollisionShapeIndex=collisionShapeId,
+                      baseVisualShapeIndex=visualShapeId,
+                      basePosition=[((-rangex / 2) + i) * meshScale[0] * 2,
+                                    (-rangey / 2 + j) * meshScale[1] * 2, 1],
+                      useMaximalCoordinates=True)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+p.stopStateLogging(logId)
+p.setGravity(0, 0, -10)
+p.setRealTimeSimulation(1)
+
+colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
+currentColor = 0
+
+while (1):
+  time.sleep(1./240.)

Engine/lib/bullet/examples/pybullet/examples/createVisualShapeArray.py

@@ -0,0 +1,115 @@
+import pybullet as p
+import time
+import math
+import pybullet_data
+
+
+def getRayFromTo(mouseX, mouseY):
+  width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
+  )
+  camPos = [
+      camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
+      camTarget[2] - dist * camForward[2]
+  ]
+  farPlane = 10000
+  rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
+  invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
+                                      rayForward[1] + rayForward[2] * rayForward[2]))
+  rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
+  rayFrom = camPos
+  oneOverWidth = float(1) / float(width)
+  oneOverHeight = float(1) / float(height)
+  dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
+  dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
+  rayToCenter = [
+      rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
+  ]
+  rayTo = [
+      rayFrom[0] + rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] -
+      float(mouseY) * dVer[0], rayFrom[1] + rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1] +
+      float(mouseX) * dHor[1] - float(mouseY) * dVer[1], rayFrom[2] + rayForward[2] -
+      0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
+  ]
+  return rayFrom, rayTo
+
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+  p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())  
+p.setPhysicsEngineParameter(numSolverIterations=10)
+p.setTimeStep(1. / 120.)
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
+#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
+p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
+#disable rendering during creation.
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+#disable tinyrenderer, software (CPU) renderer, we don't use it here
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
+
+shift = [0, -0.02, 0]
+shift1 = [0, 0.1, 0]
+shift2 = [0, 0, 0]
+
+meshScale = [0.1, 0.1, 0.1]
+#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
+visualShapeId = p.createVisualShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX],
+                                         halfExtents=[[0, 0, 0], [0.1, 0.1, 0.1]],
+                                         fileNames=["duck.obj", ""],
+                                         visualFramePositions=[
+                                             shift1,
+                                             shift2,
+                                         ],
+                                         meshScales=[meshScale, meshScale])
+collisionShapeId = p.createCollisionShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX],
+                                               halfExtents=[[0, 0, 0], [0.1, 0.1, 0.1]],
+                                               fileNames=["duck_vhacd.obj", ""],
+                                               collisionFramePositions=[
+                                                   shift1,
+                                                   shift2,
+                                               ],
+                                               meshScales=[meshScale, meshScale])
+
+rangex = 2
+rangey = 2
+for i in range(rangex):
+  for j in range(rangey):
+    mb = p.createMultiBody(baseMass=1,
+                           baseInertialFramePosition=[0, 0, 0],
+                           baseCollisionShapeIndex=collisionShapeId,
+                           baseVisualShapeIndex=visualShapeId,
+                           basePosition=[((-rangex / 2) + i * 2) * meshScale[0] * 2,
+                                         (-rangey / 2 + j) * meshScale[1] * 4, 1],
+                           useMaximalCoordinates=False)
+    p.changeVisualShape(mb, -1, rgbaColor=[1, 1, 1, 1])
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+p.stopStateLogging(logId)
+p.setGravity(0, 0, -10)
+p.setRealTimeSimulation(1)
+
+colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
+currentColor = 0
+
+p.getCameraImage(64, 64, renderer=p.ER_BULLET_HARDWARE_OPENGL)
+
+while (1):
+
+  mouseEvents = p.getMouseEvents()
+  for e in mouseEvents:
+    if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
+      mouseX = e[1]
+      mouseY = e[2]
+      rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
+      rayInfo = p.rayTest(rayFrom, rayTo)
+      #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
+      for l in range(len(rayInfo)):
+        hit = rayInfo[l]
+        objectUid = hit[0]
+        if (objectUid >= 0):
+          #p.removeBody(objectUid)
+          p.changeVisualShape(objectUid, -1, rgbaColor=colors[currentColor])
+          currentColor += 1
+          if (currentColor >= len(colors)):
+            currentColor = 0

Engine/lib/bullet/examples/pybullet/examples/debugDrawItems.py

@@ -0,0 +1,26 @@
+import pybullet as p
+import time
+import pybullet_data
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+  p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())  
+p.loadURDF("plane.urdf")
+kuka = p.loadURDF("kuka_iiwa/model.urdf")
+p.addUserDebugText("tip", [0, 0, 0.1],
+                   textColorRGB=[1, 0, 0],
+                   textSize=1.5,
+                   parentObjectUniqueId=kuka,
+                   parentLinkIndex=6)
+p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0], parentObjectUniqueId=kuka, parentLinkIndex=6)
+p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0], parentObjectUniqueId=kuka, parentLinkIndex=6)
+p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1], parentObjectUniqueId=kuka, parentLinkIndex=6)
+p.setRealTimeSimulation(0)
+angle = 0
+while (True):
+  time.sleep(0.01)
+  p.resetJointState(kuka, 2, angle)
+  p.resetJointState(kuka, 3, angle)
+  angle += 0.01

Engine/lib/bullet/examples/pybullet/examples/deformable_add_remove_objects.py

@@ -0,0 +1,42 @@
+import pybullet as p
+physicsClient = p.connect(p.GUI)
+import pybullet_data
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
+
+gravZ=-10
+p.setGravity(0, 0, gravZ)
+
+planeOrn = [0,0,0,1]
+planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
+
+def _load_softbody(basePos):
+    return p.loadSoftBody("cloth_z_up.obj", basePosition = basePos, scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, springDampingAllDirections = 1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1, collisionMargin = 0.04)
+    
+p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
+
+#load two objects and step
+cloth1 = _load_softbody([0,0,1])
+cloth2 = _load_softbody([0,1,0])
+
+for i in range(300):
+    p.stepSimulation()
+ 
+# remove one object, add two and then step
+p.removeBody(cloth2)
+_load_softbody([0,1,1])
+_load_softbody([0,-1,1])
+
+for i in range(300):
+   p.stepSimulation()
+
+# reset simulation, add objects then step
+p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
+p.setGravity(0, 0, gravZ)
+planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
+_load_softbody([0,1,0])
+_load_softbody([0,1,1])
+
+while p.isConnected():
+    p.stepSimulation()

Engine/lib/bullet/examples/pybullet/examples/deformable_anchor.py

@@ -0,0 +1,55 @@
+import pybullet as p
+from time import sleep
+
+physicsClient = p.connect(p.GUI)
+import pybullet_data
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
+
+gravZ=-10
+p.setGravity(0, 0, gravZ)
+
+planeOrn = [0,0,0,1]#p.getQuaternionFromEuler([0.3,0,0])
+#planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
+
+boxId = p.loadURDF("cube.urdf", [0,1,2],useMaximalCoordinates = True)
+
+clothId = p.loadSoftBody("cloth_z_up.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, springDampingAllDirections = 1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1)
+
+
+p.changeVisualShape(clothId, -1, flags=p.VISUAL_SHAPE_DOUBLE_SIDED)
+
+p.createSoftBodyAnchor(clothId  ,24,-1,-1)
+p.createSoftBodyAnchor(clothId ,20,-1,-1)
+p.createSoftBodyAnchor(clothId ,15,boxId,-1, [0.5,-0.5,0])
+p.createSoftBodyAnchor(clothId ,19,boxId,-1, [-0.5,-0.5,0])
+p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
+
+debug = True
+if debug:
+  data = p.getMeshData(clothId, -1, flags=p.MESH_DATA_SIMULATION_MESH)
+  print("--------------")
+  print("data=",data)
+  print(data[0])
+  print(data[1])
+  text_uid = []
+  for i in range(data[0]):
+      pos = data[1][i]
+      uid = p.addUserDebugText(str(i), pos, textColorRGB=[1,1,1])
+      text_uid.append(uid)
+
+while p.isConnected():
+  p.getCameraImage(320,200)
+  
+  if debug:
+    data = p.getMeshData(clothId, -1, flags=p.MESH_DATA_SIMULATION_MESH)
+    for i in range(data[0]):
+      pos = data[1][i]
+      uid = p.addUserDebugText(str(i), pos, textColorRGB=[1,1,1], replaceItemUniqueId=text_uid[i])
+
+  p.setGravity(0,0,gravZ)
+  p.stepSimulation()
+  #p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
+  #sleep(1./240.)
+  

Engine/lib/bullet/examples/pybullet/examples/deformable_ball.py

@@ -0,0 +1,37 @@
+import pybullet as p
+from time import sleep
+import pybullet_data
+
+physicsClient = p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
+
+p.setGravity(0, 0, -10)
+
+planeOrn = [0,0,0,1]#p.getQuaternionFromEuler([0.3,0,0])
+planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
+
+boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
+
+ballId = p.loadSoftBody("ball.obj", simFileName = "ball.vtk", basePosition = [0,0,-1], scale = 0.5, mass = 4, useNeoHookean = 1, NeoHookeanMu = 400, NeoHookeanLambda = 600, NeoHookeanDamping = 0.001, useSelfCollision = 1, frictionCoeff = .5, collisionMargin = 0.001)
+p.setTimeStep(0.001)
+p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
+
+
+#logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "perf.json")
+
+while p.isConnected():
+
+  p.stepSimulation()  
+  #there can be some artifacts in the visualizer window, 
+  #due to reading of deformable vertices in the renderer,
+  #while the simulators updates the same vertices
+  #it can be avoided using
+  #p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
+  #but then things go slower
+  p.setGravity(0,0,-10)
+  #sleep(1./240.)
+  
+#p.resetSimulation()
+#p.stopStateLogging(logId)

Engine/lib/bullet/examples/pybullet/examples/deformable_torus.py

@@ -0,0 +1,31 @@
+import pybullet as p
+from time import sleep
+import pybullet_data
+
+physicsClient = p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
+p.resetDebugVisualizerCamera(3,-420,-30,[0.3,0.9,-2])
+p.setGravity(0, 0, -10)
+
+tex = p.loadTexture("uvmap.png")
+planeId = p.loadURDF("plane.urdf", [0,0,-2])
+
+boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
+
+bunnyId = p.loadSoftBody("torus/torus_textured.obj", simFileName="torus.vtk", mass = 3, useNeoHookean = 1, NeoHookeanMu = 180, NeoHookeanLambda = 600, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 800)
+p.changeVisualShape(bunnyId, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
+
+
+bunny2 = p.loadURDF("torus_deform.urdf", [0,1,0.5], flags=p.URDF_USE_SELF_COLLISION)
+
+p.changeVisualShape(bunny2, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
+p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
+p.setRealTimeSimulation(0)
+
+while p.isConnected():
+  p.stepSimulation()
+  p.getCameraImage(320,200)
+  p.setGravity(0,0,-10)

Engine/lib/bullet/examples/pybullet/examples/draw_frames.py

@@ -0,0 +1,370 @@
+import colorsys
+from enum import Enum
+import numpy as np
+import pybullet as p
+import time
+import typing
+
+p.connect(p.GUI)
+p.setGravity(0, 0, -10)
+p.setRealTimeSimulation(0)
+p.setTimeStep(1 / 1000)
+
+test_case = 1
+rigid_body = False
+apply_force_torque = True
+apply_force_local = True
+apply_torque_local = True
+
+
+if test_case == 1:
+    cs_id = p.createCollisionShape(p.GEOM_BOX,
+                                   halfExtents=[0.25, 0.25, 0.25],
+                                   collisionFramePosition=[0.25, 0, 0],
+                                   collisionFrameOrientation=p.getQuaternionFromEuler([0, 0, 0]))
+    vs_id = p.createVisualShape(p.GEOM_BOX,
+                                halfExtents=[0.25, 0.25, 0.25],
+                                visualFramePosition=[0.25, 0.25, 0],
+                                visualFrameOrientation=p.getQuaternionFromEuler([0, 0, 0]))
+    body = p.createMultiBody(baseMass=1,
+                             baseCollisionShapeIndex=cs_id,
+                             baseVisualShapeIndex=vs_id,
+                             basePosition=[0, 0, 2],
+                             baseOrientation=p.getQuaternionFromEuler([-1.7, -0.8, 0.1]),
+                             baseInertialFramePosition=[0, 0.5, 0],
+                             baseInertialFrameOrientation=p.getQuaternionFromEuler([0.6, 0, 0.4]),
+                             useMaximalCoordinates=rigid_body)
+else:
+    cs_id = p.createCollisionShape(p.GEOM_BOX,
+                                   halfExtents=[0.25, 0.25, 0.25],
+                                   collisionFramePosition=[0, 0, 0],
+                                   collisionFrameOrientation=p.getQuaternionFromEuler([0, 0, 0]))
+    vs_id = p.createVisualShape(p.GEOM_BOX,
+                                halfExtents=[0.25, 0.25, 0.25],
+                                visualFramePosition=[0, 0, 0],
+                                visualFrameOrientation=p.getQuaternionFromEuler([0, 0, 0]))
+    body = p.createMultiBody(baseMass=1,
+                             baseCollisionShapeIndex=cs_id,
+                             baseVisualShapeIndex=vs_id,
+                             basePosition=[0, 0, 2],
+                             baseOrientation=p.getQuaternionFromEuler([0.9, 0.3, 0]),
+                             baseInertialFramePosition=[0, 0, 0],
+                             baseInertialFrameOrientation=p.getQuaternionFromEuler([0, 0, 0]),
+                             linkMasses=[1],
+                             linkCollisionShapeIndices=[cs_id],
+                             linkVisualShapeIndices=[vs_id],
+                             linkPositions=[[1, 0, 0]],
+                             linkOrientations=[p.getQuaternionFromEuler([0, 0, 0])],
+                             linkInertialFramePositions=[[0, 0, 0]],
+                             linkInertialFrameOrientations=[p.getQuaternionFromEuler([0, 0, 0])],
+                             linkParentIndices=[0],
+                             linkJointTypes=[p.JOINT_FIXED],
+                             linkJointAxis=[[1, 0, 0]],
+                             useMaximalCoordinates=False)
+
+
+def get_world_link_pose(body_unique_id: int, link_index: int):
+    """Pose of link frame with respect to world frame expressed in world frame.
+
+    Args:
+        body_unique_id (int): The body unique id, as returned by loadURDF, etc.
+        link_index (int): Link index or -1 for the base.
+
+    Returns:
+        pos_orn (tuple): See description.
+    """
+    if link_index == -1:
+        world_inertial_pose = get_world_inertial_pose(body_unique_id, -1)
+        dynamics_info = p.getDynamicsInfo(body_unique_id, -1)
+        local_inertial_pose = (dynamics_info[3], dynamics_info[4])
+
+        local_inertial_pose_inv = p.invertTransform(local_inertial_pose[0], local_inertial_pose[1])
+        pos_orn = p.multiplyTransforms(world_inertial_pose[0],
+                                       world_inertial_pose[1],
+                                       local_inertial_pose_inv[0],
+                                       local_inertial_pose_inv[1])
+    else:
+        state = p.getLinkState(body_unique_id, link_index)
+        pos_orn = (state[4], state[5])
+
+    return pos_orn
+
+
+def get_world_inertial_pose(body_unique_id: int, link_index: int):
+    """Pose of inertial frame with respect to world frame expressed in world frame.
+
+    Args:
+        body_unique_id (int): The body unique id, as returned by loadURDF, etc.
+        link_index (int): Link index or -1 for the base.
+
+    Returns:
+        pos_orn (tuple): See description.
+    """
+    if link_index == -1:
+        pos_orn = p.getBasePositionAndOrientation(body_unique_id)
+    else:
+        state = p.getLinkState(body_unique_id, link_index)
+        pos_orn = (state[0], state[1])
+
+    return pos_orn
+
+
+def get_world_visual_pose(body_unique_id: int, link_index: int):
+    """Pose of visual frame with respect to world frame expressed in world frame.
+
+    Args:
+        body_unique_id (int): The body unique id, as returned by loadURDF, etc.
+        link_index (int): Link index or -1 for the base.
+
+    Returns:
+        pos_orn (tuple): See description.
+    """
+    world_link_pose = get_world_link_pose(body_unique_id, link_index)
+    visual_shape_data = p.getVisualShapeData(body_unique_id, link_index)
+    local_visual_pose = (visual_shape_data[link_index + 1][5], visual_shape_data[link_index + 1][6])
+
+    return p.multiplyTransforms(world_link_pose[0],
+                                world_link_pose[1],
+                                local_visual_pose[0],
+                                local_visual_pose[1])
+
+
+def get_world_collision_pose(body_unique_id: int, link_index: int):
+    """Pose of collision frame with respect to world frame expressed in world frame.
+
+    Args:
+        body_unique_id (int): The body unique id, as returned by loadURDF, etc.
+        link_index (int): Link index or -1 for the base.
+
+    Returns:
+        pos_orn (tuple of ): See description.
+    """
+    world_inertial_pose = get_world_inertial_pose(body_unique_id, link_index)
+    collision_shape_data = p.getCollisionShapeData(body_unique_id, link_index)
+    if len(collision_shape_data) > 1:
+        raise NotImplementedError
+    local_collision_pose = (collision_shape_data[0][5], collision_shape_data[0][6])
+
+    return p.multiplyTransforms(world_inertial_pose[0],
+                                world_inertial_pose[1],
+                                local_collision_pose[0],
+                                local_collision_pose[1])
+
+
+def get_link_name(body_unique_id: int, link_index: int):
+    """Returns the link name.
+
+    Args:
+        body_unique_id (int): The body unique id, as returned by loadURDF, etc.
+        link_index (int): Link index or -1 for the base.
+
+    Returns:
+        link_name (str): Name of the link.
+    """
+    if link_index == -1:
+        link_name = p.getBodyInfo(body_unique_id)[0]
+    else:
+        link_name = p.getJointInfo(body_unique_id, link_index)[12]
+
+    return link_name.decode('UTF-8')
+
+
+class Frame(Enum):
+    LINK = 1,
+    INERTIAL = 2,
+    COLLISION = 3,
+    VISUAL = 4
+
+
+FRAME_NAME = dict()
+FRAME_NAME[Frame.LINK] = 'link'
+FRAME_NAME[Frame.INERTIAL] = 'inertial'
+FRAME_NAME[Frame.COLLISION] = 'collision'
+FRAME_NAME[Frame.VISUAL] = 'visual'
+
+
+def draw_frame(body_unique_id: int,
+               link_index: int,
+               frame: Frame,
+               title: str,
+               replace_item_unique_id: typing.Tuple[int, int, int, int] = None):
+    """Visualizes one of the frames/coordinate systems associated with each link (or base):
+    link, inertial, visual or collision frame.
+
+    Args:
+        body_unique_id (int): The body unique id, as returned by loadURDF, etc.
+        link_index (int): Link index or -1 for the base.
+        frame: The frame to draw (link, inertial, visual, collision)
+        title: Text which is drawn at the origin of the axis.
+        replace_item_unique_id (tuple of 4 ints): Replace existing axis and title to improve
+            performance and to avoid flickering.
+
+    Returns:
+        indices (tuple of 4 ints): The object id of the x-axis, y-axis, z-axis, and the title text.
+    """
+    if frame == Frame.LINK:
+        world_pose = get_world_link_pose(body_unique_id, link_index)
+    elif frame == Frame.INERTIAL:
+        world_pose = get_world_inertial_pose(body_unique_id, link_index)
+    elif frame == Frame.COLLISION:
+        world_pose = get_world_collision_pose(body_unique_id, link_index)
+    elif frame == Frame.VISUAL:
+        world_pose = get_world_visual_pose(body_unique_id, link_index)
+    else:
+        raise NotImplementedError
+
+    axis_scale = 0.1
+
+    pos = np.array(world_pose[0])
+    orn_mat = np.array(p.getMatrixFromQuaternion(world_pose[1])).reshape((3, 3))
+
+    kwargs = dict()
+    kwargs['lineWidth'] = 3
+
+    kwargs['lineColorRGB'] = [1, 0, 0]
+    if replace_item_unique_id is not None:
+        kwargs['replaceItemUniqueId'] = replace_item_unique_id[0]
+    x = p.addUserDebugLine(pos, pos + axis_scale * orn_mat[0:3, 0], **kwargs)
+
+    kwargs['lineColorRGB'] = [0, 1, 0]
+    if replace_item_unique_id is not None:
+        kwargs['replaceItemUniqueId'] = replace_item_unique_id[1]
+    y = p.addUserDebugLine(pos, pos + axis_scale * orn_mat[0:3, 1], **kwargs)
+
+    kwargs['lineColorRGB'] = [0, 0, 1]
+    if replace_item_unique_id is not None:
+        kwargs['replaceItemUniqueId'] = replace_item_unique_id[2]
+    z = p.addUserDebugLine(pos, pos + axis_scale * orn_mat[0:3, 2], **kwargs)
+
+    kwargs.clear()
+    if replace_item_unique_id is not None:
+        kwargs['replaceItemUniqueId'] = replace_item_unique_id[3]
+    title_index = p.addUserDebugText(title, pos, **kwargs)
+
+    return x, y, z, title_index
+
+
+class FrameDrawManager:
+    """Provides a straightforward and efficient way to draw frames/coordinate systems that are
+    associated with each link (or base). This includes the link, inertial, collision, and
+    visual frame.
+    """
+
+    def __init__(self):
+        self.line_indices = dict()
+
+    def _add_frame(self, frame: Frame, body_unique_id: int, link_index: int):
+        # Workaround for the following problem:
+        # When too many lines are added within a short period of time, the following error can occur
+        # "b3Printf: b3Warning[examples/SharedMemory/PhysicsClientSharedMemory.cpp,1286]:
+        # b3Printf: User debug draw failed".
+        time.sleep(1 / 100)
+
+        if self.line_indices.get(body_unique_id) is None:
+            self.line_indices[body_unique_id] = dict()
+
+        if self.line_indices[body_unique_id].get(frame) is None:
+            self.line_indices[body_unique_id][frame] = dict()
+
+        if self.line_indices[body_unique_id][frame].get(link_index) is None:
+            data = dict()
+            data['title'] = \
+                get_link_name(body_unique_id, link_index) + " (" + FRAME_NAME[frame] + " frame)"
+            data['replace_item_unique_id'] = draw_frame(body_unique_id,
+                                                        link_index,
+                                                        frame,
+                                                        data['title'])
+
+            self.line_indices[body_unique_id][frame][link_index] = data
+
+    def add_link_frame(self, body_unique_id: int, link_index: int):
+        self._add_frame(Frame.LINK, body_unique_id, link_index)
+
+    def add_inertial_frame(self, body_unique_id: int, link_index: int):
+        self._add_frame(Frame.INERTIAL, body_unique_id, link_index)
+
+    def add_collision_frame(self, body_unique_id: int, link_index: int):
+        self._add_frame(Frame.COLLISION, body_unique_id, link_index)
+
+    def add_visual_frame(self, body_unique_id: int, link_index: int):
+        self._add_frame(Frame.VISUAL, body_unique_id, link_index)
+
+    def update(self):
+        """Updates the drawing of all known frames. Note that this function is supposed to be
+        called after each simulation step.
+        """
+        for body_unique_id, dict0 in self.line_indices.items():
+            for frame, dict1 in dict0.items():
+                for link_index, dict2 in dict1.items():
+                    draw_frame(body_unique_id,
+                               link_index,
+                               frame,
+                               dict2['title'],
+                               dict2['replace_item_unique_id'])
+
+
+def high_contrast_bodies(alpha: float = 0.5):
+    """Makes all bodies transparent and gives each body an individual color.
+
+    Args:
+        alpha (float): Regulates the strength of transparency.
+    """
+    num_bodies = p.getNumBodies()
+
+    hsv = [(x * 1.0 / num_bodies, 0.5, 0.5) for x in range(num_bodies)]
+    rgb = list(map(lambda x: colorsys.hsv_to_rgb(*x), hsv))
+
+    for i in range(num_bodies):
+        body_unique_id = p.getBodyUniqueId(i)
+        for link_index in range(-1, p.getNumJoints(body_unique_id)):
+            p.changeVisualShape(body_unique_id,
+                                link_index,
+                                rgbaColor=[rgb[i][0], rgb[i][1], rgb[i][2], alpha])
+
+
+high_contrast_bodies()
+
+frame_draw_manager = FrameDrawManager()
+if test_case == 1:
+    frame_draw_manager.add_link_frame(body, -1)
+    frame_draw_manager.add_inertial_frame(body, -1)
+    if not rigid_body:
+        frame_draw_manager.add_collision_frame(body, -1)
+    frame_draw_manager.add_visual_frame(body, -1)
+else:
+    for i in range(-1, p.getNumJoints(body)):
+        frame_draw_manager.add_inertial_frame(body, i)
+
+if apply_force_torque:
+    while 1:
+        # The following two options are equivalent and are suppose to hold the body in place.
+        if apply_force_local:
+            for i in range(-1, p.getNumJoints(body)):
+                pose = get_world_inertial_pose(body, i)
+                pose_inv = p.invertTransform(pose[0], pose[1])
+                force = p.multiplyTransforms([0, 0, 0], pose_inv[1], [0, 0, 10], [0, 0, 0, 1])
+                p.applyExternalForce(body, i, force[0], [0, 0, 0], flags=p.LINK_FRAME)
+        else:
+            for i in range(-1, p.getNumJoints(body)):
+                pose = get_world_inertial_pose(body, i)
+                p.applyExternalForce(body, i, [0, 0, 10], pose[0], flags=p.WORLD_FRAME)
+
+        if test_case == 1:
+            if apply_torque_local:
+                p.applyExternalTorque(body, -1, [0, 0, 100], flags=p.LINK_FRAME)
+            else:
+                p.applyExternalTorque(body, -1, [0, 0, 100], flags=p.WORLD_FRAME)
+        else:
+            if apply_torque_local:
+                p.applyExternalTorque(body, -1, [0, 0, 100], flags=p.LINK_FRAME)
+                p.applyExternalTorque(body, 0, [0, 0, 100], flags=p.LINK_FRAME)
+            else:
+                p.applyExternalTorque(body, -1, [0, 0, 100], flags=p.WORLD_FRAME)
+                p.applyExternalTorque(body, 0, [0, 0, 100], flags=p.WORLD_FRAME)
+
+        p.stepSimulation()
+        frame_draw_manager.update()
+        time.sleep(1 / 10)
+else:
+    while 1:
+        time.sleep(1 / 10)

Engine/lib/bullet/examples/pybullet/examples/dumpLog.py

@@ -0,0 +1,81 @@
+import time
+import math
+from datetime import datetime
+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)
+
+  lenChunk = sz
+  log = list()
+  chunkIndex = 0
+  while (lenChunk):
+    check = f.read(2)
+    lenChunk = 0
+    if (check == b'\xaa\xbb'):
+      mychunk = f.read(sz)
+      lenChunk = len(mychunk)
+      chunks = [mychunk]
+      if verbose:
+        print("num chunks:")
+        print(len(chunks))
+
+      for chunk in chunks:
+        print("len(chunk)=", len(chunk), " sz = ", sz)
+        if len(chunk) == sz:
+          print("chunk #", chunkIndex)
+          chunkIndex = chunkIndex + 1
+          values = struct.unpack(fmt, chunk)
+          record = list()
+          for i in range(ncols):
+            record.append(values[i])
+            if verbose:
+              print("    ", keys[i], "=", values[i])
+
+          log.append(record)
+    else:
+      print("Error, expected aabb terminal")
+  return log
+
+
+numArgs = len(sys.argv)
+
+print('Number of arguments:', numArgs, 'arguments.')
+print('Argument List:', str(sys.argv))
+fileName = "log.bin"
+
+if (numArgs > 1):
+  fileName = sys.argv[1]
+
+print("filename=")
+print(fileName)
+
+verbose = True
+
+readLogFile(fileName, verbose)

Engine/lib/bullet/examples/pybullet/examples/dumpVrLog.py

@@ -0,0 +1,110 @@
+import time
+import math
+from datetime import datetime
+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()
+  if verbose:
+    print("num chunks:")
+    print(len(chunks))
+  chunkIndex = 0
+  for chunk in chunks:
+    print("len(chunk)=", len(chunk), " sz = ", sz)
+    if len(chunk) == sz:
+      print("chunk #", chunkIndex)
+      chunkIndex = chunkIndex + 1
+      values = struct.unpack(fmt, chunk)
+      record = list()
+      for i in range(ncols):
+        record.append(values[i])
+        if verbose:
+          print("    ", keys[i], "=", values[i])
+
+      log.append(record)
+
+  return log
+
+
+numArgs = len(sys.argv)
+
+print('Number of arguments:', numArgs, 'arguments.')
+print('Argument List:', str(sys.argv))
+fileName = "data/example_log_vr.bin"
+
+if (numArgs > 1):
+  fileName = sys.argv[1]
+
+print("filename=")
+print(fileName)
+
+verbose = True
+
+log = readLogFile(fileName, verbose)
+
+# the index of the first integer in the vr log file for packed buttons
+firstPackedButtonIndex = 13
+# the number of packed buttons in one integer
+numGroupedButtons = 10
+# the number of integers for packed buttons
+numPackedButtons = 7
+# the mask to get the button state
+buttonMask = 7
+
+for record in log:
+  # indices of buttons that are down
+  buttonDownIndices = []
+  # indices of buttons that are triggered
+  buttonTriggeredIndices = []
+  # indices of buttons that are released
+  buttonReleasedIndices = []
+  buttonIndex = 0
+  for packedButtonIndex in range(firstPackedButtonIndex,
+                                 firstPackedButtonIndex + numPackedButtons):
+    for packButtonShift in range(numGroupedButtons):
+      buttonEvent = buttonMask & record[packedButtonIndex]
+      if buttonEvent & 1:
+        buttonDownIndices.append(buttonIndex)
+      elif buttonEvent & 2:
+        buttonTriggeredIndices.append(buttonIndex)
+      elif buttonEvent & 4:
+        buttonReleasedIndices.append(buttonIndex)
+      record[packedButtonIndex] = record[packedButtonIndex] >> 3
+      buttonIndex += 1
+  if len(buttonDownIndices) or len(buttonTriggeredIndices) or len(buttonReleasedIndices):
+    print('timestamp: ', record[1])
+    print('button is down: ', buttonDownIndices)
+    print('button is triggered: ', buttonTriggeredIndices)
+    print('button is released: ', buttonReleasedIndices)

Engine/lib/bullet/examples/pybullet/examples/eglRenderTest.py

@@ -0,0 +1,54 @@
+import pybullet as p
+import time
+import pkgutil
+egl = pkgutil.get_loader('eglRenderer')
+import pybullet_data
+
+p.connect(p.DIRECT)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+plugin = p.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
+print("plugin=", plugin)
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+
+p.setGravity(0, 0, -10)
+p.loadURDF("plane.urdf", [0, 0, -1])
+p.loadURDF("r2d2.urdf")
+
+pixelWidth = 320
+pixelHeight = 220
+camTargetPos = [0, 0, 0]
+camDistance = 4
+pitch = -10.0
+roll = 0
+upAxisIndex = 2
+
+while (p.isConnected()):
+  for yaw in range(0, 360, 10):
+    start = time.time()
+    p.stepSimulation()
+    stop = time.time()
+    print("stepSimulation %f" % (stop - start))
+
+    #viewMatrix = [1.0, 0.0, -0.0, 0.0, -0.0, 0.1736481785774231, -0.9848078489303589, 0.0, 0.0, 0.9848078489303589, 0.1736481785774231, 0.0, -0.0, -5.960464477539063e-08, -4.0, 1.0]
+    viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll,
+                                                     upAxisIndex)
+    projectionMatrix = [
+        1.0825318098068237, 0.0, 0.0, 0.0, 0.0, 1.732050895690918, 0.0, 0.0, 0.0, 0.0,
+        -1.0002000331878662, -1.0, 0.0, 0.0, -0.020002000033855438, 0.0
+    ]
+
+    start = time.time()
+    img_arr = p.getCameraImage(pixelWidth,
+                               pixelHeight,
+                               viewMatrix=viewMatrix,
+                               projectionMatrix=projectionMatrix,
+                               shadow=1,
+                               lightDirection=[1, 1, 1])
+    stop = time.time()
+    print("renderImage %f" % (stop - start))
+    #time.sleep(.1)
+    #print("img_arr=",img_arr)
+
+p.unloadPlugin(plugin)

Engine/lib/bullet/examples/pybullet/examples/experimentalCcdSphereRadius.py

@@ -0,0 +1,55 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setPhysicsEngineParameter(allowedCcdPenetration=0.0)
+
+terrain_mass = 0
+terrain_visual_shape_id = -1
+terrain_position = [0, 0, 0]
+terrain_orientation = [0, 0, 0, 1]
+terrain_collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_MESH,
+                                                    fileName="terrain.obj",
+                                                    flags=p.GEOM_FORCE_CONCAVE_TRIMESH |
+                                                    p.GEOM_CONCAVE_INTERNAL_EDGE,
+                                                    meshScale=[0.5, 0.5, 0.5])
+p.createMultiBody(terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
+                  terrain_position, terrain_orientation)
+
+useMaximalCoordinates = True
+sphereRadius = 0.005
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+                                  halfExtents=[sphereRadius, sphereRadius, sphereRadius])
+
+mass = 1
+visualShapeId = -1
+
+for i in range(5):
+  for j in range(5):
+    for k in range(5):
+      #if (k&2):
+      sphereUid = p.createMultiBody(
+          mass,
+          colSphereId,
+          visualShapeId, [-i * 5 * sphereRadius, j * 5 * sphereRadius, k * 2 * sphereRadius + 1],
+          useMaximalCoordinates=useMaximalCoordinates)
+      #else:
+      #	sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
+      p.changeDynamics(sphereUid,
+                       -1,
+                       spinningFriction=0.001,
+                       rollingFriction=0.001,
+                       linearDamping=0.0)
+      p.changeDynamics(sphereUid, -1, ccdSweptSphereRadius=0.002)
+
+p.setGravity(0, 0, -10)
+
+pts = p.getContactPoints()
+print("num points=", len(pts))
+print(pts)
+while (p.isConnected()):
+  time.sleep(1. / 240.)
+  p.stepSimulation()

Engine/lib/bullet/examples/pybullet/examples/externalTorqueControlledSphere.py

@@ -0,0 +1,59 @@
+import pybullet as p
+import pybullet_data
+import time
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadSDF("stadium.sdf")
+p.setGravity(0, 0, -10)
+objects = p.loadMJCF("mjcf/sphere.xml")
+sphere = objects[0]
+p.resetBasePositionAndOrientation(sphere, [0, 0, 1], [0, 0, 0, 1])
+p.changeDynamics(sphere, -1, linearDamping=0.9)
+p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1])
+forward = 0
+turn = 0
+
+forwardVec = [2, 0, 0]
+cameraDistance = 1
+cameraYaw = 35
+cameraPitch = -35
+
+while (1):
+
+  spherePos, orn = p.getBasePositionAndOrientation(sphere)
+
+  cameraTargetPosition = spherePos
+  p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition)
+  camInfo = p.getDebugVisualizerCamera()
+  camForward = camInfo[5]
+
+  keys = p.getKeyboardEvents()
+  for k, v in keys.items():
+
+    if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
+      turn = -0.5
+    if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
+      turn = 0
+    if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
+      turn = 0.5
+    if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
+      turn = 0
+
+    if (k == p.B3G_UP_ARROW and (v & p.KEY_WAS_TRIGGERED)):
+      forward = 1
+    if (k == p.B3G_UP_ARROW and (v & p.KEY_WAS_RELEASED)):
+      forward = 0
+    if (k == p.B3G_DOWN_ARROW and (v & p.KEY_WAS_TRIGGERED)):
+      forward = -1
+    if (k == p.B3G_DOWN_ARROW and (v & p.KEY_WAS_RELEASED)):
+      forward = 0
+
+  force = [forward * camForward[0], forward * camForward[1], 0]
+  cameraYaw = cameraYaw + turn
+
+  if (forward):
+    p.applyExternalForce(sphere, -1, force, spherePos, flags=p.WORLD_FRAME)
+
+  p.stepSimulation()
+  time.sleep(1. / 240.)

Engine/lib/bullet/examples/pybullet/examples/fileIOPlugin.py

@@ -0,0 +1,23 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+fileIO = p.loadPlugin("fileIOPlugin")
+if (fileIO >= 0):
+  #we can have a zipfile (pickup.zip) inside a zipfile (pickup2.zip)
+  p.executePluginCommand(fileIO, "pickup2.zip", [p.AddFileIOAction, p.ZipFileIO])
+  p.executePluginCommand(fileIO, "pickup.zip", [p.AddFileIOAction, p.ZipFileIO])
+  objs = p.loadSDF("pickup/model.sdf")
+  dobot = objs[0]
+  p.changeVisualShape(dobot, -1, rgbaColor=[1, 1, 1, 1])
+
+else:
+  print("fileIOPlugin is disabled.")
+
+p.setPhysicsEngineParameter(enableFileCaching=False)
+
+while (1):
+  p.stepSimulation()
+  time.sleep(1. / 240.)

Engine/lib/bullet/examples/pybullet/examples/forcetorquesensor.py

@@ -0,0 +1,28 @@
+import pybullet as p
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+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()

Engine/lib/bullet/examples/pybullet/examples/frictionCone.py

@@ -0,0 +1,49 @@
+import pybullet as p
+import time
+import math
+
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+useMaximalCoordinates = False
+
+p.setGravity(0, 0, -10)
+plane = p.loadURDF("plane.urdf", [0, 0, -1], useMaximalCoordinates=useMaximalCoordinates)
+
+p.setRealTimeSimulation(0)
+
+velocity = 1
+num = 40
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)  #disable this to make it faster
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
+p.setPhysicsEngineParameter(enableConeFriction=1)
+
+for i in range(num):
+  print("progress:", i, num)
+
+  x = velocity * math.sin(2. * 3.1415 * float(i) / num)
+  y = velocity * math.cos(2. * 3.1415 * float(i) / num)
+  print("velocity=", x, y)
+  sphere = p.loadURDF("sphere_small_zeroinertia.urdf",
+                      flags=p.URDF_USE_INERTIA_FROM_FILE,
+                      useMaximalCoordinates=useMaximalCoordinates)
+  p.changeDynamics(sphere, -1, lateralFriction=0.02)
+  #p.changeDynamics(sphere,-1,rollingFriction=10)
+  p.changeDynamics(sphere, -1, linearDamping=0)
+  p.changeDynamics(sphere, -1, angularDamping=0)
+  p.resetBaseVelocity(sphere, linearVelocity=[x, y, 0])
+
+  prevPos = [0, 0, 0]
+  for i in range(2048):
+    p.stepSimulation()
+    pos = p.getBasePositionAndOrientation(sphere)[0]
+    if (i & 64):
+      p.addUserDebugLine(prevPos, pos, [1, 0, 0], 1)
+      prevPos = pos
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+
+while (1):
+  time.sleep(0.01)

Engine/lib/bullet/examples/pybullet/examples/getAABB.py

@@ -0,0 +1,82 @@
+import pybullet as p
+import pybullet_data
+draw = 1
+printtext = 0
+
+if (draw):
+  p.connect(p.GUI)
+else:
+  p.connect(p.DIRECT)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+r2d2 = p.loadURDF("r2d2.urdf")
+
+
+def drawAABB(aabb):
+  f = [aabbMin[0], aabbMin[1], aabbMin[2]]
+  t = [aabbMax[0], aabbMin[1], aabbMin[2]]
+  p.addUserDebugLine(f, t, [1, 0, 0])
+  f = [aabbMin[0], aabbMin[1], aabbMin[2]]
+  t = [aabbMin[0], aabbMax[1], aabbMin[2]]
+  p.addUserDebugLine(f, t, [0, 1, 0])
+  f = [aabbMin[0], aabbMin[1], aabbMin[2]]
+  t = [aabbMin[0], aabbMin[1], aabbMax[2]]
+  p.addUserDebugLine(f, t, [0, 0, 1])
+
+  f = [aabbMin[0], aabbMin[1], aabbMax[2]]
+  t = [aabbMin[0], aabbMax[1], aabbMax[2]]
+  p.addUserDebugLine(f, t, [1, 1, 1])
+
+  f = [aabbMin[0], aabbMin[1], aabbMax[2]]
+  t = [aabbMax[0], aabbMin[1], aabbMax[2]]
+  p.addUserDebugLine(f, t, [1, 1, 1])
+
+  f = [aabbMax[0], aabbMin[1], aabbMin[2]]
+  t = [aabbMax[0], aabbMin[1], aabbMax[2]]
+  p.addUserDebugLine(f, t, [1, 1, 1])
+
+  f = [aabbMax[0], aabbMin[1], aabbMin[2]]
+  t = [aabbMax[0], aabbMax[1], aabbMin[2]]
+  p.addUserDebugLine(f, t, [1, 1, 1])
+
+  f = [aabbMax[0], aabbMax[1], aabbMin[2]]
+  t = [aabbMin[0], aabbMax[1], aabbMin[2]]
+  p.addUserDebugLine(f, t, [1, 1, 1])
+
+  f = [aabbMin[0], aabbMax[1], aabbMin[2]]
+  t = [aabbMin[0], aabbMax[1], aabbMax[2]]
+  p.addUserDebugLine(f, t, [1, 1, 1])
+
+  f = [aabbMax[0], aabbMax[1], aabbMax[2]]
+  t = [aabbMin[0], aabbMax[1], aabbMax[2]]
+  p.addUserDebugLine(f, t, [1.0, 0.5, 0.5])
+  f = [aabbMax[0], aabbMax[1], aabbMax[2]]
+  t = [aabbMax[0], aabbMin[1], aabbMax[2]]
+  p.addUserDebugLine(f, t, [1, 1, 1])
+  f = [aabbMax[0], aabbMax[1], aabbMax[2]]
+  t = [aabbMax[0], aabbMax[1], aabbMin[2]]
+  p.addUserDebugLine(f, t, [1, 1, 1])
+
+
+aabb = p.getAABB(r2d2)
+aabbMin = aabb[0]
+aabbMax = aabb[1]
+if (printtext):
+  print(aabbMin)
+  print(aabbMax)
+if (draw == 1):
+  drawAABB(aabb)
+
+for i in range(p.getNumJoints(r2d2)):
+  aabb = p.getAABB(r2d2, i)
+  aabbMin = aabb[0]
+  aabbMax = aabb[1]
+  if (printtext):
+    print(aabbMin)
+    print(aabbMax)
+  if (draw == 1):
+    drawAABB(aabb)
+
+while (1):
+  a = 0
+  p.stepSimulation()

Engine/lib/bullet/examples/pybullet/examples/getCameraImageTest.py

@@ -0,0 +1,129 @@
+import matplotlib.pyplot as plt
+import numpy as np
+import pybullet as p
+import time
+import pybullet_data
+
+direct = p.connect(p.GUI)  #, options="--window_backend=2 --render_device=0")
+#egl = p.loadPlugin("eglRendererPlugin")
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF('plane.urdf')
+p.loadURDF("r2d2.urdf", [0, 0, 1])
+p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.0, 0.025])
+cube_trans = p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.1, 0.025])
+p.changeVisualShape(cube_trans, -1, rgbaColor=[1, 1, 1, 0.1])
+width = 128
+height = 128
+
+fov = 60
+aspect = width / height
+near = 0.02
+far = 1
+
+view_matrix = p.computeViewMatrix([0, 0, 0.5], [0, 0, 0], [1, 0, 0])
+projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)
+
+# Get depth values using the OpenGL renderer
+images = p.getCameraImage(width,
+                          height,
+                          view_matrix,
+                          projection_matrix,
+                          shadow=True,
+                          renderer=p.ER_BULLET_HARDWARE_OPENGL)
+# NOTE: the ordering of height and width change based on the conversion
+rgb_opengl = np.reshape(images[2], (height, width, 4)) * 1. / 255.
+depth_buffer_opengl = np.reshape(images[3], [width, height])
+depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl)
+seg_opengl = np.reshape(images[4], [width, height]) * 1. / 255.
+time.sleep(1)
+
+# Get depth values using Tiny renderer
+images = p.getCameraImage(width,
+                          height,
+                          view_matrix,
+                          projection_matrix,
+                          shadow=True,
+                          renderer=p.ER_TINY_RENDERER)
+depth_buffer_tiny = np.reshape(images[3], [width, height])
+depth_tiny = far * near / (far - (far - near) * depth_buffer_tiny)
+rgb_tiny = np.reshape(images[2], (height, width, 4)) * 1. / 255.
+seg_tiny = np.reshape(images[4], [width, height]) * 1. / 255.
+
+bearStartPos1 = [-3.3, 0, 0]
+bearStartOrientation1 = p.getQuaternionFromEuler([0, 0, 0])
+bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
+bearStartPos2 = [0, 0, 0]
+bearStartOrientation2 = p.getQuaternionFromEuler([0, 0, 0])
+bearId2 = p.loadURDF("teddy_large.urdf", bearStartPos2, bearStartOrientation2)
+textureId = p.loadTexture("checker_grid.jpg")
+for b in range(p.getNumBodies()):
+  p.changeVisualShape(b, linkIndex=-1, textureUniqueId=textureId)
+  for j in range(p.getNumJoints(b)):
+    p.changeVisualShape(b, linkIndex=j, textureUniqueId=textureId)
+
+viewMat = [
+    0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574,
+    0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004,
+    0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0
+]
+projMat = [
+    0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0,
+    0.0, 0.0, -0.02000020071864128, 0.0
+]
+images = p.getCameraImage(width,
+                          height,
+                          viewMatrix=viewMat,
+                          projectionMatrix=projMat,
+                          renderer=p.ER_BULLET_HARDWARE_OPENGL,
+                          flags=p.ER_USE_PROJECTIVE_TEXTURE,
+                          projectiveTextureView=viewMat,
+                          projectiveTextureProj=projMat)
+proj_opengl = np.reshape(images[2], (height, width, 4)) * 1. / 255.
+time.sleep(1)
+
+images = p.getCameraImage(width,
+                          height,
+                          viewMatrix=viewMat,
+                          projectionMatrix=projMat,
+                          renderer=p.ER_TINY_RENDERER,
+                          flags=p.ER_USE_PROJECTIVE_TEXTURE,
+                          projectiveTextureView=viewMat,
+                          projectiveTextureProj=projMat)
+proj_tiny = np.reshape(images[2], (height, width, 4)) * 1. / 255.
+
+# Plot both images - should show depth values of 0.45 over the cube and 0.5 over the plane
+plt.subplot(4, 2, 1)
+plt.imshow(depth_opengl, cmap='gray', vmin=0, vmax=1)
+plt.title('Depth OpenGL3')
+
+plt.subplot(4, 2, 2)
+plt.imshow(depth_tiny, cmap='gray', vmin=0, vmax=1)
+plt.title('Depth TinyRenderer')
+
+plt.subplot(4, 2, 3)
+plt.imshow(rgb_opengl)
+plt.title('RGB OpenGL3')
+
+plt.subplot(4, 2, 4)
+plt.imshow(rgb_tiny)
+plt.title('RGB Tiny')
+
+plt.subplot(4, 2, 5)
+plt.imshow(seg_opengl)
+plt.title('Seg OpenGL3')
+
+plt.subplot(4, 2, 6)
+plt.imshow(seg_tiny)
+plt.title('Seg Tiny')
+
+plt.subplot(4, 2, 7)
+plt.imshow(proj_opengl)
+plt.title('Proj OpenGL')
+
+plt.subplot(4, 2, 8)
+plt.imshow(proj_tiny)
+plt.title('Proj Tiny')
+plt.subplots_adjust(hspace=0.7)
+
+plt.show()

Engine/lib/bullet/examples/pybullet/examples/getClosestPoints.py

@@ -0,0 +1,75 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+useCollisionShapeQuery = True
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1)
+geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.2, 0.2])
+baseOrientationB = p.getQuaternionFromEuler([0, 0.3, 0])  #[0,0.5,0.5,0]
+basePositionB = [1.5, 0, 1]
+obA = -1
+obB = -1
+
+obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1])
+obB = p.createMultiBody(baseMass=0,
+                        baseCollisionShapeIndex=geomBox,
+                        basePosition=basePositionB,
+                        baseOrientation=baseOrientationB)
+
+lineWidth = 3
+colorRGB = [1, 0, 0]
+lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0],
+                            lineToXYZ=[0, 0, 0],
+                            lineColorRGB=colorRGB,
+                            lineWidth=lineWidth,
+                            lifeTime=0)
+pitch = 0
+yaw = 0
+
+while (p.isConnected()):
+  pitch += 0.01
+  if (pitch >= 3.1415 * 2.):
+    pitch = 0
+  yaw += 0.01
+  if (yaw >= 3.1415 * 2.):
+    yaw = 0
+
+  baseOrientationB = p.getQuaternionFromEuler([yaw, pitch, 0])
+  if (obB >= 0):
+    p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
+
+  if (useCollisionShapeQuery):
+    pts = p.getClosestPoints(bodyA=-1,
+                             bodyB=-1,
+                             distance=100,
+                             collisionShapeA=geom,
+                             collisionShapeB=geomBox,
+                             collisionShapePositionA=[0.5, 0, 1],
+                             collisionShapePositionB=basePositionB,
+                             collisionShapeOrientationB=baseOrientationB)
+    #pts = p.getClosestPoints(bodyA=obA, bodyB=-1, distance=100, collisionShapeB=geomBox, collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB)
+  else:
+    pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100)
+
+  if len(pts) > 0:
+    #print(pts)
+    distance = pts[0][8]
+    #print("distance=",distance)
+    ptA = pts[0][5]
+    ptB = pts[0][6]
+    p.addUserDebugLine(lineFromXYZ=ptA,
+                       lineToXYZ=ptB,
+                       lineColorRGB=colorRGB,
+                       lineWidth=lineWidth,
+                       lifeTime=0,
+                       replaceItemUniqueId=lineId)
+  #time.sleep(1./240.)
+
+#removeCollisionShape is optional:
+#only use removeCollisionShape if the collision shape is not used to create a body
+#and if you want to keep on creating new collision shapes for different queries (not recommended)
+p.removeCollisionShape(geom)
+p.removeCollisionShape(geomBox)

Engine/lib/bullet/examples/pybullet/examples/getTextureUid.py

@@ -0,0 +1,21 @@
+import pybullet as p
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+plane = p.loadURDF("plane.urdf")
+visualData = p.getVisualShapeData(plane, p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS)
+print(visualData)
+curTexUid = visualData[0][8]
+print(curTexUid)
+texUid = p.loadTexture("tex256.png")
+print("texUid=", texUid)
+
+p.changeVisualShape(plane, -1, textureUniqueId=texUid)
+
+for i in range(100):
+  p.getCameraImage(320, 200)
+p.changeVisualShape(plane, -1, textureUniqueId=curTexUid)
+
+for i in range(100):
+  p.getCameraImage(320, 200)

Engine/lib/bullet/examples/pybullet/examples/graphicsClient.py

@@ -0,0 +1,15 @@
+import pybullet as p
+import time
+
+p.connect(p.GRAPHICS_SERVER_TCP)
+import pybullet_data as pd
+p.setAdditionalSearchPath(pd.getDataPath())
+
+p.loadURDF("plane.urdf")
+p.loadURDF("r2d2.urdf", [0,0,3])
+p.setGravity(0,0,-10)
+gravId = p.addUserDebugParameter("gravity",0,10,0)
+while p.isConnected():
+  p.stepSimulation()
+
+

Engine/lib/bullet/examples/pybullet/examples/graphicsServer.py

@@ -0,0 +1,8 @@
+import pybullet as p
+import time
+
+p.connect(p.GRAPHICS_SERVER)
+#p.connect(p.GRAPHICS_SERVER_MAIN_THREAD)
+while p.isConnected():
+  p.stepSimulation()
+  time.sleep(1./240.)

Engine/lib/bullet/examples/pybullet/examples/grpcClient.py

@@ -0,0 +1,21 @@
+import pybullet as p
+import pybullet_data
+
+
+usePort = True
+
+if (usePort):
+  id = p.connect(p.GRPC, "localhost:12345")
+else:
+  id = p.connect(p.GRPC, "localhost")
+print("id=", id)
+
+if (id < 0):
+  print("Cannot connect to GRPC server")
+  exit(0)
+
+print("Connected to GRPC")
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+r2d2 = p.loadURDF("r2d2.urdf")
+print("numJoints = ", p.getNumJoints(r2d2))

Engine/lib/bullet/examples/pybullet/examples/grpcServer.py

@@ -0,0 +1,32 @@
+import pybullet as p
+import time
+
+useDirect = False
+usePort = True
+
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+id = p.loadPlugin("grpcPlugin")
+#dynamically loading the plugin
+#id = p.loadPlugin("E:/develop/bullet3/bin/pybullet_grpcPlugin_vs2010_x64_debug.dll", postFix="_grpcPlugin")
+
+#start the GRPC server at hostname, port
+if (id < 0):
+  print("Cannot load grpcPlugin")
+  exit(0)
+
+if usePort:
+  p.executePluginCommand(id, "localhost:12345")
+else:
+  p.executePluginCommand(id, "localhost")
+
+while p.isConnected():
+  if (useDirect):
+    #Only in DIRECT mode, since there is no 'ping' you need to manually call to handle RCPs:
+    numRPC = 10
+    p.executePluginCommand(id, intArgs=[numRPC])
+  else:
+    dt = 1. / 240.
+    time.sleep(dt)

Engine/lib/bullet/examples/pybullet/examples/hand.ino

@@ -0,0 +1,36 @@
+//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);
+  digitalWrite(A0, INPUT_PULLUP);
+  digitalWrite(A1, INPUT_PULLUP);
+  digitalWrite(A2, INPUT_PULLUP);
+  digitalWrite(A3, INPUT_PULLUP);
+}
+
+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);
+}

Engine/lib/bullet/examples/pybullet/examples/hand.py

@@ -0,0 +1,128 @@
+#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
+import pybullet_data
+
+
+#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)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())  
+
+#load the MuJoCo MJCF hand
+objects = p.loadMJCF("MPL/MPL.xml")
+
+hand = objects[0]
+#clamp in range 400-600
+#minV = 400
+#maxV = 600
+minVarray = [275, 280, 350, 290]
+maxVarray = [450, 550, 500, 400]
+
+pinkId = 0
+middleId = 1
+indexId = 2
+thumbId = 3
+
+p.setRealTimeSimulation(1)
+
+
+def getSerialOrNone(portname):
+  try:
+    return serial.Serial(port=portname,
+                         baudrate=115200,
+                         parity=serial.PARITY_ODD,
+                         stopbits=serial.STOPBITS_TWO,
+                         bytesize=serial.SEVENBITS)
+  except:
+    return None
+
+
+def convertSensor(x, fingerIndex):
+  minV = minVarray[fingerIndex]
+  maxV = maxVarray[fingerIndex]
+
+  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 (b)
+
+
+ser = None
+portindex = 0
+while (ser is None and portindex < 30):
+  portname = 'COM' + str(portindex)
+  print(portname)
+  ser = getSerialOrNone(portname)
+  if (ser is None):
+    portname = "/dev/cu.usbmodem14" + str(portindex)
+    print(portname)
+    ser = getSerialOrNone(portname)
+    if (ser is not None):
+      print("COnnected!")
+  portindex = portindex + 1
+
+if (ser is None):
+  ser = serial.Serial(port="/dev/cu.usbmodem1421",
+                      baudrate=115200,
+                      parity=serial.PARITY_ODD,
+                      stopbits=serial.STOPBITS_TWO,
+                      bytesize=serial.SEVENBITS)
+pi = 3.141592
+
+if (ser is not None and ser.isOpen()):
+  while True:
+    while ser.inWaiting() > 0:
+      line = str(ser.readline())
+      words = line.split(",")
+      if (len(words) == 6):
+        pink = convertSensor(words[1], pinkId)
+        middle = convertSensor(words[2], middleId)
+        index = convertSensor(words[3], indexId)
+        thumb = convertSensor(words[4], thumbId)
+
+        p.setJointMotorControl2(hand, 7, p.POSITION_CONTROL, pi / 4.)
+        p.setJointMotorControl2(hand, 9, p.POSITION_CONTROL, thumb + pi / 10)
+        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)
+else:
+  print("Cannot find port")

Engine/lib/bullet/examples/pybullet/examples/heightfield.py

@@ -0,0 +1,138 @@
+import pybullet as p
+import pybullet_data as pd
+import math
+import time
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pd.getDataPath())
+
+textureId = -1
+
+useProgrammatic = 0
+useTerrainFromPNG = 1
+useDeepLocoCSV = 2
+updateHeightfield = False
+
+heightfieldSource = useProgrammatic
+import random
+random.seed(10)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+heightPerturbationRange = 0.05
+if heightfieldSource==useProgrammatic:
+  numHeightfieldRows = 256
+  numHeightfieldColumns = 256
+  heightfieldData = [0]*numHeightfieldRows*numHeightfieldColumns 
+  for j in range (int(numHeightfieldColumns/2)):
+    for i in range (int(numHeightfieldRows/2) ):
+      height = random.uniform(0,heightPerturbationRange)
+      heightfieldData[2*i+2*j*numHeightfieldRows]=height
+      heightfieldData[2*i+1+2*j*numHeightfieldRows]=height
+      heightfieldData[2*i+(2*j+1)*numHeightfieldRows]=height
+      heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows]=height
+      
+      
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.05,.05,1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns)
+  terrain  = p.createMultiBody(0, terrainShape)
+  p.resetBasePositionAndOrientation(terrain,[0,0,0], [0,0,0,1])
+
+if heightfieldSource==useDeepLocoCSV:
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.5,.5,2.5],fileName = "heightmaps/ground0.txt", heightfieldTextureScaling=128)
+  terrain  = p.createMultiBody(0, terrainShape)
+  p.resetBasePositionAndOrientation(terrain,[0,0,0], [0,0,0,1])
+
+if heightfieldSource==useTerrainFromPNG:
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.1,.1,24],fileName = "heightmaps/wm_height_out.png")
+  textureId = p.loadTexture("heightmaps/gimp_overlay_out.png")
+  terrain  = p.createMultiBody(0, terrainShape)
+  p.changeVisualShape(terrain, -1, textureUniqueId = textureId)
+ 
+ 
+p.changeVisualShape(terrain, -1, rgbaColor=[1,1,1,1])
+
+
+sphereRadius = 0.05
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+                                  halfExtents=[sphereRadius, sphereRadius, sphereRadius])
+
+mass = 1
+visualShapeId = -1
+
+link_Masses = [1]
+linkCollisionShapeIndices = [colBoxId]
+linkVisualShapeIndices = [-1]
+linkPositions = [[0, 0, 0.11]]
+linkOrientations = [[0, 0, 0, 1]]
+linkInertialFramePositions = [[0, 0, 0]]
+linkInertialFrameOrientations = [[0, 0, 0, 1]]
+indices = [0]
+jointTypes = [p.JOINT_REVOLUTE]
+axis = [[0, 0, 1]]
+
+for i in range(3):
+  for j in range(3):
+    for k in range(3):
+      basePosition = [
+          i * 5 * sphereRadius, j * 5 * sphereRadius, 1 + k * 5 * sphereRadius + 1
+      ]
+      baseOrientation = [0, 0, 0, 1]
+      if (k & 2):
+        sphereUid = p.createMultiBody(mass, colSphereId, visualShapeId, basePosition,
+                                      baseOrientation)
+      else:
+        sphereUid = p.createMultiBody(mass,
+                                      colBoxId,
+                                      visualShapeId,
+                                      basePosition,
+                                      baseOrientation,
+                                      linkMasses=link_Masses,
+                                      linkCollisionShapeIndices=linkCollisionShapeIndices,
+                                      linkVisualShapeIndices=linkVisualShapeIndices,
+                                      linkPositions=linkPositions,
+                                      linkOrientations=linkOrientations,
+                                      linkInertialFramePositions=linkInertialFramePositions,
+                                      linkInertialFrameOrientations=linkInertialFrameOrientations,
+                                      linkParentIndices=indices,
+                                      linkJointTypes=jointTypes,
+                                      linkJointAxis=axis)
+
+      
+      p.changeDynamics(sphereUid,
+                       -1,
+                       spinningFriction=0.001,
+                       rollingFriction=0.001,
+                       linearDamping=0.0)
+      for joint in range(p.getNumJoints(sphereUid)):
+        p.setJointMotorControl2(sphereUid, joint, p.VELOCITY_CONTROL, targetVelocity=1, force=10)
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
+p.setGravity(0, 0, -10)
+p.setRealTimeSimulation(1)
+
+p.getNumJoints(sphereUid)
+for i in range(p.getNumJoints(sphereUid)):
+  p.getJointInfo(sphereUid, i)
+
+
+while (p.isConnected()):
+  keys = p.getKeyboardEvents()
+  
+  if updateHeightfield and heightfieldSource==useProgrammatic:
+    for j in range (int(numHeightfieldColumns/2)):
+      for i in range (int(numHeightfieldRows/2) ):
+        height = random.uniform(0,heightPerturbationRange)#+math.sin(time.time())
+        heightfieldData[2*i+2*j*numHeightfieldRows]=height
+        heightfieldData[2*i+1+2*j*numHeightfieldRows]=height
+        heightfieldData[2*i+(2*j+1)*numHeightfieldRows]=height
+        heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows]=height
+    #GEOM_CONCAVE_INTERNAL_EDGE may help avoid getting stuck at an internal (shared) edge of the triangle/heightfield.
+    #GEOM_CONCAVE_INTERNAL_EDGE is a bit slower to build though.
+    #flags = p.GEOM_CONCAVE_INTERNAL_EDGE
+    flags = 0
+    terrainShape2 = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, flags = flags, meshScale=[.05,.05,1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns, replaceHeightfieldIndex = terrainShape)
+    
+
+  #print(keys)
+  #getCameraImage note: software/TinyRenderer doesn't render/support heightfields!
+  #p.getCameraImage(320,200, renderer=p.ER_BULLET_HARDWARE_OPENGL)
+  time.sleep(0.01)

Engine/lib/bullet/examples/pybullet/examples/hello_pybullet.py

@@ -0,0 +1,27 @@
+import pybullet as p
+from time import sleep
+import pybullet_data
+
+
+physicsClient = p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+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()

Engine/lib/bullet/examples/pybullet/examples/humanoidMotionCapture.py

@@ -0,0 +1,614 @@
+import pybullet as p
+import json
+import time
+import pybullet_data
+
+
+useGUI = True
+if useGUI:
+  p.connect(p.GUI)
+else:
+  p.connect(p.DIRECT)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+useZUp = False
+useYUp = not useZUp
+showJointMotorTorques = False
+
+if useYUp:
+  p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1)
+
+from pdControllerExplicit import PDControllerExplicitMultiDof
+from pdControllerStable import PDControllerStableMultiDof
+
+explicitPD = PDControllerExplicitMultiDof(p)
+stablePD = PDControllerStableMultiDof(p)
+
+p.resetDebugVisualizerCamera(cameraDistance=7.4,
+                             cameraYaw=-94,
+                             cameraPitch=-14,
+                             cameraTargetPosition=[0.24, -0.02, -0.09])
+
+import pybullet_data
+p.setTimeOut(10000)
+useMotionCapture = False
+useMotionCaptureReset = False  #not	useMotionCapture
+useExplicitPD = True
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setPhysicsEngineParameter(numSolverIterations=30)
+#p.setPhysicsEngineParameter(solverResidualThreshold=1e-30)
+
+#explicit	PD control requires	small	timestep
+timeStep = 1. / 600.
+#timeStep	=	1./240.
+
+p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
+
+path = pybullet_data.getDataPath() + "/data/motions/humanoid3d_backflip.txt"
+#path	=	pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt"
+#path	=	pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
+
+#p.loadURDF("plane.urdf",[0,0,-1.03])
+print("path	=	", path)
+with open(path, 'r') as f:
+  motion_dict = json.load(f)
+#print("motion_dict	=	", motion_dict)
+print("len motion=", len(motion_dict))
+print(motion_dict['Loop'])
+numFrames = len(motion_dict['Frames'])
+print("#frames = ", numFrames)
+
+frameId = p.addUserDebugParameter("frame", 0, numFrames - 1, 0)
+
+erpId = p.addUserDebugParameter("erp", 0, 1, 0.2)
+
+kpMotorId = p.addUserDebugParameter("kpMotor", 0, 1, .2)
+forceMotorId = p.addUserDebugParameter("forceMotor", 0, 2000, 1000)
+
+jointTypes = [
+    "JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED"
+]
+
+startLocations = [[0, 0, 2], [0, 0, 0], [0, 0, -2], [0, 0, -4], [0, 0, 4]]
+
+p.addUserDebugText("Stable PD",
+                   [startLocations[0][0], startLocations[0][1] + 1, startLocations[0][2]],
+                   [0, 0, 0])
+p.addUserDebugText("Spherical	Drive",
+                   [startLocations[1][0], startLocations[1][1] + 1, startLocations[1][2]],
+                   [0, 0, 0])
+p.addUserDebugText("Explicit PD",
+                   [startLocations[2][0], startLocations[2][1] + 1, startLocations[2][2]],
+                   [0, 0, 0])
+p.addUserDebugText("Kinematic",
+                   [startLocations[3][0], startLocations[3][1] + 1, startLocations[3][2]],
+                   [0, 0, 0])
+p.addUserDebugText("Stable PD (Py)",
+                   [startLocations[4][0], startLocations[4][1] + 1, startLocations[4][2]],
+                   [0, 0, 0])
+flags=p.URDF_MAINTAIN_LINK_ORDER+p.URDF_USE_SELF_COLLISION
+humanoid = p.loadURDF("humanoid/humanoid.urdf",
+                      startLocations[0],
+                      globalScaling=0.25,
+                      useFixedBase=False,
+                      flags=flags)
+humanoid2 = p.loadURDF("humanoid/humanoid.urdf",
+                       startLocations[1],
+                       globalScaling=0.25,
+                       useFixedBase=False,
+                       flags=flags)
+humanoid3 = p.loadURDF("humanoid/humanoid.urdf",
+                       startLocations[2],
+                       globalScaling=0.25,
+                       useFixedBase=False,
+                       flags=flags)
+humanoid4 = p.loadURDF("humanoid/humanoid.urdf",
+                       startLocations[3],
+                       globalScaling=0.25,
+                       useFixedBase=False,
+                       flags=flags)
+humanoid5 = p.loadURDF("humanoid/humanoid.urdf",
+                      startLocations[4],
+                      globalScaling=0.25,
+                      useFixedBase=False,
+                      flags=flags)
+
+humanoid_fix = p.createConstraint(humanoid, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
+                                  startLocations[0], [0, 0, 0, 1])
+humanoid2_fix = p.createConstraint(humanoid2, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
+                                   startLocations[1], [0, 0, 0, 1])
+humanoid3_fix = p.createConstraint(humanoid3, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
+                                   startLocations[2], [0, 0, 0, 1])
+humanoid3_fix = p.createConstraint(humanoid4, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
+                                   startLocations[3], [0, 0, 0, 1])
+humanoid4_fix = p.createConstraint(humanoid5, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
+                                   startLocations[4], [0, 0, 0, 1])
+
+startPose = [
+    2, 0.847532, 0, 0.9986781045, 0.01410400148, -0.0006980000731, -0.04942300517, 0.9988133229,
+    0.009485003066, -0.04756001538, -0.004475001447, 1, 0, 0, 0, 0.9649395871, 0.02436898957,
+    -0.05755497537, 0.2549218909, -0.249116, 0.9993661511, 0.009952001505, 0.03265400494,
+    0.01009800153, 0.9854981188, -0.06440700776, 0.09324301124, -0.1262970152, 0.170571,
+    0.9927545808, -0.02090099117, 0.08882396249, -0.07817796699, -0.391532, 0.9828788495,
+    0.1013909845, -0.05515999155, 0.143618978, 0.9659421276, 0.1884590249, -0.1422460188,
+    0.105854014, 0.581348
+]
+
+startVel = [
+    1.235314324, -0.008525509087, 0.1515293946, -1.161516553, 0.1866449799, -0.1050802848, 0,
+    0.935706195, 0.08277326387, 0.3002461862, 0, 0, 0, 0, 0, 1.114409628, 0.3618553952,
+    -0.4505575061, 0, -1.725374735, -0.5052852598, -0.8555179722, -0.2221173515, 0, -0.1837617357,
+    0.00171895706, 0.03912837591, 0, 0.147945294, 1.837653345, 0.1534535548, 1.491385941, 0,
+    -4.632454387, -0.9111172777, -1.300648184, -1.345694622, 0, -1.084238535, 0.1313680236,
+    -0.7236998534, 0, -0.5278312973
+]
+
+p.resetBasePositionAndOrientation(humanoid, startLocations[0], [0, 0, 0, 1])
+p.resetBasePositionAndOrientation(humanoid2, startLocations[1], [0, 0, 0, 1])
+p.resetBasePositionAndOrientation(humanoid3, startLocations[2], [0, 0, 0, 1])
+p.resetBasePositionAndOrientation(humanoid4, startLocations[3], [0, 0, 0, 1])
+p.resetBasePositionAndOrientation(humanoid5, startLocations[4], [0, 0, 0, 1])
+
+index0 = 7
+for j in range(p.getNumJoints(humanoid)):
+  ji = p.getJointInfo(humanoid, j)
+  targetPosition = [0]
+  jointType = ji[2]
+  if (jointType == p.JOINT_SPHERICAL):
+    targetPosition = [
+        startPose[index0 + 1], startPose[index0 + 2], startPose[index0 + 3], startPose[index0 + 0]
+    ]
+    targetVel = [startVel[index0 + 0], startVel[index0 + 1], startVel[index0 + 2]]
+    index0 += 4
+    print("spherical position: ", targetPosition)
+    print("spherical velocity: ", targetVel)
+    p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel)
+    p.resetJointStateMultiDof(humanoid5, j, targetValue=targetPosition, targetVelocity=targetVel)
+    p.resetJointStateMultiDof(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel)
+  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
+    targetPosition = [startPose[index0]]
+    targetVel = [startVel[index0]]
+    index0 += 1
+    print("revolute:", targetPosition)
+    print("revolute	velocity:", targetVel)
+    p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel)
+    p.resetJointStateMultiDof(humanoid5, j, targetValue=targetPosition, targetVelocity=targetVel)
+    p.resetJointStateMultiDof(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel)
+
+for j in range(p.getNumJoints(humanoid)):
+  ji = p.getJointInfo(humanoid, j)
+  targetPosition = [0]
+  jointType = ji[2]
+  if (jointType == p.JOINT_SPHERICAL):
+    targetPosition = [0, 0, 0, 1]
+    p.setJointMotorControlMultiDof(humanoid,
+                                   j,
+                                   p.POSITION_CONTROL,
+                                   targetPosition,
+                                   targetVelocity=[0, 0, 0],
+                                   positionGain=0,
+                                   velocityGain=1,
+                                   force=[0, 0, 0])
+    p.setJointMotorControlMultiDof(humanoid5,
+                                   j,
+                                   p.POSITION_CONTROL,
+                                   targetPosition,
+                                   targetVelocity=[0, 0, 0],
+                                   positionGain=0,
+                                   velocityGain=1,
+                                   force=[0, 0, 0])
+    p.setJointMotorControlMultiDof(humanoid3,
+                                   j,
+                                   p.POSITION_CONTROL,
+                                   targetPosition,
+                                   targetVelocity=[0, 0, 0],
+                                   positionGain=0,
+                                   velocityGain=1,
+                                   force=[31, 31, 31])
+    p.setJointMotorControlMultiDof(humanoid4,
+                                   j,
+                                   p.POSITION_CONTROL,
+                                   targetPosition,
+                                   targetVelocity=[0, 0, 0],
+                                   positionGain=0,
+                                   velocityGain=1,
+                                   force=[1, 1, 1])
+
+  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
+    p.setJointMotorControl2(humanoid, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
+    p.setJointMotorControl2(humanoid3, j, p.VELOCITY_CONTROL, targetVelocity=0, force=31)
+    p.setJointMotorControl2(humanoid4, j, p.VELOCITY_CONTROL, targetVelocity=0, force=10)
+    p.setJointMotorControl2(humanoid5, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
+
+  #print(ji)
+  print("joint[", j, "].type=", jointTypes[ji[2]])
+  print("joint[", j, "].name=", ji[1])
+
+jointIds = []
+paramIds = []
+for j in range(p.getNumJoints(humanoid)):
+  #p.changeDynamics(humanoid,j,linearDamping=0,	angularDamping=0)
+  p.changeVisualShape(humanoid, j, rgbaColor=[1, 1, 1, 1])
+  info = p.getJointInfo(humanoid, j)
+  #print(info)
+  if (not useMotionCapture):
+    jointName = info[1]
+    jointType = info[2]
+    if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
+      jointIds.append(j)
+      #paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
+      #print("jointName=",jointName, "at ",	j)
+
+p.changeVisualShape(humanoid, 2, rgbaColor=[1, 0, 0, 1])
+chest = 1
+neck = 2
+rightHip = 3
+rightKnee = 4
+rightAnkle = 5
+rightShoulder = 6
+rightElbow = 7
+leftHip = 9
+leftKnee = 10
+leftAnkle = 11
+leftShoulder = 12
+leftElbow = 13
+
+#rightShoulder=3
+#rightElbow=4
+#leftShoulder=6
+#leftElbow = 7
+#rightHip	=	9
+#rightKnee=10
+#rightAnkle=11
+#leftHip = 12
+#leftKnee=13
+#leftAnkle=14
+
+import time
+
+kpOrg = [
+    0, 0, 0, 0, 0, 0, 0, 1000, 1000, 1000, 1000, 100, 100, 100, 100, 500, 500, 500, 500, 500, 400,
+    400, 400, 400, 400, 400, 400, 400, 300, 500, 500, 500, 500, 500, 400, 400, 400, 400, 400, 400,
+    400, 400, 300
+]
+kdOrg = [
+    0, 0, 0, 0, 0, 0, 0, 100, 100, 100, 100, 10, 10, 10, 10, 50, 50, 50, 50, 50, 40, 40, 40, 40,
+    40, 40, 40, 40, 30, 50, 50, 50, 50, 50, 40, 40, 40, 40, 40, 40, 40, 40, 30
+]
+
+once = True
+p.getCameraImage(320, 200)
+
+while (p.isConnected()):
+
+  if useGUI:
+    erp = p.readUserDebugParameter(erpId)
+    kpMotor = p.readUserDebugParameter(kpMotorId)
+    maxForce = p.readUserDebugParameter(forceMotorId)
+    frameReal = p.readUserDebugParameter(frameId)
+  else:
+    erp = 0.2
+    kpMotor = 0.2
+    maxForce = 1000
+    frameReal = 0
+
+  kp = kpMotor
+
+  frame = int(frameReal)
+  frameNext = frame + 1
+  if (frameNext >= numFrames):
+    frameNext = frame
+
+  frameFraction = frameReal - frame
+  #print("frameFraction=",frameFraction)
+  #print("frame=",frame)
+  #print("frameNext=", frameNext)
+
+  #getQuaternionSlerp
+
+  frameData = motion_dict['Frames'][frame]
+  frameDataNext = motion_dict['Frames'][frameNext]
+
+  #print("duration=",frameData[0])
+  #print(pos=[frameData])
+
+  basePos1Start = [frameData[1], frameData[2], frameData[3]]
+  basePos1End = [frameDataNext[1], frameDataNext[2], frameDataNext[3]]
+  basePos1 = [
+      basePos1Start[0] + frameFraction * (basePos1End[0] - basePos1Start[0]),
+      basePos1Start[1] + frameFraction * (basePos1End[1] - basePos1Start[1]),
+      basePos1Start[2] + frameFraction * (basePos1End[2] - basePos1Start[2])
+  ]
+  baseOrn1Start = [frameData[5], frameData[6], frameData[7], frameData[4]]
+  baseOrn1Next = [frameDataNext[5], frameDataNext[6], frameDataNext[7], frameDataNext[4]]
+  baseOrn1 = p.getQuaternionSlerp(baseOrn1Start, baseOrn1Next, frameFraction)
+  #pre-rotate	to make	z-up
+  if (useZUp):
+    y2zPos = [0, 0, 0.0]
+    y2zOrn = p.getQuaternionFromEuler([1.57, 0, 0])
+    basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, basePos1, baseOrn1)
+    p.resetBasePositionAndOrientation(humanoid, basePos, baseOrn)
+
+    y2zPos = [0, 2, 0.0]
+    y2zOrn = p.getQuaternionFromEuler([1.57, 0, 0])
+    basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, basePos1, baseOrn1)
+    p.resetBasePositionAndOrientation(humanoid2, basePos, baseOrn)
+
+  chestRotStart = [frameData[9], frameData[10], frameData[11], frameData[8]]
+  chestRotEnd = [frameDataNext[9], frameDataNext[10], frameDataNext[11], frameDataNext[8]]
+  chestRot = p.getQuaternionSlerp(chestRotStart, chestRotEnd, frameFraction)
+
+  neckRotStart = [frameData[13], frameData[14], frameData[15], frameData[12]]
+  neckRotEnd = [frameDataNext[13], frameDataNext[14], frameDataNext[15], frameDataNext[12]]
+  neckRot = p.getQuaternionSlerp(neckRotStart, neckRotEnd, frameFraction)
+
+  rightHipRotStart = [frameData[17], frameData[18], frameData[19], frameData[16]]
+  rightHipRotEnd = [frameDataNext[17], frameDataNext[18], frameDataNext[19], frameDataNext[16]]
+  rightHipRot = p.getQuaternionSlerp(rightHipRotStart, rightHipRotEnd, frameFraction)
+
+  rightKneeRotStart = [frameData[20]]
+  rightKneeRotEnd = [frameDataNext[20]]
+  rightKneeRot = [
+      rightKneeRotStart[0] + frameFraction * (rightKneeRotEnd[0] - rightKneeRotStart[0])
+  ]
+
+  rightAnkleRotStart = [frameData[22], frameData[23], frameData[24], frameData[21]]
+  rightAnkleRotEnd = [frameDataNext[22], frameDataNext[23], frameDataNext[24], frameDataNext[21]]
+  rightAnkleRot = p.getQuaternionSlerp(rightAnkleRotStart, rightAnkleRotEnd, frameFraction)
+
+  rightShoulderRotStart = [frameData[26], frameData[27], frameData[28], frameData[25]]
+  rightShoulderRotEnd = [
+      frameDataNext[26], frameDataNext[27], frameDataNext[28], frameDataNext[25]
+  ]
+  rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart, rightShoulderRotEnd,
+                                          frameFraction)
+
+  rightElbowRotStart = [frameData[29]]
+  rightElbowRotEnd = [frameDataNext[29]]
+  rightElbowRot = [
+      rightElbowRotStart[0] + frameFraction * (rightElbowRotEnd[0] - rightElbowRotStart[0])
+  ]
+
+  leftHipRotStart = [frameData[31], frameData[32], frameData[33], frameData[30]]
+  leftHipRotEnd = [frameDataNext[31], frameDataNext[32], frameDataNext[33], frameDataNext[30]]
+  leftHipRot = p.getQuaternionSlerp(leftHipRotStart, leftHipRotEnd, frameFraction)
+
+  leftKneeRotStart = [frameData[34]]
+  leftKneeRotEnd = [frameDataNext[34]]
+  leftKneeRot = [leftKneeRotStart[0] + frameFraction * (leftKneeRotEnd[0] - leftKneeRotStart[0])]
+
+  leftAnkleRotStart = [frameData[36], frameData[37], frameData[38], frameData[35]]
+  leftAnkleRotEnd = [frameDataNext[36], frameDataNext[37], frameDataNext[38], frameDataNext[35]]
+  leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart, leftAnkleRotEnd, frameFraction)
+
+  leftShoulderRotStart = [frameData[40], frameData[41], frameData[42], frameData[39]]
+  leftShoulderRotEnd = [frameDataNext[40], frameDataNext[41], frameDataNext[42], frameDataNext[39]]
+  leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart, leftShoulderRotEnd, frameFraction)
+  leftElbowRotStart = [frameData[43]]
+  leftElbowRotEnd = [frameDataNext[43]]
+  leftElbowRot = [
+      leftElbowRotStart[0] + frameFraction * (leftElbowRotEnd[0] - leftElbowRotStart[0])
+  ]
+
+  if (0):  #if (once):
+    p.resetJointStateMultiDof(humanoid, chest, chestRot)
+    p.resetJointStateMultiDof(humanoid, neck, neckRot)
+    p.resetJointStateMultiDof(humanoid, rightHip, rightHipRot)
+    p.resetJointStateMultiDof(humanoid, rightKnee, rightKneeRot)
+    p.resetJointStateMultiDof(humanoid, rightAnkle, rightAnkleRot)
+    p.resetJointStateMultiDof(humanoid, rightShoulder, rightShoulderRot)
+    p.resetJointStateMultiDof(humanoid, rightElbow, rightElbowRot)
+    p.resetJointStateMultiDof(humanoid, leftHip, leftHipRot)
+    p.resetJointStateMultiDof(humanoid, leftKnee, leftKneeRot)
+    p.resetJointStateMultiDof(humanoid, leftAnkle, leftAnkleRot)
+    p.resetJointStateMultiDof(humanoid, leftShoulder, leftShoulderRot)
+    p.resetJointStateMultiDof(humanoid, leftElbow, leftElbowRot)
+  once = False
+  #print("chestRot=",chestRot)
+  p.setGravity(0, 0, -10)
+
+  kp = kpMotor
+  if (useExplicitPD):
+    jointDofCounts = [4, 4, 4, 1, 4, 4, 1, 4, 1, 4, 4, 1]
+    #[x,y,z] base	position and [x,y,z,w] base	orientation!
+    totalDofs = 7
+    for dof in jointDofCounts:
+      totalDofs += dof
+
+    jointIndicesAll = [
+        chest, neck, rightHip, rightKnee, rightAnkle, rightShoulder, rightElbow, leftHip, leftKnee,
+        leftAnkle, leftShoulder, leftElbow
+    ]
+    basePos, baseOrn = p.getBasePositionAndOrientation(humanoid)
+    pose = [
+        basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2], baseOrn[3],
+        chestRot[0], chestRot[1], chestRot[2], chestRot[3], neckRot[0], neckRot[1], neckRot[2],
+        neckRot[3], rightHipRot[0], rightHipRot[1], rightHipRot[2], rightHipRot[3],
+        rightKneeRot[0], rightAnkleRot[0], rightAnkleRot[1], rightAnkleRot[2], rightAnkleRot[3],
+        rightShoulderRot[0], rightShoulderRot[1], rightShoulderRot[2], rightShoulderRot[3],
+        rightElbowRot[0], leftHipRot[0], leftHipRot[1], leftHipRot[2], leftHipRot[3],
+        leftKneeRot[0], leftAnkleRot[0], leftAnkleRot[1], leftAnkleRot[2], leftAnkleRot[3],
+        leftShoulderRot[0], leftShoulderRot[1], leftShoulderRot[2], leftShoulderRot[3],
+        leftElbowRot[0]
+    ]
+
+    #print("pose=")
+    #for po	in pose:
+    #	print(po)
+    
+    
+    taus = stablePD.computePD(bodyUniqueId=humanoid5,
+                                jointIndices=jointIndicesAll,
+                                desiredPositions=pose,
+                                desiredVelocities=[0] * totalDofs,
+                                kps=kpOrg,
+                                kds=kdOrg,
+                                maxForces=[maxForce] * totalDofs,
+                                timeStep=timeStep)
+    
+    indices = [chest, neck, rightHip, rightKnee,
+              rightAnkle, rightShoulder, rightElbow,
+               leftHip, leftKnee, leftAnkle,
+               leftShoulder, leftElbow]
+    targetPositions = [chestRot,neckRot,rightHipRot, rightKneeRot,
+                      rightAnkleRot, rightShoulderRot, rightElbowRot,
+                      leftHipRot, leftKneeRot, leftAnkleRot,
+                      leftShoulderRot, leftElbowRot]
+    maxForces = [  [maxForce,maxForce,maxForce], [maxForce,maxForce,maxForce],[maxForce,maxForce,maxForce],[maxForce],
+                  [maxForce,maxForce,maxForce],[maxForce,maxForce,maxForce],[maxForce],
+                  [maxForce,maxForce,maxForce], [maxForce], [maxForce,maxForce,maxForce],
+                  [maxForce,maxForce,maxForce], [maxForce]]
+    
+   
+    kps = [1000]*12
+    kds = [100]*12
+    
+                      
+    p.setJointMotorControlMultiDofArray(humanoid,
+                                   indices,
+                                   p.STABLE_PD_CONTROL,
+                                   targetPositions=targetPositions,
+                                   positionGains=kps,
+                                   velocityGains=kds,
+                                   forces=maxForces)
+
+    taus3 = explicitPD.computePD(bodyUniqueId=humanoid3,
+                                 jointIndices=jointIndicesAll,
+                                 desiredPositions=pose,
+                                 desiredVelocities=[0] * totalDofs,
+                                 kps=kpOrg,
+                                 kds=kdOrg,
+                                 maxForces=[maxForce * 0.05] * totalDofs,
+                                 timeStep=timeStep)
+
+    #taus=[0]*43
+    dofIndex = 7
+    for index in range(len(jointIndicesAll)):
+      jointIndex = jointIndicesAll[index]
+      if jointDofCounts[index] == 4:
+        
+        p.setJointMotorControlMultiDof(
+              humanoid5,
+            jointIndex,
+            p.TORQUE_CONTROL,
+            force=[taus[dofIndex + 0], taus[dofIndex + 1], taus[dofIndex + 2]])
+        p.setJointMotorControlMultiDof(
+            humanoid3,
+            jointIndex,
+            p.TORQUE_CONTROL,
+            force=[taus3[dofIndex + 0], taus3[dofIndex + 1], taus3[dofIndex + 2]])
+
+      if jointDofCounts[index] == 1:
+
+        
+        p.setJointMotorControlMultiDof(humanoid5,
+                                       jointIndex,
+                                       controlMode=p.TORQUE_CONTROL,
+                                       force=[taus[dofIndex]])
+        p.setJointMotorControlMultiDof(humanoid3,
+                                       jointIndex,
+                                       controlMode=p.TORQUE_CONTROL,
+                                       force=[taus3[dofIndex]])
+
+      dofIndex += jointDofCounts[index]
+
+    #print("len(taus)=",len(taus))
+    #print("taus=",taus)
+
+  p.setJointMotorControlMultiDof(humanoid2,
+                                 chest,
+                                 p.POSITION_CONTROL,
+                                 targetPosition=chestRot,
+                                 positionGain=kp,
+                                 force=[maxForce])
+  p.setJointMotorControlMultiDof(humanoid2,
+                                 neck,
+                                 p.POSITION_CONTROL,
+                                 targetPosition=neckRot,
+                                 positionGain=kp,
+                                 force=[maxForce])
+  p.setJointMotorControlMultiDof(humanoid2,
+                                 rightHip,
+                                 p.POSITION_CONTROL,
+                                 targetPosition=rightHipRot,
+                                 positionGain=kp,
+                                 force=[maxForce])
+  p.setJointMotorControlMultiDof(humanoid2,
+                                 rightKnee,
+                                 p.POSITION_CONTROL,
+                                 targetPosition=rightKneeRot,
+                                 positionGain=kp,
+                                 force=[maxForce])
+  p.setJointMotorControlMultiDof(humanoid2,
+                                 rightAnkle,
+                                 p.POSITION_CONTROL,
+                                 targetPosition=rightAnkleRot,
+                                 positionGain=kp,
+                                 force=[maxForce])
+  p.setJointMotorControlMultiDof(humanoid2,
+                                 rightShoulder,
+                                 p.POSITION_CONTROL,
+                                 targetPosition=rightShoulderRot,
+                                 positionGain=kp,
+                                 force=[maxForce])
+  p.setJointMotorControlMultiDof(humanoid2,
+                                 rightElbow,
+                                 p.POSITION_CONTROL,
+                                 targetPosition=rightElbowRot,
+                                 positionGain=kp,
+                                 force=[maxForce])
+  p.setJointMotorControlMultiDof(humanoid2,
+                                 leftHip,
+                                 p.POSITION_CONTROL,
+                                 targetPosition=leftHipRot,
+                                 positionGain=kp,
+                                 force=[maxForce])
+  p.setJointMotorControlMultiDof(humanoid2,
+                                 leftKnee,
+                                 p.POSITION_CONTROL,
+                                 targetPosition=leftKneeRot,
+                                 positionGain=kp,
+                                 force=[maxForce])
+  p.setJointMotorControlMultiDof(humanoid2,
+                                 leftAnkle,
+                                 p.POSITION_CONTROL,
+                                 targetPosition=leftAnkleRot,
+                                 positionGain=kp,
+                                 force=[maxForce])
+  p.setJointMotorControlMultiDof(humanoid2,
+                                 leftShoulder,
+                                 p.POSITION_CONTROL,
+                                 targetPosition=leftShoulderRot,
+                                 positionGain=kp,
+                                 force=[maxForce])
+  p.setJointMotorControlMultiDof(humanoid2,
+                                 leftElbow,
+                                 p.POSITION_CONTROL,
+                                 targetPosition=leftElbowRot,
+                                 positionGain=kp,
+                                 force=[maxForce])
+
+  kinematicHumanoid4 = True
+  if (kinematicHumanoid4):
+    p.resetJointStateMultiDof(humanoid4, chest, chestRot)
+    p.resetJointStateMultiDof(humanoid4, neck, neckRot)
+    p.resetJointStateMultiDof(humanoid4, rightHip, rightHipRot)
+    p.resetJointStateMultiDof(humanoid4, rightKnee, rightKneeRot)
+    p.resetJointStateMultiDof(humanoid4, rightAnkle, rightAnkleRot)
+    p.resetJointStateMultiDof(humanoid4, rightShoulder, rightShoulderRot)
+    p.resetJointStateMultiDof(humanoid4, rightElbow, rightElbowRot)
+    p.resetJointStateMultiDof(humanoid4, leftHip, leftHipRot)
+    p.resetJointStateMultiDof(humanoid4, leftKnee, leftKneeRot)
+    p.resetJointStateMultiDof(humanoid4, leftAnkle, leftAnkleRot)
+    p.resetJointStateMultiDof(humanoid4, leftShoulder, leftShoulderRot)
+    p.resetJointStateMultiDof(humanoid4, leftElbow, leftElbowRot)
+  p.stepSimulation()
+
+  if showJointMotorTorques:
+    for j in range(p.getNumJoints(humanoid2)):
+      jointState = p.getJointStateMultiDof(humanoid2, j)
+      print("jointStateMultiDof[", j, "].pos=", jointState[0])
+      print("jointStateMultiDof[", j, "].vel=", jointState[1])
+      print("jointStateMultiDof[", j, "].jointForces=", jointState[3])
+  time.sleep(timeStep)

Engine/lib/bullet/examples/pybullet/examples/humanoid_benchmark.py

@@ -0,0 +1,43 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.DIRECT)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setGravity(0, 0, -10)
+p.setPhysicsEngineParameter(numSolverIterations=5)
+p.setPhysicsEngineParameter(fixedTimeStep=1. / 240.)
+p.setPhysicsEngineParameter(numSubSteps=1)
+
+p.loadURDF("plane.urdf")
+
+objects = p.loadMJCF("mjcf/humanoid_symmetric.xml")
+ob = objects[0]
+p.resetBasePositionAndOrientation(ob, [0.789351, 0.962124, 0.113124],
+                                  [0.710965, 0.218117, 0.519402, -0.420923])
+jointPositions = [
+    -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000,
+    -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000,
+    0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000,
+    -0.007374, 0.000000
+]
+for jointIndex in range(p.getNumJoints(ob)):
+  p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
+
+#first let the humanoid fall
+#p.setRealTimeSimulation(1)
+#time.sleep(5)
+p.setRealTimeSimulation(0)
+#p.saveWorld("lyiing.py")
+
+#now do a benchmark
+print("Starting benchmark")
+fileName = "pybullet_humanoid_timings.json"
+
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, fileName)
+for i in range(1000):
+  p.stepSimulation()
+p.stopStateLogging(logId)
+
+print("ended benchmark")
+print("Use Chrome browser, visit about://tracing, and load the %s file" % fileName)

Engine/lib/bullet/examples/pybullet/examples/humanoid_knee_position_control.py

@@ -0,0 +1,56 @@
+import pybullet as p
+import time
+import pybullet_data
+
+
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+  cid = p.connect(p.GUI)
+
+p.resetSimulation()
+
+useRealTime = 0
+
+p.setRealTimeSimulation(useRealTime)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.setGravity(0, 0, -10)
+
+p.loadSDF("stadium.sdf")
+
+obUids = p.loadMJCF("mjcf/humanoid_fixed.xml")
+human = obUids[0]
+
+for i in range(p.getNumJoints(human)):
+  p.setJointMotorControl2(human, i, p.POSITION_CONTROL, targetPosition=0, force=500)
+
+kneeAngleTargetId = p.addUserDebugParameter("kneeAngle", -4, 4, -1)
+maxForceId = p.addUserDebugParameter("maxForce", 0, 500, 10)
+
+kneeAngleTargetLeftId = p.addUserDebugParameter("kneeAngleL", -4, 4, -1)
+maxForceLeftId = p.addUserDebugParameter("maxForceL", 0, 500, 10)
+
+kneeJointIndex = 11
+kneeJointIndexLeft = 18
+
+while (1):
+  time.sleep(0.01)
+  kneeAngleTarget = p.readUserDebugParameter(kneeAngleTargetId)
+  maxForce = p.readUserDebugParameter(maxForceId)
+  p.setJointMotorControl2(human,
+                          kneeJointIndex,
+                          p.POSITION_CONTROL,
+                          targetPosition=kneeAngleTarget,
+                          force=maxForce)
+  kneeAngleTargetLeft = p.readUserDebugParameter(kneeAngleTargetLeftId)
+  maxForceLeft = p.readUserDebugParameter(maxForceLeftId)
+  p.setJointMotorControl2(human,
+                          kneeJointIndexLeft,
+                          p.POSITION_CONTROL,
+                          targetPosition=kneeAngleTargetLeft,
+                          force=maxForceLeft)
+
+  if (useRealTime == 0):
+    p.stepSimulation()

Engine/lib/bullet/examples/pybullet/examples/humanoid_manual_control.py

@@ -0,0 +1,35 @@
+import pybullet as p
+import time
+
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+obUids = p.loadMJCF("mjcf/humanoid.xml")
+humanoid = obUids[1]
+
+gravId = p.addUserDebugParameter("gravity", -10, 10, -10)
+jointIds = []
+paramIds = []
+
+p.setPhysicsEngineParameter(numSolverIterations=10)
+p.changeDynamics(humanoid, -1, linearDamping=0, angularDamping=0)
+
+for j in range(p.getNumJoints(humanoid)):
+  p.changeDynamics(humanoid, j, linearDamping=0, angularDamping=0)
+  info = p.getJointInfo(humanoid, j)
+  #print(info)
+  jointName = info[1]
+  jointType = info[2]
+  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
+    jointIds.append(j)
+    paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, 0))
+
+p.setRealTimeSimulation(1)
+while (1):
+  p.setGravity(0, 0, p.readUserDebugParameter(gravId))
+  for i in range(len(paramIds)):
+    c = paramIds[i]
+    targetPos = p.readUserDebugParameter(c)
+    p.setJointMotorControl2(humanoid, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
+  time.sleep(0.01)

Engine/lib/bullet/examples/pybullet/examples/ik_end_effector_orientation.py

@@ -0,0 +1,44 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+from time import sleep
+
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+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)
+
+#Joint damping coefficents. Using large values for the joints that we don't want to move.
+jd = [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 0.5]
+#jd=[0.5,0.5,0.5,0.5,0.5,0.5,0.5]
+
+p.setGravity(0, 0, 0)
+
+while 1:
+  p.stepSimulation()
+  for i in range(1):
+    pos = [0, 0, 1.26]
+    orn = p.getQuaternionFromEuler([0, 0, 3.14])
+
+    jointPoses = p.calculateInverseKinematics(kukaId,
+                                              kukaEndEffectorIndex,
+                                              pos,
+                                              orn,
+                                              jointDamping=jd)
+
+  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)
+  sleep(0.05)

Engine/lib/bullet/examples/pybullet/examples/integrate.py

@@ -0,0 +1,15 @@
+import pybullet as p
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+cube = p.loadURDF("cube.urdf")
+frequency = 240
+timeStep = 1. / frequency
+p.setGravity(0, 0, -9.8)
+p.changeDynamics(cube, -1, linearDamping=0, angularDamping=0)
+p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
+for i in range(frequency):
+  p.stepSimulation()
+pos, orn = p.getBasePositionAndOrientation(cube)
+print(pos)

Engine/lib/bullet/examples/pybullet/examples/internalEdge.py

@@ -0,0 +1,43 @@
+import pybullet as p
+import time
+
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+if (1):
+  box_collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_BOX,
+                                                  halfExtents=[0.01, 0.01, 0.055])
+  box_mass = 0.1
+  box_visual_shape_id = -1
+  box_position = [0, 0.1, 1]
+  box_orientation = [0, 0, 0, 1]
+
+  p.createMultiBody(box_mass,
+                    box_collision_shape_id,
+                    box_visual_shape_id,
+                    box_position,
+                    box_orientation,
+                    useMaximalCoordinates=True)
+
+terrain_mass = 0
+terrain_visual_shape_id = -1
+terrain_position = [0, 0, 0]
+terrain_orientation = [0, 0, 0, 1]
+terrain_collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_MESH,
+                                                    fileName="terrain.obj",
+                                                    flags=p.GEOM_FORCE_CONCAVE_TRIMESH |
+                                                    p.GEOM_CONCAVE_INTERNAL_EDGE,
+                                                    meshScale=[0.5, 0.5, 0.5])
+p.createMultiBody(terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
+                  terrain_position, terrain_orientation)
+
+p.setGravity(0, 0, -10)
+
+pts = p.getContactPoints()
+print("num points=", len(pts))
+print(pts)
+while (p.isConnected()):
+  time.sleep(1. / 240.)
+  p.stepSimulation()

Engine/lib/bullet/examples/pybullet/examples/inverse_dynamics.py

@@ -0,0 +1,164 @@
+import pybullet as bullet
+plot = True
+import time
+
+if (plot):
+  import matplotlib.pyplot as plt
+import math
+verbose = False
+
+# Parameters:
+robot_base = [0., 0., 0.]
+robot_orientation = [0., 0., 0., 1.]
+delta_t = 0.0001
+
+# Initialize Bullet Simulator
+id_simulator = bullet.connect(bullet.GUI)  # or bullet.DIRECT for non-graphical version
+bullet.setTimeStep(delta_t)
+
+# Switch between URDF with/without FIXED joints
+with_fixed_joints = True
+
+if with_fixed_joints:
+  id_revolute_joints = [0, 3]
+  id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf",
+                             robot_base,
+                             robot_orientation,
+                             useFixedBase=True)
+else:
+  id_revolute_joints = [0, 1]
+  id_robot = bullet.loadURDF("TwoJointRobot_wo_fixedJoints.urdf",
+                             robot_base,
+                             robot_orientation,
+                             useFixedBase=True)
+
+bullet.changeDynamics(id_robot, -1, linearDamping=0, angularDamping=0)
+bullet.changeDynamics(id_robot, 0, linearDamping=0, angularDamping=0)
+bullet.changeDynamics(id_robot, 1, linearDamping=0, angularDamping=0)
+
+jointTypeNames = [
+    "JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED",
+    "JOINT_POINT2POINT", "JOINT_GEAR"
+]
+
+# Disable the motors for torque control:
+bullet.setJointMotorControlArray(id_robot,
+                                 id_revolute_joints,
+                                 bullet.VELOCITY_CONTROL,
+                                 forces=[0.0, 0.0])
+
+# Target Positions:
+start = 0.0
+end = 1.0
+
+steps = int((end - start) / delta_t)
+t = [0] * steps
+q_pos_desired = [[0.] * steps, [0.] * steps]
+q_vel_desired = [[0.] * steps, [0.] * steps]
+q_acc_desired = [[0.] * steps, [0.] * steps]
+
+for s in range(steps):
+  t[s] = start + s * delta_t
+  q_pos_desired[0][s] = 1. / (2. * math.pi) * math.sin(2. * math.pi * t[s]) - t[s]
+  q_pos_desired[1][s] = -1. / (2. * math.pi) * (math.cos(2. * math.pi * t[s]) - 1.0)
+
+  q_vel_desired[0][s] = math.cos(2. * math.pi * t[s]) - 1.
+  q_vel_desired[1][s] = math.sin(2. * math.pi * t[s])
+
+  q_acc_desired[0][s] = -2. * math.pi * math.sin(2. * math.pi * t[s])
+  q_acc_desired[1][s] = 2. * math.pi * math.cos(2. * math.pi * t[s])
+
+q_pos = [[0.] * steps, [0.] * steps]
+q_vel = [[0.] * steps, [0.] * steps]
+q_tor = [[0.] * steps, [0.] * steps]
+
+# Do Torque Control:
+for i in range(len(t)):
+
+  # Read Sensor States:
+  joint_states = bullet.getJointStates(id_robot, id_revolute_joints)
+
+  q_pos[0][i] = joint_states[0][0]
+  a = joint_states[1][0]
+  if (verbose):
+    print("joint_states[1][0]")
+    print(joint_states[1][0])
+  q_pos[1][i] = a
+
+  q_vel[0][i] = joint_states[0][1]
+  q_vel[1][i] = joint_states[1][1]
+
+  # Computing the torque from inverse dynamics:
+  obj_pos = [q_pos[0][i], q_pos[1][i]]
+  obj_vel = [q_vel[0][i], q_vel[1][i]]
+  obj_acc = [q_acc_desired[0][i], q_acc_desired[1][i]]
+
+  if (verbose):
+    print("calculateInverseDynamics")
+    print("id_robot")
+    print(id_robot)
+    print("obj_pos")
+    print(obj_pos)
+    print("obj_vel")
+    print(obj_vel)
+    print("obj_acc")
+    print(obj_acc)
+
+  torque = bullet.calculateInverseDynamics(id_robot, obj_pos, obj_vel, obj_acc)
+  q_tor[0][i] = torque[0]
+  q_tor[1][i] = torque[1]
+  if (verbose):
+    print("torque=")
+    print(torque)
+
+  # Set the Joint Torques:
+  bullet.setJointMotorControlArray(id_robot,
+                                   id_revolute_joints,
+                                   bullet.TORQUE_CONTROL,
+                                   forces=[torque[0], torque[1]])
+
+  # Step Simulation
+  bullet.stepSimulation()
+
+# Plot the Position, Velocity and Acceleration:
+if plot:
+  figure = plt.figure(figsize=[15, 4.5])
+  figure.subplots_adjust(left=0.05, bottom=0.11, right=0.97, top=0.9, wspace=0.4, hspace=0.55)
+
+  ax_pos = figure.add_subplot(141)
+  ax_pos.set_title("Joint Position")
+  ax_pos.plot(t, q_pos_desired[0], '--r', lw=4, label='Desired q0')
+  ax_pos.plot(t, q_pos_desired[1], '--b', lw=4, label='Desired q1')
+  ax_pos.plot(t, q_pos[0], '-r', lw=1, label='Measured q0')
+  ax_pos.plot(t, q_pos[1], '-b', lw=1, label='Measured q1')
+  ax_pos.set_ylim(-1., 1.)
+  ax_pos.legend()
+
+  ax_vel = figure.add_subplot(142)
+  ax_vel.set_title("Joint Velocity")
+  ax_vel.plot(t, q_vel_desired[0], '--r', lw=4, label='Desired q0')
+  ax_vel.plot(t, q_vel_desired[1], '--b', lw=4, label='Desired q1')
+  ax_vel.plot(t, q_vel[0], '-r', lw=1, label='Measured q0')
+  ax_vel.plot(t, q_vel[1], '-b', lw=1, label='Measured q1')
+  ax_vel.set_ylim(-2., 2.)
+  ax_vel.legend()
+
+  ax_acc = figure.add_subplot(143)
+  ax_acc.set_title("Joint Acceleration")
+  ax_acc.plot(t, q_acc_desired[0], '--r', lw=4, label='Desired q0')
+  ax_acc.plot(t, q_acc_desired[1], '--b', lw=4, label='Desired q1')
+  ax_acc.set_ylim(-10., 10.)
+  ax_acc.legend()
+
+  ax_tor = figure.add_subplot(144)
+  ax_tor.set_title("Executed Torque")
+  ax_tor.plot(t, q_tor[0], '-r', lw=2, label='Torque q0')
+  ax_tor.plot(t, q_tor[1], '-b', lw=2, label='Torque q1')
+  ax_tor.set_ylim(-20., 20.)
+  ax_tor.legend()
+
+  plt.pause(0.01)
+
+while (1):
+  bullet.stepSimulation()
+  time.sleep(0.01)

Engine/lib/bullet/examples/pybullet/examples/inverse_kinematics.py

@@ -0,0 +1,125 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+import pybullet_data
+
+clid = p.connect(p.SHARED_MEMORY)
+if (clid < 0):
+  p.connect(p.GUI)
+  #p.connect(p.SHARED_MEMORY_GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+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 = 1
+
+useOrientation = 1
+#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
+#This can be used to test the IK result accuracy.
+useSimulation = 1
+useRealTimeSimulation = 0
+ikSolver = 0
+p.setRealTimeSimulation(useRealTimeSimulation)
+#trailDuration is duration (in seconds) after debug lines will be removed automatically
+#use 0 for no-removal
+trailDuration = 15
+
+i=0
+while 1:
+  i+=1
+  #p.getCameraImage(320,
+  #                 200,
+  #                 flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
+  #                 renderer=p.ER_BULLET_HARDWARE_OPENGL)
+  if (useRealTimeSimulation):
+    dt = datetime.now()
+    t = (dt.second / 60.) * 2. * math.pi
+  else:
+    t = t + 0.01
+
+  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,
+                                                  solver=ikSolver,
+                                                  maxNumIterations=100,
+                                                  residualThreshold=.01)
+      else:
+        jointPoses = p.calculateInverseKinematics(kukaId,
+                                                  kukaEndEffectorIndex,
+                                                  pos,
+                                                  solver=ikSolver)
+
+    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
+p.disconnect()

Engine/lib/bullet/examples/pybullet/examples/inverse_kinematics_husky_kuka.py

@@ -0,0 +1,202 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+from datetime import datetime
+import pybullet_data
+
+clid = p.connect(p.SHARED_MEMORY)
+
+
+if (clid < 0):
+  p.connect(p.GUI)
+
+p.setPhysicsEngineParameter(enableConeFriction=0)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+
+p.loadURDF("plane.urdf", [0, 0, -0.3])
+husky = p.loadURDF("husky/husky.urdf", [0.290388, 0.329902, -0.310270],
+                   [0.002328, -0.000984, 0.996491, 0.083659])
+for i in range(p.getNumJoints(husky)):
+  print(p.getJointInfo(husky, i))
+kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749, 0.345564, 0.120208, 0.002327,
+                    -0.000988, 0.996491, 0.083659)
+ob = kukaId
+jointPositions = [3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001]
+for jointIndex in range(p.getNumJoints(ob)):
+  p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
+
+#put kuka on top of husky
+
+cid = p.createConstraint(husky, -1, kukaId, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0., 0., -.5],
+                         [0, 0, 0, 1])
+
+baseorn = p.getQuaternionFromEuler([3.1415, 0, 0.3])
+baseorn = [0, 0, 0, 1]
+#[0, 0, 0.707, 0.707]
+
+#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[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, -10)
+t = 0.
+prevPose = [0, 0, 0]
+prevPose1 = [0, 0, 0]
+hasPrevPose = 0
+useNullSpace = 0
+
+useOrientation = 0
+#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
+#This can be used to test the IK result accuracy.
+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
+basepos = [0, 0, 0]
+ang = 0
+ang = 0
+
+
+def accurateCalculateInverseKinematics(kukaId, endEffectorId, targetPos, threshold, maxIter):
+  closeEnough = False
+  iter = 0
+  dist2 = 1e30
+  while (not closeEnough and iter < maxIter):
+    jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, targetPos)
+    for i in range(numJoints):
+      p.resetJointState(kukaId, i, jointPoses[i])
+    ls = p.getLinkState(kukaId, kukaEndEffectorIndex)
+    newPos = ls[4]
+    diff = [targetPos[0] - newPos[0], targetPos[1] - newPos[1], targetPos[2] - newPos[2]]
+    dist2 = (diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2])
+    closeEnough = (dist2 < threshold)
+    iter = iter + 1
+  #print ("Num iter: "+str(iter) + "threshold: "+str(dist2))
+  return jointPoses
+
+
+wheels = [2, 3, 4, 5]
+#(2, b'front_left_wheel', 0, 7, 6, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_left_wheel_link')
+#(3, b'front_right_wheel', 0, 8, 7, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_right_wheel_link')
+#(4, b'rear_left_wheel', 0, 9, 8, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_left_wheel_link')
+#(5, b'rear_right_wheel', 0, 10, 9, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_right_wheel_link')
+wheelVelocities = [0, 0, 0, 0]
+wheelDeltasTurn = [1, -1, 1, -1]
+wheelDeltasFwd = [1, 1, 1, 1]
+while 1:
+  keys = p.getKeyboardEvents()
+  shift = 0.01
+  wheelVelocities = [0, 0, 0, 0]
+  speed = 1.0
+  for k in keys:
+    if ord('s') in keys:
+      p.saveWorld("state.py")
+    if ord('a') in keys:
+      basepos = basepos = [basepos[0], basepos[1] - shift, basepos[2]]
+    if ord('d') in keys:
+      basepos = basepos = [basepos[0], basepos[1] + shift, basepos[2]]
+
+    if p.B3G_LEFT_ARROW in keys:
+      for i in range(len(wheels)):
+        wheelVelocities[i] = wheelVelocities[i] - speed * wheelDeltasTurn[i]
+    if p.B3G_RIGHT_ARROW in keys:
+      for i in range(len(wheels)):
+        wheelVelocities[i] = wheelVelocities[i] + speed * wheelDeltasTurn[i]
+    if p.B3G_UP_ARROW in keys:
+      for i in range(len(wheels)):
+        wheelVelocities[i] = wheelVelocities[i] + speed * wheelDeltasFwd[i]
+    if p.B3G_DOWN_ARROW in keys:
+      for i in range(len(wheels)):
+        wheelVelocities[i] = wheelVelocities[i] - speed * wheelDeltasFwd[i]
+
+  baseorn = p.getQuaternionFromEuler([0, 0, ang])
+  for i in range(len(wheels)):
+    p.setJointMotorControl2(husky,
+                            wheels[i],
+                            p.VELOCITY_CONTROL,
+                            targetVelocity=wheelVelocities[i],
+                            force=1000)
+  #p.resetBasePositionAndOrientation(kukaId,basepos,baseorn)#[0,0,0,1])
+  if (useRealTimeSimulation):
+    t = time.time()  #(dt, micro) = datetime.utcnow().strftime('%Y-%m-%d %H:%M:%S.%f').split('.')
+    #t = (dt.second/60.)*2.*math.pi
+  else:
+    t = t + 0.001
+
+  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)]
+    pos = [0.2 * math.cos(t), 0, 0. + 0.2 * math.sin(t) + 0.7]
+    #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:
+        threshold = 0.001
+        maxIter = 100
+        jointPoses = accurateCalculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos,
+                                                        threshold, maxIter)
+
+    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=1,
+                                velocityGain=0.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

Engine/lib/bullet/examples/pybullet/examples/inverse_kinematics_pole.py

@@ -0,0 +1,66 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+import pybullet_data
+
+clid = p.connect(p.SHARED_MEMORY)
+if (clid < 0):
+  p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF("plane.urdf", [0, 0, -1.3])
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+sawyerId = p.loadURDF("pole.urdf", [0, 0, 0])
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+p.resetBasePositionAndOrientation(sawyerId, [0, 0, 0], [0, 0, 0, 1])
+
+sawyerEndEffectorIndex = 3
+numJoints = p.getNumJoints(sawyerId)
+#joint damping coefficents
+jd = [0.1, 0.1, 0.1, 0.1]
+
+p.setGravity(0, 0, 0)
+t = 0.
+prevPose = [0, 0, 0]
+prevPose1 = [0, 0, 0]
+hasPrevPose = 0
+
+ikSolver = 0
+useRealTimeSimulation = 0
+p.setRealTimeSimulation(useRealTimeSimulation)
+#trailDuration is duration (in seconds) after debug lines will be removed automatically
+#use 0 for no-removal
+trailDuration = 1
+
+while 1:
+  if (useRealTimeSimulation):
+    dt = datetime.now()
+    t = (dt.second / 60.) * 2. * math.pi
+  else:
+    t = t + 0.01
+    time.sleep(0.01)
+
+  for i in range(1):
+    pos = [2. * math.cos(t), 2. * math.cos(t), 0. + 2. * math.sin(t)]
+    jointPoses = p.calculateInverseKinematics(sawyerId,
+                                              sawyerEndEffectorIndex,
+                                              pos,
+                                              jointDamping=jd,
+                                              solver=ikSolver,
+                                              maxNumIterations=100)
+
+    #reset the joint state (ignoring all dynamics, not recommended to use during simulation)
+    for i in range(numJoints):
+      jointInfo = p.getJointInfo(sawyerId, i)
+      qIndex = jointInfo[3]
+      if qIndex > -1:
+        p.resetJointState(sawyerId, i, jointPoses[qIndex - 7])
+
+  ls = p.getLinkState(sawyerId, sawyerEndEffectorIndex)
+  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

Engine/lib/bullet/examples/pybullet/examples/inverted_pendulum_tendon_actuation.py

@@ -0,0 +1,167 @@
+"""
+https://valerolab.org/
+
+PID control of an inverted pendulum actuated by strings.
+"""
+
+import pybullet as p
+import time
+import math as m
+import numpy as np
+import pybullet_data
+import matplotlib.pyplot as plt
+
+
+p.connect(p.GUI)
+plane = p.loadURDF("plane.urdf")
+
+"""_____________________________________________________________________________________________________________________________"""
+"""Gains, motor forces, daq and timing parameters"""
+
+""" Gains for pendulum angle"""
+proportional_gain = 30000
+integral_gain = 18000
+derivative_gain = 22000
+
+"""Motor force parameters"""
+tension_force = 600
+
+"""Control input parameters"""
+u_factor = 1.5
+u_lower_limit =  tension_force
+u_upper_limit=9000
+
+"""Data aquisition, timing and history"""
+time_steps = 2000
+history = np.array( [[1000,-1000,0]] )
+time_history = np.array([[0]])
+previous_pendulum_angle = 0
+previous_cart_position = 0
+
+"""_____________________________________________________________________________________________________________________________"""
+"""Loading URDF files"""
+
+cubeStartPos = [-2.15,0,.75]
+cubeStartPos2 = [0,0,1.4]
+cubeStartPos3 = [2.15,0,.75]
+
+PulleyStartOrientation = p.getQuaternionFromEuler([1.570796, 0, 0])  
+cubeStartOrientation = p.getQuaternionFromEuler([0,0,0]) 
+cubeStartOrientation2 = p.getQuaternionFromEuler([0,-1.570796,0])
+
+base_1 = p.loadURDF("Base_1.urdf",cubeStartPos3, cubeStartOrientation, useFixedBase=1, flags=p.URDF_USE_SELF_COLLISION) #Base 1: magenta base and tendon
+base_2 = p.loadURDF("Base_2.urdf",cubeStartPos, cubeStartOrientation, useFixedBase=1, flags=p.URDF_USE_SELF_COLLISION)  #Base 2: white base and tendon
+pendulum = p.loadURDF("Pendulum_Tendon_1_Cart_Rail.urdf",cubeStartPos2, cubeStartOrientation2, useFixedBase=1, flags=p.URDF_USE_SELF_COLLISION) 
+
+
+"""_____________________________________________________________________________________________________________________________"""
+"""Getting access and information from specific joints in each body (each body has links and joint described in the URDF files):"""
+nJoints = p.getNumJoints(base_1)  #Base 1: magenta base and tendon
+jointNameToId = {}
+for i in range(nJoints):
+  jointInfo = p.getJointInfo(base_1, i)
+  jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
+Base_pulley_1 = jointNameToId['Base_pulley1']
+
+nJoints = p.getNumJoints(pendulum)
+jointNameToId = {}
+for i in range(nJoints):
+  jointInfo = p.getJointInfo(pendulum, i)
+  jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
+last_tendon_link_1 = jointNameToId['tendon1_13_tendon1_14']
+cart_pendulumAxis = jointNameToId['cart_pendulumAxis']
+cart = jointNameToId['slider_cart']
+
+nJoints = p.getNumJoints(base_2)  #Base 2: white base and tendon
+jointNameToId = {}
+for i in range(nJoints):
+  jointInfo = p.getJointInfo(base_2, i)
+  jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
+last_tendon_link_2 = jointNameToId['tendon1_13_tendon1_14']
+Base_pulley_2 = jointNameToId['Base_pulley1']
+"""_____________________________________________________________________________________________________________________________"""
+"""Creating new contraints (joints), with the information obtained in the previous step"""
+
+pulley_1_tendon_magenta = p.createConstraint(base_1, Base_pulley_1, pendulum, last_tendon_link_1, p.JOINT_FIXED, [0, 0, 1], [0, 0, 0], [-.56, 0, 0])
+tendon_white_cart = p.createConstraint(base_2, last_tendon_link_2, pendulum, cart, p.JOINT_FIXED, [0, 0, 1], [0, 0, 0], [0,-.55, 0])
+
+"""_____________________________________________________________________________________________________________________________"""
+"""Defining some motor conditions"""
+p.setJointMotorControl2(pendulum, cart_pendulumAxis, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
+p.setJointMotorControl2(base_1, Base_pulley_1, p.VELOCITY_CONTROL, targetVelocity=10, force=1000) #Base 1: magenta base and tendon
+p.setJointMotorControl2(base_2, Base_pulley_2, p.VELOCITY_CONTROL, targetVelocity=10, force=-1000)#Base 2: white base and tendon
+
+
+"""_____________________________________________________________________________________________________________________________"""
+
+
+p.setGravity(0,0,-10)
+
+
+for i in range (time_steps):
+    p.stepSimulation()
+    pendulum_angle = p.getJointState(pendulum,cart_pendulumAxis)
+    pendulum_angle = pendulum_angle[0]
+    angle_delta_error = -pendulum_angle
+
+    #PROPPORTIONAL
+    p_correction = proportional_gain * pendulum_angle
+
+    #INTEGRAL
+    i_correction = integral_gain * (previous_pendulum_angle + pendulum_angle)
+    previous_pendulum_angle = pendulum_angle
+
+    #DERIVATIVE
+    d_correction = derivative_gain * angle_delta_error
+
+    u = p_correction + i_correction + d_correction + 10
+    
+    u = abs(u)
+    if u<u_lower_limit:
+      u=u_lower_limit
+    elif u>u_upper_limit:
+      u=u_upper_limit   
+    
+    if pendulum_angle > 0:
+      u_pulley_1 = u * u_factor   #Base 1: magenta base and tendon
+      u_pulley_2 = -tension_force #Base 2: white base and tendon
+      #print(">0")
+    else:
+      u_pulley_1 = tension_force  #Base 1: magenta base and tendon
+      u_pulley_2 = -u * u_factor  #Base 2: white base and tendon
+      #print("<0")
+
+    p.setJointMotorControl2(base_1, Base_pulley_1, p.VELOCITY_CONTROL, targetVelocity=100, force = u_pulley_1)  
+    p.setJointMotorControl2(base_2, Base_pulley_2, p.VELOCITY_CONTROL, targetVelocity=100, force = u_pulley_2)  
+
+    history = np.append(history , [[ u_pulley_1, u_pulley_2, pendulum_angle]] , axis = 0)    
+        
+    time.sleep(1./240.)
+
+print("Done with simulation")
+
+fig, ax1 = plt.subplots()
+ax1.set_xlabel("Time Steps")
+ax1.set_ylabel("Activation Values")
+ax1.plot(history[:,0],label="u_pulley_1")
+ax1.plot(history[:,1],label="u_pulley_2")
+ax1.set_ylim((-12000,12000))
+
+plt.legend(loc='best', bbox_to_anchor=(0.5, 0., 0.5, 0.5),
+           ncol=1, mode=None, borderaxespad=0.)
+plt.title("Ctrl Input and Angle History") 
+plt.grid(True)
+color = 'tab:red'
+ax2 = ax1.twinx()
+ax2.set_ylabel('Pendulum Angle', color=color)
+ax2.plot(np.rad2deg(history[:,2]),label="Angle",color=color)
+ax2.tick_params(axis='y', labelcolor=color)
+ax2.set_ylim((-90,90))
+
+fig.tight_layout()
+plt.show()
+    
+p.disconnect()
+
+
+

Engine/lib/bullet/examples/pybullet/examples/jacobian.py

@@ -0,0 +1,102 @@
+import pybullet as p
+import pybullet_data
+
+
+
+def getJointStates(robot):
+  joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
+  joint_positions = [state[0] for state in joint_states]
+  joint_velocities = [state[1] for state in joint_states]
+  joint_torques = [state[3] for state in joint_states]
+  return joint_positions, joint_velocities, joint_torques
+
+
+def getMotorJointStates(robot):
+  joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
+  joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
+  joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1]
+  joint_positions = [state[0] for state in joint_states]
+  joint_velocities = [state[1] for state in joint_states]
+  joint_torques = [state[3] for state in joint_states]
+  return joint_positions, joint_velocities, joint_torques
+
+
+def setJointPosition(robot, position, kp=1.0, kv=0.3):
+  num_joints = p.getNumJoints(robot)
+  zero_vec = [0.0] * num_joints
+  if len(position) == num_joints:
+    p.setJointMotorControlArray(robot,
+                                range(num_joints),
+                                p.POSITION_CONTROL,
+                                targetPositions=position,
+                                targetVelocities=zero_vec,
+                                positionGains=[kp] * num_joints,
+                                velocityGains=[kv] * num_joints)
+  else:
+    print("Not setting torque. "
+          "Expected torque vector of "
+          "length {}, got {}".format(num_joints, len(torque)))
+
+
+def multiplyJacobian(robot, jacobian, vector):
+  result = [0.0, 0.0, 0.0]
+  i = 0
+  for c in range(len(vector)):
+    if p.getJointInfo(robot, c)[3] > -1:
+      for r in range(3):
+        result[r] += jacobian[r][i] * vector[c]
+      i += 1
+  return result
+
+
+clid = p.connect(p.SHARED_MEMORY)
+if (clid < 0):
+  p.connect(p.DIRECT)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+time_step = 0.001
+gravity_constant = -9.81
+p.resetSimulation()
+p.setTimeStep(time_step)
+p.setGravity(0.0, 0.0, gravity_constant)
+
+p.loadURDF("plane.urdf", [0, 0, -0.3])
+
+kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True)
+#kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0])
+#kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
+#kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0])
+#kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0])
+p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
+numJoints = p.getNumJoints(kukaId)
+kukaEndEffectorIndex = numJoints - 1
+
+# Set a joint target for the position control and step the sim.
+setJointPosition(kukaId, [0.1] * numJoints)
+p.stepSimulation()
+
+# Get the joint and link state directly from Bullet.
+pos, vel, torq = getJointStates(kukaId)
+mpos, mvel, mtorq = getMotorJointStates(kukaId)
+
+result = p.getLinkState(kukaId,
+                        kukaEndEffectorIndex,
+                        computeLinkVelocity=1,
+                        computeForwardKinematics=1)
+link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
+# Get the Jacobians for the CoM of the end-effector link.
+# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
+# The localPosition is always defined in terms of the link frame coordinates.
+
+zero_vec = [0.0] * len(mpos)
+jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, mpos, zero_vec, zero_vec)
+
+print("Link linear velocity of CoM from getLinkState:")
+print(link_vt)
+print("Link linear velocity of CoM from linearJacobian * q_dot:")
+print(multiplyJacobian(kukaId, jac_t, vel))
+print("Link angular velocity of CoM from getLinkState:")
+print(link_vr)
+print("Link angular velocity of CoM from angularJacobian * q_dot:")
+print(multiplyJacobian(kukaId, jac_r, vel))

Engine/lib/bullet/examples/pybullet/examples/jointFrictionAndMotor.py

@@ -0,0 +1,27 @@
+import pybullet as p
+import time
+
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+door = p.loadURDF("door.urdf")
+
+#linear/angular damping for base and all children=0
+p.changeDynamics(door, -1, linearDamping=0, angularDamping=0)
+for j in range(p.getNumJoints(door)):
+  p.changeDynamics(door, j, linearDamping=0, angularDamping=0)
+  print(p.getJointInfo(door, j))
+
+frictionId = p.addUserDebugParameter("jointFriction", 0, 20, 10)
+torqueId = p.addUserDebugParameter("joint torque", 0, 20, 5)
+
+while (1):
+  frictionForce = p.readUserDebugParameter(frictionId)
+  jointTorque = p.readUserDebugParameter(torqueId)
+  #set the joint friction
+  p.setJointMotorControl2(door, 1, p.VELOCITY_CONTROL, targetVelocity=0, force=frictionForce)
+  #apply a joint torque
+  p.setJointMotorControl2(door, 1, p.TORQUE_CONTROL, force=jointTorque)
+  p.stepSimulation()
+  time.sleep(0.01)

Engine/lib/bullet/examples/pybullet/examples/jointFrictionDamping.py

@@ -0,0 +1,38 @@
+import pybullet as p
+import time
+
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF("plane.urdf", [0, 0, -0.25])
+minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True)
+print(p.getNumJoints(minitaur))
+p.resetDebugVisualizerCamera(cameraDistance=1,
+                             cameraYaw=23.2,
+                             cameraPitch=-6.6,
+                             cameraTargetPosition=[-0.064, .621, -0.2])
+motorJointId = 1
+p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0)
+
+p.resetJointState(minitaur, motorJointId, targetValue=0, targetVelocity=1)
+angularDampingSlider = p.addUserDebugParameter("angularDamping", 0, 1, 0)
+jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0, 0.1, 0)
+
+textId = p.addUserDebugText("jointVelocity=0", [0, 0, -0.2])
+p.setRealTimeSimulation(1)
+while (1):
+  frictionForce = p.readUserDebugParameter(jointFrictionForceSlider)
+  angularDamping = p.readUserDebugParameter(angularDampingSlider)
+  p.setJointMotorControl2(minitaur,
+                          motorJointId,
+                          p.VELOCITY_CONTROL,
+                          targetVelocity=0,
+                          force=frictionForce)
+  p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping)
+
+  time.sleep(0.01)
+  txt = "jointVelocity=" + str(p.getJointState(minitaur, motorJointId)[1])
+  prevTextId = textId
+  textId = p.addUserDebugText(txt, [0, 0, -0.2])
+  p.removeUserDebugItem(prevTextId)

Engine/lib/bullet/examples/pybullet/examples/kuka_grasp_block_playback.py

@@ -0,0 +1,87 @@
+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)
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+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)

Engine/lib/bullet/examples/pybullet/examples/kuka_with_cube.py

@@ -0,0 +1,117 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+
+#clid = p.connect(p.SHARED_MEMORY)
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+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
+
+logId1 = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "LOG0001.txt", [0, 1, 2])
+logId2 = p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS, "LOG0002.txt", bodyUniqueIdA=2)
+
+for i in range(5):
+  print("Body %d's name is %s." % (i, p.getBodyInfo(i)[1]))
+
+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

Engine/lib/bullet/examples/pybullet/examples/kuka_with_cube_playback.py

@@ -0,0 +1,85 @@
+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)
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF("plane.urdf", [0, 0, -0.3])
+p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 1])
+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)

Engine/lib/bullet/examples/pybullet/examples/load_soft_body.py

@@ -0,0 +1,33 @@
+import pybullet as p
+from time import sleep
+import pybullet_data
+
+
+physicsClient = p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setGravity(0, 0, -10)
+planeId = p.loadURDF("plane.urdf", [0,0,-2])
+boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
+bunnyId = p.loadSoftBody("bunny.obj")#.obj")#.vtk")
+
+#meshData = p.getMeshData(bunnyId)
+#print("meshData=",meshData)
+#p.loadURDF("cube_small.urdf", [1, 0, 1])
+useRealTimeSimulation = 1
+
+if (useRealTimeSimulation):
+  p.setRealTimeSimulation(1)
+
+print(p.getDynamicsInfo(planeId, -1))
+#print(p.getDynamicsInfo(bunnyId, 0))
+print(p.getDynamicsInfo(boxId, -1))
+p.changeDynamics(boxId,-1,mass=10)
+while p.isConnected():
+  p.setGravity(0, 0, -10)
+  if (useRealTimeSimulation):
+
+    sleep(0.01)  # Time in seconds.
+    #p.getCameraImage(320,200,renderer=p.ER_BULLET_HARDWARE_OPENGL )
+  else:
+    p.stepSimulation()

Engine/lib/bullet/examples/pybullet/examples/loadingBench.py

@@ -0,0 +1,29 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.resetSimulation()
+timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json")
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+print("load plane.urdf")
+p.loadURDF("plane.urdf")
+print("load r2d2.urdf")
+
+p.loadURDF("r2d2.urdf", 0, 0, 1)
+print("load kitchen/1.sdf")
+p.loadSDF("kitchens/1.sdf")
+print("load 100 times plate.urdf")
+for i in range(100):
+  p.loadURDF("dinnerware/plate.urdf", 0, i, 1)
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+
+p.stopStateLogging(timinglog)
+print("stopped state logging")
+p.getCameraImage(320, 200)
+
+while (1):
+  p.stepSimulation()

Engine/lib/bullet/examples/pybullet/examples/logMinitaur.py

@@ -0,0 +1,20 @@
+import pybullet as p
+import pybullet_data
+
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+  p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+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)

Engine/lib/bullet/examples/pybullet/examples/manyspheres.py

@@ -0,0 +1,38 @@
+import pybullet as p
+import time
+import pybullet_data
+
+
+conid = p.connect(p.SHARED_MEMORY)
+if (conid < 0):
+  p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setInternalSimFlags(0)
+p.resetSimulation()
+
+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)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+for i in range(10):
+  for j in range(10):
+    for k in range(10):
+      ob = p.loadURDF("sphere_1cm.urdf", [0.02 * i, 0.02 * j, 0.2 + 0.02 * k],
+                      useMaximalCoordinates=True)
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+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)

Engine/lib/bullet/examples/pybullet/examples/mimicJointConstraint.py

@@ -0,0 +1,52 @@
+#a mimic joint can act as a gear between two joints
+#you can control the gear ratio in magnitude and sign (>0 reverses direction)
+
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.resetDebugVisualizerCamera(cameraDistance=1.1, cameraYaw = 3.6,cameraPitch=-27, cameraTargetPosition=[0.19,1.21,-0.44])
+
+p.loadURDF("plane.urdf", 0, 0, -2)
+wheelA = p.loadURDF("differential/diff_ring.urdf", [0, 0, 0])
+for i in range(p.getNumJoints(wheelA)):
+  print(p.getJointInfo(wheelA, i))
+  p.setJointMotorControl2(wheelA, i, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
+
+c = p.createConstraint(wheelA,
+                       1,
+                       wheelA,
+                       3,
+                       jointType=p.JOINT_GEAR,
+                       jointAxis=[0, 1, 0],
+                       parentFramePosition=[0, 0, 0],
+                       childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=1, maxForce=10000,erp=0.2)
+
+c = p.createConstraint(wheelA,
+                       2,
+                       wheelA,
+                       4,
+                       jointType=p.JOINT_GEAR,
+                       jointAxis=[0, 1, 0],
+                       parentFramePosition=[0, 0, 0],
+                       childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=-1, maxForce=10000,erp=0.2)
+
+c = p.createConstraint(wheelA,
+                       1,
+                       wheelA,
+                       4,
+                       jointType=p.JOINT_GEAR,
+                       jointAxis=[0, 1, 0],
+                       parentFramePosition=[0, 0, 0],
+                       childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=-1, maxForce=10000,erp=0.2)
+
+p.setRealTimeSimulation(1)
+while (1):
+  p.setGravity(0, 0, -10)
+  time.sleep(0.01)
+#p.removeConstraint(c)

Engine/lib/bullet/examples/pybullet/examples/minitaur.py

@@ -0,0 +1,194 @@
+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

Engine/lib/bullet/examples/pybullet/examples/minitaur_evaluate.py

@@ -0,0 +1,112 @@
+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

Engine/lib/bullet/examples/pybullet/examples/minitaur_test.py

@@ -0,0 +1,39 @@
+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
+import pybullet_data
+
+
+
+
+def main(unused_args):
+  timeStep = 0.01
+  c = p.connect(p.SHARED_MEMORY)
+  if (c < 0):
+    c = p.connect(p.GUI)
+  p.setAdditionalSearchPath(pybullet_data.getDataPath())
+  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)

Engine/lib/bullet/examples/pybullet/examples/motorMaxVelocity.py

@@ -0,0 +1,22 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+cartpole = p.loadURDF("cartpole.urdf")
+p.setRealTimeSimulation(1)
+p.setJointMotorControl2(cartpole,
+                        1,
+                        p.POSITION_CONTROL,
+                        targetPosition=1000,
+                        targetVelocity=0,
+                        force=1000,
+                        positionGain=1,
+                        velocityGain=0,
+                        maxVelocity=0.5)
+while (1):
+  p.setGravity(0, 0, -10)
+  js = p.getJointState(cartpole, 1)
+  print("position=", js[0], "velocity=", js[1])
+  time.sleep(0.01)

Engine/lib/bullet/examples/pybullet/examples/otherPhysicsEngine.py

@@ -0,0 +1,108 @@
+import pybullet as p
+import pybullet_data as pd
+import time
+import math
+import pybullet_data
+
+usePhysX = True
+useMaximalCoordinates = True
+if usePhysX:
+  p.connect(p.PhysX, options="--numCores=8 --solver=pgs")
+  p.loadPlugin("eglRendererPlugin")
+else:
+  p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setPhysicsEngineParameter(fixedTimeStep=1. / 240.,
+                            numSolverIterations=4,
+                            minimumSolverIslandSize=1024)
+p.setPhysicsEngineParameter(contactBreakingThreshold=0.01)
+
+p.setAdditionalSearchPath(pd.getDataPath())
+#Always make ground plane maximal coordinates, to avoid performance drop in PhysX
+#See https://github.com/NVIDIAGameWorks/PhysX/issues/71
+
+p.loadURDF("plane.urdf", useMaximalCoordinates=True)  #useMaximalCoordinates)
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "physx_create_dominoes.json")
+jran = 50
+iran = 100
+
+num = 64
+radius = 0.1
+numDominoes = 0
+
+for i in range(int(num * 50)):
+  num = (radius * 2 * math.pi) / 0.08
+  radius += 0.05 / float(num)
+  orn = p.getQuaternionFromEuler([0, 0, 0.5 * math.pi + math.pi * 2 * i / float(num)])
+  pos = [
+      radius * math.cos(2 * math.pi * (i / float(num))),
+      radius * math.sin(2 * math.pi * (i / float(num))), 0.03
+  ]
+  sphere = p.loadURDF("domino/domino.urdf", pos, orn, useMaximalCoordinates=useMaximalCoordinates)
+  numDominoes += 1
+
+pos = [pos[0], pos[1], pos[2] + 0.3]
+orn = p.getQuaternionFromEuler([0, 0, -math.pi / 4.])
+sphere = p.loadURDF("domino/domino.urdf", pos, orn, useMaximalCoordinates=useMaximalCoordinates)
+
+print("numDominoes=", numDominoes)
+
+#for j in range (20):
+#    for i in range (100):
+#        if (i<99):
+#          sphere = p.loadURDF("domino/domino.urdf",[i*0.04,1+j*.25,0.03], useMaximalCoordinates=useMaximalCoordinates)
+#        else:
+#          orn = p.getQuaternionFromEuler([0,-3.14*0.24,0])
+#          sphere = p.loadURDF("domino/domino.urdf",[(i-1)*0.04,1+j*.25,0.03], orn, useMaximalCoordinates=useMaximalCoordinates)
+
+print("loaded!")
+
+#p.changeDynamics(sphere ,-1, mass=1000)
+
+door = p.loadURDF("door.urdf", [0, 0, -11])
+p.changeDynamics(door, 1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
+print("numJoints = ", p.getNumJoints(door))
+
+p.setGravity(0, 0, -10)
+position_control = True
+
+angle = math.pi * 0.25
+p.resetJointState(door, 1, angle)
+angleread = p.getJointState(door, 1)
+print("angleread = ", angleread)
+prevTime = time.time()
+
+angle = math.pi * 0.5
+
+count = 0
+while (1):
+  count += 1
+  if (count == 12):
+    p.stopStateLogging(logId)
+    p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+
+  curTime = time.time()
+
+  diff = curTime - prevTime
+  #every second, swap target angle
+  if (diff > 1):
+    angle = -angle
+    prevTime = curTime
+
+  if position_control:
+    p.setJointMotorControl2(door,
+                            1,
+                            p.POSITION_CONTROL,
+                            targetPosition=angle,
+                            positionGain=10.1,
+                            velocityGain=1,
+                            force=11.001)
+  else:
+    p.setJointMotorControl2(door, 1, p.VELOCITY_CONTROL, targetVelocity=1, force=1011)
+  #contacts = p.getContactPoints()
+  #print("contacts=",contacts)
+  p.stepSimulation()
+  #time.sleep(1./240.)

Engine/lib/bullet/examples/pybullet/examples/pdControl.py

@@ -0,0 +1,152 @@
+import pybullet as p
+from pdControllerExplicit import PDControllerExplicitMultiDof
+from pdControllerExplicit import PDControllerExplicit
+from pdControllerStable import PDControllerStable
+
+import time
+
+useMaximalCoordinates = False
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+pole = p.loadURDF("cartpole.urdf", [0, 0, 0], useMaximalCoordinates=useMaximalCoordinates)
+pole2 = p.loadURDF("cartpole.urdf", [0, 1, 0], useMaximalCoordinates=useMaximalCoordinates)
+pole3 = p.loadURDF("cartpole.urdf", [0, 2, 0], useMaximalCoordinates=useMaximalCoordinates)
+pole4 = p.loadURDF("cartpole.urdf", [0, 3, 0], useMaximalCoordinates=useMaximalCoordinates)
+
+exPD = PDControllerExplicitMultiDof(p)
+sPD = PDControllerStable(p)
+
+for i in range(p.getNumJoints(pole2)):
+  #disable default constraint-based motors
+  p.setJointMotorControl2(pole, i, p.POSITION_CONTROL, targetPosition=0, force=0)
+  p.setJointMotorControl2(pole2, i, p.POSITION_CONTROL, targetPosition=0, force=0)
+  p.setJointMotorControl2(pole3, i, p.POSITION_CONTROL, targetPosition=0, force=0)
+  p.setJointMotorControl2(pole4, i, p.POSITION_CONTROL, targetPosition=0, force=0)
+
+  #print("joint",i,"=",p.getJointInfo(pole2,i))
+
+timeStepId = p.addUserDebugParameter("timeStep", 0.001, 0.1, 0.01)
+desiredPosCartId = p.addUserDebugParameter("desiredPosCart", -10, 10, 2)
+desiredVelCartId = p.addUserDebugParameter("desiredVelCart", -10, 10, 0)
+kpCartId = p.addUserDebugParameter("kpCart", 0, 500, 1300)
+kdCartId = p.addUserDebugParameter("kdCart", 0, 300, 150)
+maxForceCartId = p.addUserDebugParameter("maxForceCart", 0, 5000, 1000)
+
+textColor = [1, 1, 1]
+shift = 0.05
+p.addUserDebugText("explicit PD", [shift, 0, .1],
+                   textColor,
+                   parentObjectUniqueId=pole,
+                   parentLinkIndex=1)
+p.addUserDebugText("explicit PD plugin", [shift, 0, -.1],
+                   textColor,
+                   parentObjectUniqueId=pole2,
+                   parentLinkIndex=1)
+p.addUserDebugText("stablePD", [shift, 0, .1],
+                   textColor,
+                   parentObjectUniqueId=pole4,
+                   parentLinkIndex=1)
+p.addUserDebugText("position constraint", [shift, 0, -.1],
+                   textColor,
+                   parentObjectUniqueId=pole3,
+                   parentLinkIndex=1)
+
+desiredPosPoleId = p.addUserDebugParameter("desiredPosPole", -10, 10, 0)
+desiredVelPoleId = p.addUserDebugParameter("desiredVelPole", -10, 10, 0)
+kpPoleId = p.addUserDebugParameter("kpPole", 0, 500, 1200)
+kdPoleId = p.addUserDebugParameter("kdPole", 0, 300, 100)
+maxForcePoleId = p.addUserDebugParameter("maxForcePole", 0, 5000, 1000)
+
+pd = p.loadPlugin("pdControlPlugin")
+
+p.setGravity(0, 0, -10)
+
+useRealTimeSim = False
+
+p.setRealTimeSimulation(useRealTimeSim)
+
+timeStep = 0.001
+
+while p.isConnected():
+  #p.getCameraImage(320,200)
+  timeStep = p.readUserDebugParameter(timeStepId)
+  p.setTimeStep(timeStep)
+
+  desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
+  desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
+  kpCart = p.readUserDebugParameter(kpCartId)
+  kdCart = p.readUserDebugParameter(kdCartId)
+  maxForceCart = p.readUserDebugParameter(maxForceCartId)
+
+  desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
+  desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
+  kpPole = p.readUserDebugParameter(kpPoleId)
+  kdPole = p.readUserDebugParameter(kdPoleId)
+  maxForcePole = p.readUserDebugParameter(maxForcePoleId)
+
+  basePos, baseOrn = p.getBasePositionAndOrientation(pole)
+
+  baseDof = 7
+  taus = exPD.computePD(pole, [0, 1], [
+      basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2], baseOrn[3],
+      desiredPosCart, desiredPosPole
+  ], [0, 0, 0, 0, 0, 0, 0, desiredVelCart, desiredVelPole], [0, 0, 0, 0, 0, 0, 0, kpCart, kpPole],
+                        [0, 0, 0, 0, 0, 0, 0, kdCart, kdPole],
+                        [0, 0, 0, 0, 0, 0, 0, maxForceCart, maxForcePole], timeStep)
+
+  for j in [0, 1]:
+    p.setJointMotorControlMultiDof(pole,
+                                   j,
+                                   controlMode=p.TORQUE_CONTROL,
+                                   force=[taus[j + baseDof]])
+  #p.setJointMotorControlArray(pole, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
+
+  if (pd >= 0):
+    link = 0
+    p.setJointMotorControl2(bodyUniqueId=pole2,
+                            jointIndex=link,
+                            controlMode=p.PD_CONTROL,
+                            targetPosition=desiredPosCart,
+                            targetVelocity=desiredVelCart,
+                            force=maxForceCart,
+                            positionGain=kpCart,
+                            velocityGain=kdCart)
+    link = 1
+    p.setJointMotorControl2(bodyUniqueId=pole2,
+                            jointIndex=link,
+                            controlMode=p.PD_CONTROL,
+                            targetPosition=desiredPosPole,
+                            targetVelocity=desiredVelPole,
+                            force=maxForcePole,
+                            positionGain=kpPole,
+                            velocityGain=kdPole)
+
+  taus = sPD.computePD(pole4, [0, 1], [desiredPosCart, desiredPosPole],
+                       [desiredVelCart, desiredVelPole], [kpCart, kpPole], [kdCart, kdPole],
+                       [maxForceCart, maxForcePole], timeStep)
+  #p.setJointMotorControlArray(pole4, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
+  for j in [0, 1]:
+    p.setJointMotorControlMultiDof(pole4, j, controlMode=p.TORQUE_CONTROL, force=[taus[j]])
+
+  p.setJointMotorControl2(pole3,
+                          0,
+                          p.POSITION_CONTROL,
+                          targetPosition=desiredPosCart,
+                          targetVelocity=desiredVelCart,
+                          positionGain=timeStep * (kpCart / 150.),
+                          velocityGain=0.5,
+                          force=maxForceCart)
+  p.setJointMotorControl2(pole3,
+                          1,
+                          p.POSITION_CONTROL,
+                          targetPosition=desiredPosPole,
+                          targetVelocity=desiredVelPole,
+                          positionGain=timeStep * (kpPole / 150.),
+                          velocityGain=0.5,
+                          force=maxForcePole)
+
+  if (not useRealTimeSim):
+    p.stepSimulation()
+    time.sleep(timeStep)

Engine/lib/bullet/examples/pybullet/examples/pdControllerExplicit.py

@@ -0,0 +1,98 @@
+import numpy as np
+
+
+class PDControllerExplicitMultiDof(object):
+
+  def __init__(self, pb):
+    self._pb = pb
+
+  def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
+                maxForces, timeStep):
+
+    numJoints = len(jointIndices)  #self._pb.getNumJoints(bodyUniqueId)
+    curPos, curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
+    q1 = [curPos[0], curPos[1], curPos[2], curOrn[0], curOrn[1], curOrn[2], curOrn[3]]
+    baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
+    qdot1 = [
+        baseLinVel[0], baseLinVel[1], baseLinVel[2], baseAngVel[0], baseAngVel[1], baseAngVel[2], 0
+    ]
+    qError = [0, 0, 0, 0, 0, 0, 0]
+    qIndex = 7
+    qdotIndex = 7
+    zeroAccelerations = [0, 0, 0, 0, 0, 0, 0]
+    for i in range(numJoints):
+      js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
+
+      jointPos = js[0]
+      jointVel = js[1]
+      q1 += jointPos
+
+      if len(js[0]) == 1:
+        desiredPos = desiredPositions[qIndex]
+
+        qdiff = desiredPos - jointPos[0]
+        qError.append(qdiff)
+        zeroAccelerations.append(0.)
+        qdot1 += jointVel
+        qIndex += 1
+        qdotIndex += 1
+      if len(js[0]) == 4:
+        desiredPos = [
+            desiredPositions[qIndex], desiredPositions[qIndex + 1], desiredPositions[qIndex + 2],
+            desiredPositions[qIndex + 3]
+        ]
+        axis = self._pb.getAxisDifferenceQuaternion(desiredPos, jointPos)
+        jointVelNew = [jointVel[0], jointVel[1], jointVel[2], 0]
+        qdot1 += jointVelNew
+        qError.append(axis[0])
+        qError.append(axis[1])
+        qError.append(axis[2])
+        qError.append(0)
+        desiredVel = [
+            desiredVelocities[qdotIndex], desiredVelocities[qdotIndex + 1],
+            desiredVelocities[qdotIndex + 2]
+        ]
+        zeroAccelerations += [0., 0., 0., 0.]
+        qIndex += 4
+        qdotIndex += 4
+
+    q = np.array(q1)
+    qdot = np.array(qdot1)
+    qdotdesired = np.array(desiredVelocities)
+    qdoterr = qdotdesired - qdot
+    Kp = np.diagflat(kps)
+    Kd = np.diagflat(kds)
+    p = Kp.dot(qError)
+    d = Kd.dot(qdoterr)
+    forces = p + d
+    maxF = np.array(maxForces)
+    forces = np.clip(forces, -maxF, maxF)
+    return forces
+
+
+class PDControllerExplicit(object):
+
+  def __init__(self, pb):
+    self._pb = pb
+
+  def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
+                maxForces, timeStep):
+    numJoints = self._pb.getNumJoints(bodyUniqueId)
+    jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
+    q1 = []
+    qdot1 = []
+    for i in range(numJoints):
+      q1.append(jointStates[i][0])
+      qdot1.append(jointStates[i][1])
+    q = np.array(q1)
+    qdot = np.array(qdot1)
+    qdes = np.array(desiredPositions)
+    qdotdes = np.array(desiredVelocities)
+    qError = qdes - q
+    qdotError = qdotdes - qdot
+    Kp = np.diagflat(kps)
+    Kd = np.diagflat(kds)
+    forces = Kp.dot(qError) + Kd.dot(qdotError)
+    maxF = np.array(maxForces)
+    forces = np.clip(forces, -maxF, maxF)
+    return forces

Engine/lib/bullet/examples/pybullet/examples/pdControllerStable.py

@@ -0,0 +1,147 @@
+import numpy as np
+
+
+class PDControllerStableMultiDof(object):
+
+  def __init__(self, pb):
+    self._pb = pb
+
+  def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
+                maxForces, timeStep):
+
+    numJoints = len(jointIndices)  #self._pb.getNumJoints(bodyUniqueId)
+    curPos, curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
+    #q1 = [desiredPositions[0],desiredPositions[1],desiredPositions[2],desiredPositions[3],desiredPositions[4],desiredPositions[5],desiredPositions[6]]
+    q1 = [curPos[0], curPos[1], curPos[2], curOrn[0], curOrn[1], curOrn[2], curOrn[3]]
+
+    #qdot1 = [0,0,0, 0,0,0,0]
+    baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
+
+    qdot1 = [
+        baseLinVel[0], baseLinVel[1], baseLinVel[2], baseAngVel[0], baseAngVel[1], baseAngVel[2], 0
+    ]
+    qError = [0, 0, 0, 0, 0, 0, 0]
+
+    qIndex = 7
+    qdotIndex = 7
+    zeroAccelerations = [0, 0, 0, 0, 0, 0, 0]
+    for i in range(numJoints):
+      js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
+
+      jointPos = js[0]
+      jointVel = js[1]
+      q1 += jointPos
+
+      if len(js[0]) == 1:
+        desiredPos = desiredPositions[qIndex]
+
+        qdiff = desiredPos - jointPos[0]
+        qError.append(qdiff)
+        zeroAccelerations.append(0.)
+        qdot1 += jointVel
+        qIndex += 1
+        qdotIndex += 1
+      if len(js[0]) == 4:
+        desiredPos = [
+            desiredPositions[qIndex], desiredPositions[qIndex + 1], desiredPositions[qIndex + 2],
+            desiredPositions[qIndex + 3]
+        ]
+        axis = self._pb.getAxisDifferenceQuaternion(desiredPos, jointPos)
+        jointVelNew = [jointVel[0], jointVel[1], jointVel[2], 0]
+        qdot1 += jointVelNew
+        qError.append(axis[0])
+        qError.append(axis[1])
+        qError.append(axis[2])
+        qError.append(0)
+        desiredVel = [
+            desiredVelocities[qdotIndex], desiredVelocities[qdotIndex + 1],
+            desiredVelocities[qdotIndex + 2]
+        ]
+        zeroAccelerations += [0., 0., 0., 0.]
+        qIndex += 4
+        qdotIndex += 4
+
+    q = np.array(q1)
+    qdot = np.array(qdot1)
+
+    qdotdesired = np.array(desiredVelocities)
+    qdoterr = qdotdesired - qdot
+
+    Kp = np.diagflat(kps)
+    Kd = np.diagflat(kds)
+
+    # Compute -Kp(q + qdot - qdes)
+    p_term = Kp.dot(qError - qdot*timeStep)
+    # Compute -Kd(qdot - qdotdes)
+    d_term = Kd.dot(qdoterr)
+
+    # Compute Inertia matrix M(q)
+    M = self._pb.calculateMassMatrix(bodyUniqueId, q1, flags=1)
+    M = np.array(M)
+    # Given: M(q) * qddot + C(q, qdot) = T_ext + T_int
+    # Compute Coriolis and External (Gravitational) terms G = C - T_ext
+    G = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)
+    G = np.array(G)
+    # Obtain estimated generalized accelerations, considering Coriolis and Gravitational forces, and stable PD actions
+    qddot = np.linalg.solve(a=(M + Kd * timeStep),
+                            b=p_term + d_term - G)
+    # Compute control generalized forces (T_int)
+    tau = p_term + d_term - Kd.dot(qddot) * timeStep
+    # Clip generalized forces to actuator limits
+    maxF = np.array(maxForces)
+    generalized_forces = np.clip(tau, -maxF, maxF)
+    return generalized_forces
+
+
+class PDControllerStable(object):
+  """
+  Implementation based on: Tan, J., Liu, K., & Turk, G. (2011). "Stable proportional-derivative controllers"
+  DOI: 10.1109/MCG.2011.30
+  """
+  def __init__(self, pb):
+    self._pb = pb
+
+  def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
+                maxForces, timeStep):
+    numJoints = self._pb.getNumJoints(bodyUniqueId)
+    jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
+    q1 = []
+    qdot1 = []
+    zeroAccelerations = []
+    for i in range(numJoints):
+      q1.append(jointStates[i][0])
+      qdot1.append(jointStates[i][1])
+      zeroAccelerations.append(0)
+
+    q = np.array(q1)
+    qdot = np.array(qdot1)
+    qdes = np.array(desiredPositions)
+    qdotdes = np.array(desiredVelocities)
+
+    qError = qdes - q
+    qdotError = qdotdes - qdot
+
+    Kp = np.diagflat(kps)
+    Kd = np.diagflat(kds)
+
+    # Compute -Kp(q + qdot - qdes)
+    p_term = Kp.dot(qError - qdot*timeStep)
+    # Compute -Kd(qdot - qdotdes)
+    d_term = Kd.dot(qdotError)
+
+    # Compute Inertia matrix M(q)
+    M = self._pb.calculateMassMatrix(bodyUniqueId, q1)
+    M = np.array(M)
+    # Given: M(q) * qddot + C(q, qdot) = T_ext + T_int
+    # Compute Coriolis and External (Gravitational) terms G = C - T_ext
+    G = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
+    G = np.array(G)
+    # Obtain estimated generalized accelerations, considering Coriolis and Gravitational forces, and stable PD actions
+    qddot = np.linalg.solve(a=(M + Kd * timeStep),
+                            b=(-G + p_term + d_term))
+    # Compute control generalized forces (T_int)
+    tau = p_term + d_term - (Kd.dot(qddot) * timeStep)
+    # Clip generalized forces to actuator limits
+    maxF = np.array(maxForces)
+    generalized_forces = np.clip(tau, -maxF, maxF)
+    return generalized_forces

Engine/lib/bullet/examples/pybullet/examples/pointCloudFromCameraImage.py

@@ -0,0 +1,140 @@
+import pybullet as p
+import math
+import numpy as np
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+plane = p.loadURDF("plane100.urdf")
+cube = p.loadURDF("cube.urdf", [0, 0, 1])
+
+
+def getRayFromTo(mouseX, mouseY):
+  width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
+  )
+  camPos = [
+      camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
+      camTarget[2] - dist * camForward[2]
+  ]
+  farPlane = 10000
+  rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
+  lenFwd = math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] * rayForward[1] +
+                     rayForward[2] * rayForward[2])
+  invLen = farPlane * 1. / lenFwd
+  rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
+  rayFrom = camPos
+  oneOverWidth = float(1) / float(width)
+  oneOverHeight = float(1) / float(height)
+
+  dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
+  dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
+  rayToCenter = [
+      rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
+  ]
+  ortho = [
+      -0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] - float(mouseY) * dVer[0],
+      -0.5 * horizon[1] + 0.5 * vertical[1] + float(mouseX) * dHor[1] - float(mouseY) * dVer[1],
+      -0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
+  ]
+
+  rayTo = [
+      rayFrom[0] + rayForward[0] + ortho[0], rayFrom[1] + rayForward[1] + ortho[1],
+      rayFrom[2] + rayForward[2] + ortho[2]
+  ]
+  lenOrtho = math.sqrt(ortho[0] * ortho[0] + ortho[1] * ortho[1] + ortho[2] * ortho[2])
+  alpha = math.atan(lenOrtho / farPlane)
+  return rayFrom, rayTo, alpha
+
+
+width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
+)
+camPos = [
+    camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
+    camTarget[2] - dist * camForward[2]
+]
+farPlane = 10000
+rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
+lenFwd = math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] * rayForward[1] +
+                   rayForward[2] * rayForward[2])
+oneOverWidth = float(1) / float(width)
+oneOverHeight = float(1) / float(height)
+dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
+dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
+
+lendHor = math.sqrt(dHor[0] * dHor[0] + dHor[1] * dHor[1] + dHor[2] * dHor[2])
+lendVer = math.sqrt(dVer[0] * dVer[0] + dVer[1] * dVer[1] + dVer[2] * dVer[2])
+
+cornersX = [0, width, width, 0]
+cornersY = [0, 0, height, height]
+corners3D = []
+
+imgW = int(width / 10)
+imgH = int(height / 10)
+
+img = p.getCameraImage(imgW, imgH, renderer=p.ER_BULLET_HARDWARE_OPENGL)
+rgbBuffer = np.reshape(img[2], (imgH, imgW, 4))
+# NOTE: this depth buffer's reshaping does not match the [w, h] convention for
+# OpenGL depth buffers.  See getCameraImageTest.py for an OpenGL depth buffer
+depthBuffer = np.reshape(img[3], [imgH, imgW])
+print("rgbBuffer.shape=", rgbBuffer.shape)
+print("depthBuffer.shape=", depthBuffer.shape)
+
+#disable rendering temporary makes adding objects faster
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
+visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE, rgbaColor=[1, 1, 1, 1], radius=0.03)
+collisionShapeId = -1  #p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale)
+
+for i in range(4):
+  w = cornersX[i]
+  h = cornersY[i]
+  rayFrom, rayTo, _ = getRayFromTo(w, h)
+  rf = np.array(rayFrom)
+  rt = np.array(rayTo)
+  vec = rt - rf
+  l = np.sqrt(np.dot(vec, vec))
+  newTo = (0.01 / l) * vec + rf
+  #print("len vec=",np.sqrt(np.dot(vec,vec)))
+
+  p.addUserDebugLine(rayFrom, newTo, [1, 0, 0])
+  corners3D.append(newTo)
+count = 0
+
+stepX = 5
+stepY = 5
+for w in range(0, imgW, stepX):
+  for h in range(0, imgH, stepY):
+    count += 1
+    if ((count % 100) == 0):
+      print(count, "out of ", imgW * imgH / (stepX * stepY))
+    rayFrom, rayTo, alpha = getRayFromTo(w * (width / imgW), h * (height / imgH))
+    rf = np.array(rayFrom)
+    rt = np.array(rayTo)
+    vec = rt - rf
+    l = np.sqrt(np.dot(vec, vec))
+    depthImg = float(depthBuffer[h, w])
+    far = 1000.
+    near = 0.01
+    depth = far * near / (far - (far - near) * depthImg)
+    depth /= math.cos(alpha)
+    newTo = (depth / l) * vec + rf
+    p.addUserDebugLine(rayFrom, newTo, [1, 0, 0])
+    mb = p.createMultiBody(baseMass=0,
+                           baseCollisionShapeIndex=collisionShapeId,
+                           baseVisualShapeIndex=visualShapeId,
+                           basePosition=newTo,
+                           useMaximalCoordinates=True)
+    color = rgbBuffer[h, w]
+    color = [color[0] / 255., color[1] / 255., color[2] / 255., 1]
+    p.changeVisualShape(mb, -1, rgbaColor=color)
+p.addUserDebugLine(corners3D[0], corners3D[1], [1, 0, 0])
+p.addUserDebugLine(corners3D[1], corners3D[2], [1, 0, 0])
+p.addUserDebugLine(corners3D[2], corners3D[3], [1, 0, 0])
+p.addUserDebugLine(corners3D[3], corners3D[0], [1, 0, 0])
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+print("ready\n")
+#p.removeBody(plane)
+#p.removeBody(cube)
+while (1):
+  p.setGravity(0, 0, -10)

Engine/lib/bullet/examples/pybullet/examples/profileTiming.py

@@ -0,0 +1,27 @@
+import pybullet as p
+import time
+#you can visualize the timings using Google Chrome, visit about://tracing 
+#and load the json file
+import pybullet_data
+
+p.connect(p.GUI)
+#p.configureDebugVisualizer(p.ENABLE_RENDERING,0)
+p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+t = time.time() + 3.1
+
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "single_step_no_stepsim_chrome_about_tracing.json")
+while (time.time() < t):
+  #p.stepSimulation()
+  p.submitProfileTiming("pythontest")
+  time.sleep(1./240.)
+  p.submitProfileTiming("nested")
+  for i in range (100):
+    p.submitProfileTiming("deep_nested")
+    p.submitProfileTiming()
+  time.sleep(1./1000.)
+  p.submitProfileTiming()
+  p.submitProfileTiming()
+
+p.stopStateLogging(logId)

Engine/lib/bullet/examples/pybullet/examples/projective_texture.py

@@ -0,0 +1,41 @@
+import pybullet as p
+from time import sleep
+import matplotlib.pyplot as plt
+import numpy as np
+import pybullet_data
+
+physicsClient = p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setGravity(0, 0, 0)
+bearStartPos1 = [-3.3, 0, 0]
+bearStartOrientation1 = p.getQuaternionFromEuler([0, 0, 0])
+bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
+bearStartPos2 = [0, 0, 0]
+bearStartOrientation2 = p.getQuaternionFromEuler([0, 0, 0])
+bearId2 = p.loadURDF("teddy_large.urdf", bearStartPos2, bearStartOrientation2)
+textureId = p.loadTexture("checker_grid.jpg")
+#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
+#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
+
+useRealTimeSimulation = 1
+
+if (useRealTimeSimulation):
+  p.setRealTimeSimulation(1)
+
+while 1:
+  if (useRealTimeSimulation):
+    camera = p.getDebugVisualizerCamera()
+    viewMat = camera[2]
+    projMat = camera[3]
+    #An example of setting the view matrix for the projective texture.
+    #viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
+    p.getCameraImage(300,
+                     300,
+                     renderer=p.ER_BULLET_HARDWARE_OPENGL,
+                     flags=p.ER_USE_PROJECTIVE_TEXTURE,
+                     projectiveTextureView=viewMat,
+                     projectiveTextureProj=projMat)
+    p.setGravity(0, 0, 0)
+  else:
+    p.stepSimulation()

Engine/lib/bullet/examples/pybullet/examples/quadruped.py

@@ -0,0 +1,469 @@
+import pybullet as p
+import time
+import math
+import pybullet_data
+
+
+
+
+def drawInertiaBox(parentUid, parentLinkIndex, color):
+  dyn = p.getDynamicsInfo(parentUid, parentLinkIndex)
+  mass = dyn[0]
+  frictionCoeff = dyn[1]
+  inertia = dyn[2]
+  if (mass > 0):
+    Ixx = inertia[0]
+    Iyy = inertia[1]
+    Izz = inertia[2]
+    boxScaleX = 0.5 * math.sqrt(6 * (Izz + Iyy - Ixx) / mass)
+    boxScaleY = 0.5 * math.sqrt(6 * (Izz + Ixx - Iyy) / mass)
+    boxScaleZ = 0.5 * math.sqrt(6 * (Ixx + Iyy - Izz) / mass)
+
+    halfExtents = [boxScaleX, boxScaleY, boxScaleZ]
+    pts = [[halfExtents[0], halfExtents[1], halfExtents[2]],
+           [-halfExtents[0], halfExtents[1], halfExtents[2]],
+           [halfExtents[0], -halfExtents[1], halfExtents[2]],
+           [-halfExtents[0], -halfExtents[1], halfExtents[2]],
+           [halfExtents[0], halfExtents[1], -halfExtents[2]],
+           [-halfExtents[0], halfExtents[1], -halfExtents[2]],
+           [halfExtents[0], -halfExtents[1], -halfExtents[2]],
+           [-halfExtents[0], -halfExtents[1], -halfExtents[2]]]
+
+    p.addUserDebugLine(pts[0],
+                       pts[1],
+                       color,
+                       1,
+                       parentObjectUniqueId=parentUid,
+                       parentLinkIndex=parentLinkIndex)
+    p.addUserDebugLine(pts[1],
+                       pts[3],
+                       color,
+                       1,
+                       parentObjectUniqueId=parentUid,
+                       parentLinkIndex=parentLinkIndex)
+    p.addUserDebugLine(pts[3],
+                       pts[2],
+                       color,
+                       1,
+                       parentObjectUniqueId=parentUid,
+                       parentLinkIndex=parentLinkIndex)
+    p.addUserDebugLine(pts[2],
+                       pts[0],
+                       color,
+                       1,
+                       parentObjectUniqueId=parentUid,
+                       parentLinkIndex=parentLinkIndex)
+
+    p.addUserDebugLine(pts[0],
+                       pts[4],
+                       color,
+                       1,
+                       parentObjectUniqueId=parentUid,
+                       parentLinkIndex=parentLinkIndex)
+    p.addUserDebugLine(pts[1],
+                       pts[5],
+                       color,
+                       1,
+                       parentObjectUniqueId=parentUid,
+                       parentLinkIndex=parentLinkIndex)
+    p.addUserDebugLine(pts[2],
+                       pts[6],
+                       color,
+                       1,
+                       parentObjectUniqueId=parentUid,
+                       parentLinkIndex=parentLinkIndex)
+    p.addUserDebugLine(pts[3],
+                       pts[7],
+                       color,
+                       1,
+                       parentObjectUniqueId=parentUid,
+                       parentLinkIndex=parentLinkIndex)
+
+    p.addUserDebugLine(pts[4 + 0],
+                       pts[4 + 1],
+                       color,
+                       1,
+                       parentObjectUniqueId=parentUid,
+                       parentLinkIndex=parentLinkIndex)
+    p.addUserDebugLine(pts[4 + 1],
+                       pts[4 + 3],
+                       color,
+                       1,
+                       parentObjectUniqueId=parentUid,
+                       parentLinkIndex=parentLinkIndex)
+    p.addUserDebugLine(pts[4 + 3],
+                       pts[4 + 2],
+                       color,
+                       1,
+                       parentObjectUniqueId=parentUid,
+                       parentLinkIndex=parentLinkIndex)
+    p.addUserDebugLine(pts[4 + 2],
+                       pts[4 + 0],
+                       color,
+                       1,
+                       parentObjectUniqueId=parentUid,
+                       parentLinkIndex=parentLinkIndex)
+
+
+toeConstraint = True
+useMaximalCoordinates = False
+useRealTime = 0
+
+#the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance
+fixedTimeStep = 1. / 100
+numSolverIterations = 50
+
+if (useMaximalCoordinates):
+  fixedTimeStep = 1. / 500
+  numSolverIterations = 200
+
+speed = 10
+amplitude = 0.8
+jump_amp = 0.5
+maxForce = 3.5
+kneeFrictionForce = 0
+kp = 1
+kd = .5
+maxKneeForce = 1000
+
+physId = p.connect(p.SHARED_MEMORY_GUI)
+if (physId < 0):
+  p.connect(p.GUI)
+#p.resetSimulation()
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+angle = 0  # pick in range 0..0.2 radians
+orn = p.getQuaternionFromEuler([0, angle, 0])
+p.loadURDF("plane.urdf", [0, 0, 0], orn)
+p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
+p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
+                    "genericlogdata.bin",
+                    maxLogDof=16,
+                    logFlags=p.STATE_LOG_JOINT_TORQUES)
+p.setTimeOut(4000000)
+
+p.setGravity(0, 0, 0)
+p.setTimeStep(fixedTimeStep)
+
+orn = p.getQuaternionFromEuler([0, 0, 0.4])
+p.setRealTimeSimulation(0)
+quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3],
+                       orn,
+                       useFixedBase=False,
+                       useMaximalCoordinates=useMaximalCoordinates,
+                       flags=p.URDF_USE_IMPLICIT_CYLINDER)
+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']
+motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
+knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
+hip_front_rightR_link = jointNameToId['hip_front_rightR_link']
+knee_front_rightR_link = jointNameToId['knee_front_rightR_link']
+motor_front_rightL_link = jointNameToId['motor_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']
+
+#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
+
+motordir = [-1, -1, -1, -1, 1, 1, 1, 1]
+halfpi = 1.57079632679
+twopi = 4 * halfpi
+kneeangle = -2.1834
+
+dyn = p.getDynamicsInfo(quadruped, -1)
+mass = dyn[0]
+friction = dyn[1]
+localInertiaDiagonal = dyn[2]
+
+print("localInertiaDiagonal", localInertiaDiagonal)
+
+#this is a no-op, just to show the API
+p.changeDynamics(quadruped, -1, localInertiaDiagonal=localInertiaDiagonal)
+
+#for i in range (nJoints):
+#	p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
+
+drawInertiaBox(quadruped, -1, [1, 0, 0])
+#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
+
+for i in range(nJoints):
+  drawInertiaBox(quadruped, i, [0, 1, 0])
+
+if (useMaximalCoordinates):
+  steps = 400
+  for aa in range(steps):
+    p.setJointMotorControl2(quadruped, motor_front_leftL_joint, p.POSITION_CONTROL,
+                            motordir[0] * halfpi * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, motor_front_leftR_joint, p.POSITION_CONTROL,
+                            motordir[1] * halfpi * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, motor_back_leftL_joint, p.POSITION_CONTROL,
+                            motordir[2] * halfpi * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, motor_back_leftR_joint, p.POSITION_CONTROL,
+                            motordir[3] * halfpi * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, motor_front_rightL_joint, p.POSITION_CONTROL,
+                            motordir[4] * halfpi * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, motor_front_rightR_joint, p.POSITION_CONTROL,
+                            motordir[5] * halfpi * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, motor_back_rightL_joint, p.POSITION_CONTROL,
+                            motordir[6] * halfpi * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, motor_back_rightR_joint, p.POSITION_CONTROL,
+                            motordir[7] * halfpi * float(aa) / steps)
+
+    p.setJointMotorControl2(quadruped, knee_front_leftL_link, p.POSITION_CONTROL,
+                            motordir[0] * (kneeangle + twopi) * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, knee_front_leftR_link, p.POSITION_CONTROL,
+                            motordir[1] * kneeangle * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, knee_back_leftL_link, p.POSITION_CONTROL,
+                            motordir[2] * kneeangle * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, knee_back_leftR_link, p.POSITION_CONTROL,
+                            motordir[3] * (kneeangle + twopi) * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, knee_front_rightL_link, p.POSITION_CONTROL,
+                            motordir[4] * (kneeangle) * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, knee_front_rightR_link, p.POSITION_CONTROL,
+                            motordir[5] * (kneeangle + twopi) * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, knee_back_rightL_link, p.POSITION_CONTROL,
+                            motordir[6] * (kneeangle + twopi) * float(aa) / steps)
+    p.setJointMotorControl2(quadruped, knee_back_rightR_link, p.POSITION_CONTROL,
+                            motordir[7] * kneeangle * float(aa) / steps)
+
+    p.stepSimulation()
+    #time.sleep(fixedTimeStep)
+else:
+
+  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)
+
+  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)
+
+  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)
+
+  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)
+
+#p.getNumJoints(1)
+
+if (toeConstraint):
+  cid = p.createConstraint(quadruped, knee_front_leftR_link, quadruped, knee_front_leftL_link,
+                           p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
+  p.changeConstraint(cid, maxForce=maxKneeForce)
+  cid = p.createConstraint(quadruped, knee_front_rightR_link, quadruped, knee_front_rightL_link,
+                           p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
+  p.changeConstraint(cid, maxForce=maxKneeForce)
+  cid = p.createConstraint(quadruped, knee_back_leftR_link, quadruped, knee_back_leftL_link,
+                           p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
+  p.changeConstraint(cid, maxForce=maxKneeForce)
+  cid = p.createConstraint(quadruped, knee_back_rightR_link, quadruped, knee_back_rightL_link,
+                           p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
+  p.changeConstraint(cid, maxForce=maxKneeForce)
+
+if (1):
+  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.setJointMotorControl(quadruped, knee_front_rightL_link, p.VELOCITY_CONTROL, 0,
+                         kneeFrictionForce)
+  p.setJointMotorControl(quadruped, knee_front_rightR_link, p.VELOCITY_CONTROL, 0,
+                         kneeFrictionForce)
+  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.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
+  p.setJointMotorControl(quadruped, knee_back_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
+  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)
+
+t = 0.0
+t_end = t + 15
+ref_time = time.time()
+while (t < t_end):
+  p.setGravity(0, 0, -10)
+  if (useRealTime):
+    t = time.time() - ref_time
+  else:
+    t = t + fixedTimeStep
+  if (useRealTime == 0):
+    p.stepSimulation()
+    time.sleep(fixedTimeStep)
+
+print("quadruped Id = ")
+print(quadruped)
+p.saveWorld("quadru.py")
+logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "quadrupedLog.bin", [quadruped])
+
+#jump
+t = 0.0
+t_end = t + 100
+i = 0
+ref_time = time.time()
+
+while (1):
+  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)

Engine/lib/bullet/examples/pybullet/examples/quadruped_playback.py

@@ -0,0 +1,178 @@
+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
+import pybullet_data
+
+
+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)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+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)

Engine/lib/bullet/examples/pybullet/examples/quadruped_setup_playback.py

@@ -0,0 +1,47 @@
+import pybullet as p
+import pybullet_data
+
+p.connect(p.SHARED_MEMORY)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+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()

Engine/lib/bullet/examples/pybullet/examples/racecar.py

@@ -0,0 +1,62 @@
+import os, inspect
+currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
+print("current_dir=" + currentdir)
+parentdir = os.path.join(currentdir, "../gym")
+
+os.sys.path.insert(0, parentdir)
+
+import pybullet as p
+import pybullet_data
+
+import time
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+  p.connect(p.GUI)
+
+p.resetSimulation()
+p.setGravity(0, 0, -10)
+
+useRealTimeSim = 1
+
+#for video recording (works best on Mac and Linux, not well on Windows)
+#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
+p.setRealTimeSimulation(useRealTimeSim)  # either this
+#p.loadURDF("plane.urdf")
+p.loadSDF(os.path.join(pybullet_data.getDataPath(), "stadium.sdf"))
+
+car = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "racecar/racecar.urdf"))
+for i in range(p.getNumJoints(car)):
+  print(p.getJointInfo(car, i))
+
+inactive_wheels = [3, 5, 7]
+wheels = [2]
+
+for wheel in inactive_wheels:
+  p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
+
+steering = [4, 6]
+
+targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -10, 10, 0)
+maxForceSlider = p.addUserDebugParameter("maxForce", 0, 10, 10)
+steeringSlider = p.addUserDebugParameter("steering", -0.5, 0.5, 0)
+while (True):
+  maxForce = p.readUserDebugParameter(maxForceSlider)
+  targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
+  steeringAngle = p.readUserDebugParameter(steeringSlider)
+  #print(targetVelocity)
+
+  for wheel in wheels:
+    p.setJointMotorControl2(car,
+                            wheel,
+                            p.VELOCITY_CONTROL,
+                            targetVelocity=targetVelocity,
+                            force=maxForce)
+
+  for steer in steering:
+    p.setJointMotorControl2(car, steer, p.POSITION_CONTROL, targetPosition=steeringAngle)
+
+  steering
+  if (useRealTimeSim == 0):
+    p.stepSimulation()
+  time.sleep(0.01)

Engine/lib/bullet/examples/pybullet/examples/racecar_differential.py

@@ -0,0 +1,135 @@
+import pybullet as p
+import time
+import pybullet_data
+
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+  p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.resetSimulation()
+p.setGravity(0, 0, -10)
+useRealTimeSim = 1
+
+#for video recording (works best on Mac and Linux, not well on Windows)
+#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
+p.setRealTimeSimulation(useRealTimeSim)  # either this
+p.loadURDF("plane.urdf")
+#p.loadSDF("stadium.sdf")
+
+car = p.loadURDF("racecar/racecar_differential.urdf")  #, [0,0,2],useFixedBase=True)
+for i in range(p.getNumJoints(car)):
+  print(p.getJointInfo(car, i))
+for wheel in range(p.getNumJoints(car)):
+  p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
+  p.getJointInfo(car, wheel)
+
+wheels = [8, 15]
+print("----------------")
+
+#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
+c = p.createConstraint(car,
+                       9,
+                       car,
+                       11,
+                       jointType=p.JOINT_GEAR,
+                       jointAxis=[0, 1, 0],
+                       parentFramePosition=[0, 0, 0],
+                       childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=1, maxForce=10000)
+
+c = p.createConstraint(car,
+                       10,
+                       car,
+                       13,
+                       jointType=p.JOINT_GEAR,
+                       jointAxis=[0, 1, 0],
+                       parentFramePosition=[0, 0, 0],
+                       childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(car,
+                       9,
+                       car,
+                       13,
+                       jointType=p.JOINT_GEAR,
+                       jointAxis=[0, 1, 0],
+                       parentFramePosition=[0, 0, 0],
+                       childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(car,
+                       16,
+                       car,
+                       18,
+                       jointType=p.JOINT_GEAR,
+                       jointAxis=[0, 1, 0],
+                       parentFramePosition=[0, 0, 0],
+                       childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=1, maxForce=10000)
+
+c = p.createConstraint(car,
+                       16,
+                       car,
+                       19,
+                       jointType=p.JOINT_GEAR,
+                       jointAxis=[0, 1, 0],
+                       parentFramePosition=[0, 0, 0],
+                       childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(car,
+                       17,
+                       car,
+                       19,
+                       jointType=p.JOINT_GEAR,
+                       jointAxis=[0, 1, 0],
+                       parentFramePosition=[0, 0, 0],
+                       childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(car,
+                       1,
+                       car,
+                       18,
+                       jointType=p.JOINT_GEAR,
+                       jointAxis=[0, 1, 0],
+                       parentFramePosition=[0, 0, 0],
+                       childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
+c = p.createConstraint(car,
+                       3,
+                       car,
+                       19,
+                       jointType=p.JOINT_GEAR,
+                       jointAxis=[0, 1, 0],
+                       parentFramePosition=[0, 0, 0],
+                       childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
+
+steering = [0, 2]
+
+targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -50, 50, 0)
+maxForceSlider = p.addUserDebugParameter("maxForce", 0, 50, 20)
+steeringSlider = p.addUserDebugParameter("steering", -1, 1, 0)
+while (True):
+  maxForce = p.readUserDebugParameter(maxForceSlider)
+  targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
+  steeringAngle = p.readUserDebugParameter(steeringSlider)
+  #print(targetVelocity)
+
+  for wheel in wheels:
+    p.setJointMotorControl2(car,
+                            wheel,
+                            p.VELOCITY_CONTROL,
+                            targetVelocity=targetVelocity,
+                            force=maxForce)
+
+  for steer in steering:
+    p.setJointMotorControl2(car, steer, p.POSITION_CONTROL, targetPosition=-steeringAngle)
+
+  steering
+  if (useRealTimeSim == 0):
+    p.stepSimulation()
+  time.sleep(0.01)

Engine/lib/bullet/examples/pybullet/examples/reduced_deformable_cube.py

@@ -0,0 +1,27 @@
+import pybullet as p
+from time import sleep
+import pybullet_data
+
+physicsClient = p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.resetSimulation(p.RESET_USE_REDUCED_DEFORMABLE_WORLD)
+p.resetDebugVisualizerCamera(4,-40,-30,[0, 0, 0])
+p.setGravity(0, 0, -10)
+
+tex = p.loadTexture("uvmap.png")
+planeId = p.loadURDF("plane.urdf", [0,0,-2])
+
+boxId = p.loadURDF("cube.urdf", [1,1,5],useMaximalCoordinates = True)
+
+#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "reduced_cube.mp4")
+cube = p.loadURDF("reduced_cube/reduced_cube.urdf", [1,1,1])
+p.changeVisualShape(cube, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
+p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
+p.setRealTimeSimulation(0)
+
+while p.isConnected():
+  p.stepSimulation()
+  p.getCameraImage(320,200)
+  p.setGravity(0,0,-10)

Engine/lib/bullet/examples/pybullet/examples/reduced_deformable_torus.py

@@ -0,0 +1,32 @@
+import pybullet as p
+from time import sleep
+import pybullet_data
+
+physicsClient = p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.resetSimulation(p.RESET_USE_REDUCED_DEFORMABLE_WORLD)
+p.resetDebugVisualizerCamera(4,-40,-30,[0, 0, 0])
+p.setGravity(0, 0, -10)
+
+tex = p.loadTexture("uvmap.png")
+planeId = p.loadURDF("plane.urdf", [0,0,-2])
+
+box1 = p.loadURDF("cube.urdf", [1,1,3],useMaximalCoordinates = True)
+box2 = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
+
+# p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "reduced_torus.mp4")
+reduced_obj1= p.loadURDF("reduced_torus/reduced_torus.urdf", [1,1,1])
+p.changeVisualShape(reduced_obj1, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
+
+reduced_obj2 = p.loadURDF("reduced_torus/reduced_torus.urdf", [1,2,1])
+p.changeVisualShape(reduced_obj2, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
+
+p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
+p.setRealTimeSimulation(0)
+
+while p.isConnected():
+  p.stepSimulation()
+  p.getCameraImage(320,200)
+  p.setGravity(0,0,-10)

Engine/lib/bullet/examples/pybullet/examples/renderPlugin.py

@@ -0,0 +1,12 @@
+import pybullet as p
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+plugin = p.loadPlugin("d:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll",
+                      "_testPlugin")
+print("plugin=", plugin)
+p.loadURDF("r2d2.urdf")
+
+while (1):
+  p.getCameraImage(320, 200)

Engine/lib/bullet/examples/pybullet/examples/rendertest.py

@@ -0,0 +1,153 @@
+#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled
+#otherwise use testrender.py (slower but compatible without numpy)
+#you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU)
+
+import os
+import sys
+import time
+import itertools
+import subprocess
+import numpy as np
+import pybullet
+from multiprocessing import Process
+import pybullet_data
+
+
+camTargetPos = [0, 0, 0]
+cameraUp = [0, 0, 1]
+cameraPos = [1, 1, 1]
+
+pitch = -10.0
+roll = 0
+upAxisIndex = 2
+camDistance = 4
+pixelWidth = 84  # 320
+pixelHeight = 84  # 200
+nearPlane = 0.01
+farPlane = 100
+fov = 60
+
+import matplotlib.pyplot as plt
+
+
+class BulletSim():
+
+  def __init__(self, connection_mode, *argv):
+    self.connection_mode = connection_mode
+    self.argv = argv
+
+  def __enter__(self):
+    print("connecting")
+    optionstring = '--width={} --height={}'.format(pixelWidth, pixelHeight)
+    optionstring += ' --window_backend=2 --render_device=0'
+
+    print(self.connection_mode, optionstring, *self.argv)
+    cid = pybullet.connect(self.connection_mode, options=optionstring, *self.argv)
+    pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
+    if cid < 0:
+      raise ValueError
+    print("connected")
+    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
+    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
+    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
+    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
+
+    pybullet.resetSimulation()
+    pybullet.loadURDF("plane.urdf", [0, 0, -1])
+    pybullet.loadURDF("r2d2.urdf")
+    pybullet.loadURDF("duck_vhacd.urdf")
+    pybullet.setGravity(0, 0, -10)
+
+  def __exit__(self, *_, **__):
+    pybullet.disconnect()
+
+
+def test(num_runs=300, shadow=1, log=True, plot=False):
+  if log:
+    logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")
+
+  if plot:
+    plt.ion()
+
+    img = np.random.rand(200, 320)
+    #img = [tandard_normal((50,100))
+    image = plt.imshow(img, interpolation='none', animated=True, label="blah")
+    ax = plt.gca()
+
+  times = np.zeros(num_runs)
+  yaw_gen = itertools.cycle(range(0, 360, 10))
+  for i, yaw in zip(range(num_runs), yaw_gen):
+    pybullet.stepSimulation()
+    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,
+                                      shadow=shadow,
+                                      lightDirection=[1, 1, 1],
+                                      renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
+    #renderer=pybullet.ER_TINY_RENDERER)
+    stop = time.time()
+    duration = (stop - start)
+    if (duration):
+      fps = 1. / duration
+      #print("fps=",fps)
+    else:
+      fps = 0
+      #print("fps=",fps)
+    #print("duraction=",duration)
+    #print("fps=",fps)
+    times[i] = fps
+
+    if plot:
+      rgb = img_arr[2]
+      image.set_data(rgb)  #np_img_arr)
+      ax.plot([0])
+      #plt.draw()
+      #plt.show()
+      plt.pause(0.01)
+
+  mean_time = float(np.mean(times))
+  print("mean: {0} for {1} runs".format(mean_time, num_runs))
+  print("")
+  if log:
+    pybullet.stopStateLogging(logId)
+  return mean_time
+
+
+if __name__ == "__main__":
+
+  res = []
+
+  with BulletSim(pybullet.DIRECT):
+    print("\nTesting DIRECT")
+    mean_time = test(log=False, plot=True)
+    res.append(("tiny", mean_time))
+
+  with BulletSim(pybullet.DIRECT):
+    plugin_fn = os.path.join(
+        pybullet.__file__.split("bullet3")[0],
+        "bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
+    plugin = pybullet.loadPlugin(plugin_fn, "_tinyRendererPlugin")
+    if plugin < 0:
+      print("\nPlugin Failed to load!\n")
+      sys.exit()
+
+    print("\nTesting DIRECT+OpenGL")
+    mean_time = test(log=True)
+    res.append(("plugin", mean_time))
+
+  with BulletSim(pybullet.GUI):
+    print("\nTesting GUI")
+    mean_time = test(log=False)
+    res.append(("egl", mean_time))
+
+  print()
+  print("rendertest.py")
+  print("back nenv fps fps_tot")
+  for r in res:
+    print(r[0], "\t", 1, round(r[1]), "\t", round(r[1]))

Engine/lib/bullet/examples/pybullet/examples/rendertest_sync.py

@@ -0,0 +1,155 @@
+#!/usr/bin/env python
+import os, logging, gym
+from baselines import logger
+from baselines.common import set_global_seeds
+from baselines.common.misc_util import boolean_flag
+from baselines import bench
+from baselines.a2c.a2c import learn
+from baselines.common.vec_env.subproc_vec_env import SubprocVecEnv
+from baselines.common.vec_env.vec_frame_stack import VecFrameStack
+import time
+
+import gym
+from gym import spaces
+import pybullet as p
+from itertools import cycle
+import numpy as np
+
+camTargetPos = [0, 0, 0]
+cameraUp = [0, 0, 1]
+cameraPos = [1, 1, 1]
+pitch = -10.0
+roll = 0
+upAxisIndex = 2
+camDistance = 4
+pixelWidth = 320
+pixelHeight = 200
+nearPlane = 0.01
+farPlane = 100
+fov = 60
+
+
+class TestEnv(gym.Env):
+
+  def __init__(
+      self,
+      renderer='tiny',  # ('tiny', 'egl', 'debug')
+  ):
+    self.action_space = spaces.Discrete(2)
+    self.iter = cycle(range(0, 360, 10))
+
+    # how we want to show
+    assert renderer in ('tiny', 'egl', 'debug', 'plugin')
+    self._renderer = renderer
+    self._render_width = 84
+    self._render_height = 84
+    # connecting
+    if self._renderer == "tiny" or self._renderer == "plugin":
+      optionstring = '--width={} --height={}'.format(self._render_width, self._render_height)
+      p.connect(p.DIRECT, options=optionstring)
+
+      if self._renderer == "plugin":
+        plugin_fn = os.path.join(
+            p.__file__.split("bullet3")[0],
+            "bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
+        plugin = p.loadPlugin(plugin_fn, "_tinyRendererPlugin")
+        if plugin < 0:
+          print("\nPlugin Failed to load! Try installing via `pip install -e .`\n")
+          sys.exit()
+        print("plugin =", plugin)
+
+    elif self._renderer == "egl":
+      optionstring = '--width={} --height={}'.format(self._render_width, self._render_height)
+      optionstring += ' --window_backend=2 --render_device=0'
+      p.connect(p.GUI, options=optionstring)
+
+    elif self._renderer == "debug":
+      #print("Connection: SHARED_MEMORY")
+      #cid = p.connect(p.SHARED_MEMORY)
+      #if (cid<0):
+      cid = p.connect(p.GUI)
+      p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
+
+    p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+    p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
+    p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
+    p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
+
+  def __del__(self):
+    p.disconnect()
+
+  def reset(self):
+    pass
+
+  def step(self, action):
+    p.stepSimulation()
+    start = time.time()
+    yaw = next(self.iter)
+    viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll,
+                                                     upAxisIndex)
+    aspect = pixelWidth / pixelHeight
+    projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
+    img_arr = p.getCameraImage(pixelWidth,
+                               pixelHeight,
+                               viewMatrix,
+                               projectionMatrix,
+                               shadow=1,
+                               lightDirection=[1, 1, 1],
+                               renderer=p.ER_BULLET_HARDWARE_OPENGL)
+    #renderer=pybullet.ER_TINY_RENDERER)
+    self._observation = img_arr[2]
+    return np.array(self._observation), 0, 0, {}
+
+  def seed(self, seed=None):
+    pass
+
+
+def train(env_id, num_timesteps=300, seed=0, num_env=2, renderer='tiny'):
+
+  def make_env(rank):
+
+    def _thunk():
+      if env_id == "TestEnv":
+        env = TestEnv(renderer=renderer)  #gym.make(env_id)
+      else:
+        env = gym.make(env_id)
+      env.seed(seed + rank)
+      env = bench.Monitor(env, logger.get_dir() and os.path.join(logger.get_dir(), str(rank)))
+      gym.logger.setLevel(logging.WARN)
+      # only clip rewards when not evaluating
+      return env
+
+    return _thunk
+
+  set_global_seeds(seed)
+  env = SubprocVecEnv([make_env(i) for i in range(num_env)])
+
+  env.reset()
+  start = time.time()
+  for i in range(num_timesteps):
+    action = [env.action_space.sample() for _ in range(num_env)]
+    env.step(action)
+  stop = time.time()
+  duration = (stop - start)
+  if (duration):
+    fps = num_timesteps / duration
+  else:
+    fps = 0
+  env.close()
+  return num_env, fps
+
+
+if __name__ == "__main__":
+  env_id = "TestEnv"
+  res = []
+
+  for renderer in ('tiny', 'plugin', 'egl'):
+    for i in (1, 8):
+      tmp = train(env_id, num_env=i, renderer=renderer)
+      print(renderer, tmp)
+      res.append((renderer, tmp))
+  print()
+  print("rendertest_sync.py")
+  print("back nenv fps fps_tot")
+  for renderer, i in res:
+    print(renderer, '\t', i[0], round(i[1]), '\t', round(i[0] * i[1]))

Engine/lib/bullet/examples/pybullet/examples/reset_dynamic_info.py

@@ -0,0 +1,129 @@
+import pybullet as p
+import time
+import math
+
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+planeId = p.loadURDF(fileName="plane.urdf", baseOrientation=[0.25882, 0, 0, 0.96593])
+p.loadURDF(fileName="cube.urdf", basePosition=[0, 0, 2])
+cubeId = p.loadURDF(fileName="cube.urdf", baseOrientation=[0, 0, 0, 1], basePosition=[0, 0, 4])
+#p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=0.1)
+p.changeDynamics(bodyUniqueId=2, linkIndex=-1, mass=1.0)
+p.setGravity(0, 0, -10)
+p.setRealTimeSimulation(0)
+
+
+def drawInertiaBox(parentUid, parentLinkIndex):
+  mass, frictionCoeff, inertia = p.getDynamicsInfo(bodyUniqueId=parentUid,
+                                                   linkIndex=parentLinkIndex,
+                                                   flags=p.DYNAMICS_INFO_REPORT_INERTIA)
+  Ixx = inertia[0]
+  Iyy = inertia[1]
+  Izz = inertia[2]
+  boxScaleX = 0.5 * math.sqrt(6 * (Izz + Iyy - Ixx) / mass)
+  boxScaleY = 0.5 * math.sqrt(6 * (Izz + Ixx - Iyy) / mass)
+  boxScaleZ = 0.5 * math.sqrt(6 * (Ixx + Iyy - Izz) / mass)
+
+  halfExtents = [boxScaleX, boxScaleY, boxScaleZ]
+  pts = [[halfExtents[0], halfExtents[1], halfExtents[2]],
+         [-halfExtents[0], halfExtents[1], halfExtents[2]],
+         [halfExtents[0], -halfExtents[1], halfExtents[2]],
+         [-halfExtents[0], -halfExtents[1], halfExtents[2]],
+         [halfExtents[0], halfExtents[1], -halfExtents[2]],
+         [-halfExtents[0], halfExtents[1], -halfExtents[2]],
+         [halfExtents[0], -halfExtents[1], -halfExtents[2]],
+         [-halfExtents[0], -halfExtents[1], -halfExtents[2]]]
+
+  color = [1, 0, 0]
+  p.addUserDebugLine(pts[0],
+                     pts[1],
+                     color,
+                     1,
+                     parentObjectUniqueId=parentUid,
+                     parentLinkIndex=parentLinkIndex)
+  p.addUserDebugLine(pts[1],
+                     pts[3],
+                     color,
+                     1,
+                     parentObjectUniqueId=parentUid,
+                     parentLinkIndex=parentLinkIndex)
+  p.addUserDebugLine(pts[3],
+                     pts[2],
+                     color,
+                     1,
+                     parentObjectUniqueId=parentUid,
+                     parentLinkIndex=parentLinkIndex)
+  p.addUserDebugLine(pts[2],
+                     pts[0],
+                     color,
+                     1,
+                     parentObjectUniqueId=parentUid,
+                     parentLinkIndex=parentLinkIndex)
+
+  p.addUserDebugLine(pts[0],
+                     pts[4],
+                     color,
+                     1,
+                     parentObjectUniqueId=parentUid,
+                     parentLinkIndex=parentLinkIndex)
+  p.addUserDebugLine(pts[1],
+                     pts[5],
+                     color,
+                     1,
+                     parentObjectUniqueId=parentUid,
+                     parentLinkIndex=parentLinkIndex)
+  p.addUserDebugLine(pts[2],
+                     pts[6],
+                     color,
+                     1,
+                     parentObjectUniqueId=parentUid,
+                     parentLinkIndex=parentLinkIndex)
+  p.addUserDebugLine(pts[3],
+                     pts[7],
+                     color,
+                     1,
+                     parentObjectUniqueId=parentUid,
+                     parentLinkIndex=parentLinkIndex)
+
+  p.addUserDebugLine(pts[4 + 0],
+                     pts[4 + 1],
+                     color,
+                     1,
+                     parentObjectUniqueId=parentUid,
+                     parentLinkIndex=parentLinkIndex)
+  p.addUserDebugLine(pts[4 + 1],
+                     pts[4 + 3],
+                     color,
+                     1,
+                     parentObjectUniqueId=parentUid,
+                     parentLinkIndex=parentLinkIndex)
+  p.addUserDebugLine(pts[4 + 3],
+                     pts[4 + 2],
+                     color,
+                     1,
+                     parentObjectUniqueId=parentUid,
+                     parentLinkIndex=parentLinkIndex)
+  p.addUserDebugLine(pts[4 + 2],
+                     pts[4 + 0],
+                     color,
+                     1,
+                     parentObjectUniqueId=parentUid,
+                     parentLinkIndex=parentLinkIndex)
+
+
+drawInertiaBox(cubeId, -1)
+
+t = 0
+while 1:
+  t = t + 1
+  if t > 400:
+    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)
+  print(mass2, frictionCoeff2)
+  time.sleep(1. / 240.)
+  p.stepSimulation()

Engine/lib/bullet/examples/pybullet/examples/restitution.py

@@ -0,0 +1,43 @@
+#you can set the restitution (bouncyness) of an object in the URDF file
+#or using changeDynamics
+
+import pybullet as p
+import time
+import pybullet_data
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+  cid = p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+restitutionId = p.addUserDebugParameter("restitution", 0, 1, 1)
+restitutionVelocityThresholdId = p.addUserDebugParameter("res. vel. threshold", 0, 3, 0.2)
+
+lateralFrictionId = p.addUserDebugParameter("lateral friction", 0, 1, 0.5)
+spinningFrictionId = p.addUserDebugParameter("spinning friction", 0, 1, 0.03)
+rollingFrictionId = p.addUserDebugParameter("rolling friction", 0, 1, 0.03)
+
+plane = p.loadURDF("plane_with_restitution.urdf")
+sphere = p.loadURDF("sphere_with_restitution.urdf", [0, 0, 2])
+
+p.setRealTimeSimulation(1)
+p.setGravity(0, 0, -10)
+while (1):
+  restitution = p.readUserDebugParameter(restitutionId)
+  restitutionVelocityThreshold = p.readUserDebugParameter(restitutionVelocityThresholdId)
+  p.setPhysicsEngineParameter(restitutionVelocityThreshold=restitutionVelocityThreshold)
+
+  lateralFriction = p.readUserDebugParameter(lateralFrictionId)
+  spinningFriction = p.readUserDebugParameter(spinningFrictionId)
+  rollingFriction = p.readUserDebugParameter(rollingFrictionId)
+  p.changeDynamics(plane, -1, lateralFriction=1)
+  p.changeDynamics(sphere, -1, lateralFriction=lateralFriction)
+  p.changeDynamics(sphere, -1, spinningFriction=spinningFriction)
+  p.changeDynamics(sphere, -1, rollingFriction=rollingFriction)
+
+  p.changeDynamics(plane, -1, restitution=restitution)
+  p.changeDynamics(sphere, -1, restitution=restitution)
+  pos, orn = p.getBasePositionAndOrientation(sphere)
+  #print("pos=")
+  #print(pos)
+  time.sleep(0.01)

Engine/lib/bullet/examples/pybullet/examples/robotcontrol.py

@@ -0,0 +1,31 @@
+import pybullet as p
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+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()

Engine/lib/bullet/examples/pybullet/examples/rollPitchYaw.py

@@ -0,0 +1,28 @@
+import pybullet as p
+import time
+import pybullet_data
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+  p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+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

Engine/lib/bullet/examples/pybullet/examples/rotationalFriction.py

@@ -0,0 +1,18 @@
+import time
+import pybullet as p
+import pybullet_data as pd
+useMaximalCoordinatesA = True
+useMaximalCoordinatesB = True
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setAdditionalSearchPath(pd.getDataPath())
+cube=p.loadURDF("cube_rotate.urdf",useMaximalCoordinates=useMaximalCoordinatesA)
+p.loadURDF("sphere2.urdf",[0,0,2],useMaximalCoordinates=useMaximalCoordinatesB)
+p.setGravity(0,0,-10)
+p.setJointMotorControl2(cube,0,p.VELOCITY_CONTROL,targetVelocity=1, force=100)
+while (1):
+  p.stepSimulation()
+  time.sleep(1./240.)
+

Engine/lib/bullet/examples/pybullet/examples/satCollision.py

@@ -0,0 +1,21 @@
+import pybullet as p
+import time
+
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setGravity(0, 0, -10)
+p.setPhysicsEngineParameter(enableSAT=1)
+p.loadURDF("cube_concave.urdf", [0, 0, -25],
+           globalScaling=50,
+           useFixedBase=True,
+           flags=p.URDF_INITIALIZE_SAT_FEATURES)
+p.loadURDF("cube.urdf", [0, 0, 1], globalScaling=1, flags=p.URDF_INITIALIZE_SAT_FEATURES)
+p.loadURDF("duck_vhacd.urdf", [1, 0, 1], globalScaling=1, flags=p.URDF_INITIALIZE_SAT_FEATURES)
+
+while (p.isConnected()):
+  p.stepSimulation()
+  pts = p.getContactPoints()
+  #print("num contacts = ", len(pts))
+  time.sleep(1. / 240.)

Engine/lib/bullet/examples/pybullet/examples/saveRestoreState.py

@@ -0,0 +1,139 @@
+import pybullet as p
+import math, time
+import difflib, sys
+import pybullet_data
+
+
+numSteps = 500
+numSteps2 = 30
+p.connect(p.GUI, options="--width=1024 --height=768")
+numObjects = 50
+verbose = 0
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "saveRestoreTimings.log")
+
+
+def setupWorld():
+  p.resetSimulation()
+  p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
+  p.loadURDF("planeMesh.urdf")
+  kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", [0, 0, 10])
+  for i in range(p.getNumJoints(kukaId)):
+    p.setJointMotorControl2(kukaId, i, p.POSITION_CONTROL, force=0)
+  for i in range(numObjects):
+    cube = p.loadURDF("cube_small.urdf", [0, i * 0.02, (i + 1) * 0.2])
+    #p.changeDynamics(cube,-1,mass=100)
+  p.stepSimulation()
+  p.setGravity(0, 0, -10)
+
+
+def dumpStateToFile(file):
+  for i in range(p.getNumBodies()):
+    pos, orn = p.getBasePositionAndOrientation(i)
+    linVel, angVel = p.getBaseVelocity(i)
+    txtPos = "pos=" + str(pos) + "\n"
+    txtOrn = "orn=" + str(orn) + "\n"
+    txtLinVel = "linVel" + str(linVel) + "\n"
+    txtAngVel = "angVel" + str(angVel) + "\n"
+    file.write(txtPos)
+    file.write(txtOrn)
+    file.write(txtLinVel)
+    file.write(txtAngVel)
+
+
+def compareFiles(file1, file2):
+  diff = difflib.unified_diff(
+      file1.readlines(),
+      file2.readlines(),
+      fromfile='saveFile.txt',
+      tofile='restoreFile.txt',
+  )
+  numDifferences = 0
+  for line in diff:
+    numDifferences = numDifferences + 1
+    sys.stdout.write(line)
+  if (numDifferences > 0):
+    print("Error:", numDifferences, " lines are different between files.")
+  else:
+    print("OK, files are identical")
+
+
+setupWorld()
+for i in range(numSteps):
+  p.stepSimulation()
+p.saveBullet("state.bullet")
+if verbose:
+  p.setInternalSimFlags(1)
+p.stepSimulation()
+if verbose:
+  p.setInternalSimFlags(0)
+  print("contact points=")
+  for q in p.getContactPoints():
+    print(q)
+
+for i in range(numSteps2):
+  p.stepSimulation()
+
+file = open("saveFile.txt", "w")
+dumpStateToFile(file)
+file.close()
+
+#################################
+setupWorld()
+
+#both restore from file or from in-memory state should work
+p.restoreState(fileName="state.bullet")
+stateId = p.saveState()
+print("stateId=", stateId)
+p.removeState(stateId)
+stateId = p.saveState()
+print("stateId=", stateId)
+
+if verbose:
+  p.setInternalSimFlags(1)
+p.stepSimulation()
+if verbose:
+  p.setInternalSimFlags(0)
+  print("contact points restored=")
+  for q in p.getContactPoints():
+    print(q)
+for i in range(numSteps2):
+  p.stepSimulation()
+
+file = open("restoreFile.txt", "w")
+dumpStateToFile(file)
+file.close()
+
+p.restoreState(stateId)
+if verbose:
+  p.setInternalSimFlags(1)
+p.stepSimulation()
+if verbose:
+  p.setInternalSimFlags(0)
+  print("contact points restored=")
+  for q in p.getContactPoints():
+    print(q)
+for i in range(numSteps2):
+  p.stepSimulation()
+
+file = open("restoreFile2.txt", "w")
+dumpStateToFile(file)
+file.close()
+
+file1 = open("saveFile.txt", "r")
+file2 = open("restoreFile.txt", "r")
+compareFiles(file1, file2)
+file1.close()
+file2.close()
+
+file1 = open("saveFile.txt", "r")
+file2 = open("restoreFile2.txt", "r")
+compareFiles(file1, file2)
+file1.close()
+file2.close()
+
+p.stopStateLogging(logId)
+
+#while (p.getConnectionInfo()["isConnected"]):
+#	time.sleep(1)

Engine/lib/bullet/examples/pybullet/examples/saveWorld.py

@@ -0,0 +1,11 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.SHARED_MEMORY)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+timestr = time.strftime("%Y%m%d-%H%M%S")
+filename = "saveWorld" + timestr + ".py"
+p.saveWorld(filename)
+p.disconnect()