add yapf style and apply yapf to format all Python files

This recreates pull request #2192

.style.yapf

@@ -0,0 +1,5 @@
+[style]
+based_on_style = google
+column_limit = 99
+indent_width = 2
+

Extras/Serialize/HeaderGenerator/blenderGenerate.py

@@ -1,6 +1,5 @@
 import dump
 
-
 header = """/* Copyright (C) 2006 Charlie C
 *
 * This software is provided 'as-is', without any express or implied
@@ -27,27 +26,30 @@ dtList = dump.DataTypeList
 out = "../BlenderSerialize/autogenerated/"
 spaces = 4
 
+
 def addSpaces(file, space):
-	for i in range(0, space):
-		file.write(" ")
+  for i in range(0, space):
+    file.write(" ")
+
 
 def write(file, spaces, string):
-	addSpaces(file, spaces)
-	file.write(string)
+  addSpaces(file, spaces)
+  file.write(string)
+
 
 ###################################################################################
-blender = open(out+"blender.h", 'w')
+blender = open(out + "blender.h", 'w')
 blender.write(header)
 blender.write("#ifndef __BLENDER_H__\n")
 blender.write("#define __BLENDER_H__\n")
 for dt in dtList:
-	blender.write("#include \"%s.h\"\n"%dt.filename)
+  blender.write("#include \"%s.h\"\n" % dt.filename)
 
 blender.write("#endif//__BLENDER_H__")
 blender.close()
 
 ###################################################################################
-blenderC = open(out+"blender_Common.h", 'w')
+blenderC = open(out + "blender_Common.h", 'w')
 blenderC.write(header)
 blenderC.write("#ifndef __BLENDERCOMMON_H__\n")
 blenderC.write("#define __BLENDERCOMMON_H__\n")
@@ -63,49 +65,43 @@ blenderC.write(strUnRes)
 
 blenderC.write("namespace Blender {\n")
 for dt in dtList:
-	write(blenderC, 4, "class %s;\n"%dt.name)
+  write(blenderC, 4, "class %s;\n" % dt.name)
 
 blenderC.write("}\n")
 blenderC.write("#endif//__BLENDERCOMMON_H__")
 blenderC.close()
 
-
 for dt in dtList:
-	fp = open(out+dt.filename+".h", 'w')
-	
-	fp.write(header)
-	strUpper = dt.filename.upper()
-	
-	fp.write("#ifndef __%s__H__\n"%strUpper)
-	fp.write("#define __%s__H__\n"%strUpper)
-	fp.write("\n\n")
-	
-	
-	fp.write("// -------------------------------------------------- //\n")
-	fp.write("#include \"blender_Common.h\"\n")
+  fp = open(out + dt.filename + ".h", 'w')
 
-	for i in dt.includes:
-		fp.write("#include \"%s\"\n"%i)
+  fp.write(header)
+  strUpper = dt.filename.upper()
 
-	fp.write("\nnamespace Blender {\n")
-	fp.write("\n\n")
+  fp.write("#ifndef __%s__H__\n" % strUpper)
+  fp.write("#define __%s__H__\n" % strUpper)
+  fp.write("\n\n")
 
-	addSpaces(fp,4)
-	fp.write("// ---------------------------------------------- //\n")
+  fp.write("// -------------------------------------------------- //\n")
+  fp.write("#include \"blender_Common.h\"\n")
 
+  for i in dt.includes:
+    fp.write("#include \"%s\"\n" % i)
 
-	write(fp, 4, "class %s\n"%dt.name)
+  fp.write("\nnamespace Blender {\n")
+  fp.write("\n\n")
 
-	write(fp, 4, "{\n")
-	write(fp, 4, "public:\n")
-	for i in dt.dataTypes:
-		write(fp, 8, i+";\n")
+  addSpaces(fp, 4)
+  fp.write("// ---------------------------------------------- //\n")
 
+  write(fp, 4, "class %s\n" % dt.name)
 
-	write(fp, 4, "};\n")
-	fp.write("}\n")
-	fp.write("\n\n")
-	fp.write("#endif//__%s__H__\n"%strUpper)
-	fp.close()
+  write(fp, 4, "{\n")
+  write(fp, 4, "public:\n")
+  for i in dt.dataTypes:
+    write(fp, 8, i + ";\n")
 
-	
+  write(fp, 4, "};\n")
+  fp.write("}\n")
+  fp.write("\n\n")
+  fp.write("#endif//__%s__H__\n" % strUpper)
+  fp.close()

Extras/Serialize/HeaderGenerator/bulletGenerate.py

@@ -2,8 +2,6 @@ import sys
 sys.path.append(".")
 import dump
 
-
-
 header = """/* Copyright (C) 2011 Erwin Coumans & Charlie C
 *
 * This software is provided 'as-is', without any express or implied
@@ -30,23 +28,25 @@ dtList = dump.DataTypeList
 out = "autogenerated/"
 spaces = 4
 
+
 def addSpaces(file, space):
-	for i in range(0, space):
-		file.write(" ")
+  for i in range(0, space):
+    file.write(" ")
+
 
 def write(file, spaces, string):
-	addSpaces(file, spaces)
-	file.write(string)
+  addSpaces(file, spaces)
+  file.write(string)
+
 
 ###################################################################################
-blender = open(out+"bullet.h", 'w')
+blender = open(out + "bullet.h", 'w')
 blender.write(header)
 blender.write("#ifndef __BULLET_H__\n")
 blender.write("#define __BULLET_H__\n")
 #for dt in dtList:
 #	blender.write("struct %s;\n"%dt.filename)
 
-
 ###################################################################################
 
 blender.write("namespace Bullet {\n")
@@ -61,26 +61,25 @@ typedef struct bInvalidHandle {
 blender.write(strUnRes)
 
 for dt in dtList:
-	write(blender, 4, "class %s;\n"%dt.name)
-
+  write(blender, 4, "class %s;\n" % dt.name)
 
 for dt in dtList:
-		
-	strUpper = dt.filename.upper()
-	
-	blender.write("// -------------------------------------------------- //\n")
 
-	write(blender, 4, "class %s\n"%dt.name)
+  strUpper = dt.filename.upper()
 
-	write(blender, 4, "{\n")
-	write(blender, 4, "public:\n")
-	for i in dt.dataTypes:
-		write(blender, 8, i+";\n")
+  blender.write("// -------------------------------------------------- //\n")
+
+  write(blender, 4, "class %s\n" % dt.name)
+
+  write(blender, 4, "{\n")
+  write(blender, 4, "public:\n")
+  for i in dt.dataTypes:
+    write(blender, 8, i + ";\n")
+
+  write(blender, 4, "};\n")
+
+  blender.write("\n\n")
 
-	write(blender, 4, "};\n")
-	
-	blender.write("\n\n")
-	
 blender.write("}\n")
 blender.write("#endif//__BULLET_H__")
 blender.close()

build3/cmake/FindLibPython.py

@@ -15,11 +15,11 @@ print("exec_prefix:%s" % sys.exec_prefix)
 print("major_version:%s" % str(sys.version_info[0]))
 print("minor_version:%s" % str(sys.version_info[1]))
 print("patch_version:%s" % str(sys.version_info[2]))
-print("short_version:%s" % '.'.join(map(lambda x:str(x), sys.version_info[0:2])))
-print("long_version:%s" % '.'.join(map(lambda x:str(x), sys.version_info[0:3])))
+print("short_version:%s" % '.'.join(map(lambda x: str(x), sys.version_info[0:2])))
+print("long_version:%s" % '.'.join(map(lambda x: str(x), sys.version_info[0:3])))
 print("py_inc_dir:%s" % distutils.sysconfig.get_python_inc())
 print("site_packages_dir:%s" % distutils.sysconfig.get_python_lib(plat_specific=1))
-for e in distutils.sysconfig.get_config_vars ('LIBDIR'):
-    if e != None:
-        print("py_lib_dir:%s" % e)
-        break
+for e in distutils.sysconfig.get_config_vars('LIBDIR'):
+  if e != None:
+    print("py_lib_dir:%s" % e)
+    break

data/dinnerware/generate.py

@@ -1,252 +1,253 @@
 from __future__ import print_function
 import numpy as np
 
+
 class Obj:
-	def __init__(self, fn):
-		self.ind_v  = 0
-		self.ind_vt = 0
-		self.ind_vn = 0
-		self.fn = fn
-		self.out = open(fn + ".tmp", "w")
-		self.out.write("mtllib dinnerware.mtl\n")
-	def __del__(self):
-		self.out.close()
-		import shutil
-		shutil.move(self.fn + ".tmp", self.fn)
-	def push_v(self, v):
-		self.out.write("v %f %f %f\n" % (v[0],v[1],v[2]))
-		self.ind_v += 1
-		return self.ind_v
-	def push_vt(self, vt):
-		self.out.write("vt %f %f\n" % (vt[0],vt[1]))
-		self.ind_vt += 1
-		return self.ind_vt
-	def push_vn(self, vn):
-		vn /= np.linalg.norm(vn)
-		self.out.write("vn %f %f %f\n" % (vn[0],vn[1],vn[2]))
-		self.ind_vn += 1
-		return self.ind_vn
+
+  def __init__(self, fn):
+    self.ind_v = 0
+    self.ind_vt = 0
+    self.ind_vn = 0
+    self.fn = fn
+    self.out = open(fn + ".tmp", "w")
+    self.out.write("mtllib dinnerware.mtl\n")
+
+  def __del__(self):
+    self.out.close()
+    import shutil
+    shutil.move(self.fn + ".tmp", self.fn)
+
+  def push_v(self, v):
+    self.out.write("v %f %f %f\n" % (v[0], v[1], v[2]))
+    self.ind_v += 1
+    return self.ind_v
+
+  def push_vt(self, vt):
+    self.out.write("vt %f %f\n" % (vt[0], vt[1]))
+    self.ind_vt += 1
+    return self.ind_vt
+
+  def push_vn(self, vn):
+    vn /= np.linalg.norm(vn)
+    self.out.write("vn %f %f %f\n" % (vn[0], vn[1], vn[2]))
+    self.ind_vn += 1
+    return self.ind_vn
 
 
 def convex_hull(points, vind, nind, tind, obj):
-	"super ineffective"
-	cnt = len(points)
-	for a in range(cnt):
-		for b in range(a+1,cnt):
-			for c in range(b+1,cnt):
-				vec1 = points[a] - points[b]
-				vec2 = points[a] - points[c]
-				n  = np.cross(vec1, vec2)
-				n /= np.linalg.norm(n)
-				C = np.dot(n, points[a])
-				inner = np.inner(n, points)
-				pos = (inner <= C+0.0001).all()
-				neg = (inner >= C-0.0001).all()
-				if not pos and not neg: continue
-				obj.out.write("f %i//%i %i//%i %i//%i\n" % ( 
-					(vind[a], nind[a], vind[b], nind[b], vind[c], nind[c])
-					if (inner - C).sum() < 0 else
-					(vind[a], nind[a], vind[c], nind[c], vind[b], nind[b]) ) )
-				#obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % ( 
-				#	(vind[a], tind[a], nind[a], vind[b], tind[b], nind[b], vind[c], tind[c], nind[c])
-				#	if (inner - C).sum() < 0 else
-				#	(vind[a], tind[a], nind[a], vind[c], tind[c], nind[c], vind[b], tind[b], nind[b]) ) )
+  "super ineffective"
+  cnt = len(points)
+  for a in range(cnt):
+    for b in range(a + 1, cnt):
+      for c in range(b + 1, cnt):
+        vec1 = points[a] - points[b]
+        vec2 = points[a] - points[c]
+        n = np.cross(vec1, vec2)
+        n /= np.linalg.norm(n)
+        C = np.dot(n, points[a])
+        inner = np.inner(n, points)
+        pos = (inner <= C + 0.0001).all()
+        neg = (inner >= C - 0.0001).all()
+        if not pos and not neg: continue
+        obj.out.write("f %i//%i %i//%i %i//%i\n" %
+                      ((vind[a], nind[a], vind[b], nind[b], vind[c], nind[c]) if
+                       (inner - C).sum() < 0 else
+                       (vind[a], nind[a], vind[c], nind[c], vind[b], nind[b])))
+        #obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % (
+        #	(vind[a], tind[a], nind[a], vind[b], tind[b], nind[b], vind[c], tind[c], nind[c])
+        #	if (inner - C).sum() < 0 else
+        #	(vind[a], tind[a], nind[a], vind[c], tind[c], nind[c], vind[b], tind[b], nind[b]) ) )
+
 
 def test_convex_hull():
-	obj = Obj("convex_test.obj")
-	vlist = np.random.uniform( low=-0.1, high=+0.1, size=(100,3) )
-	nlist = vlist.copy()
-	tlist = np.random.uniform( low=0, high=+1, size=(100,2) )
-	vind = [obj.push_v(xyz) for xyz in vlist]
-	nind = [obj.push_vn(xyz) for xyz in nlist]
-	tind = [obj.push_vt(uv) for uv in tlist]
-	convex_hull(vlist, vind, nind, tind, obj)
+  obj = Obj("convex_test.obj")
+  vlist = np.random.uniform(low=-0.1, high=+0.1, size=(100, 3))
+  nlist = vlist.copy()
+  tlist = np.random.uniform(low=0, high=+1, size=(100, 2))
+  vind = [obj.push_v(xyz) for xyz in vlist]
+  nind = [obj.push_vn(xyz) for xyz in nlist]
+  tind = [obj.push_vt(uv) for uv in tlist]
+  convex_hull(vlist, vind, nind, tind, obj)
+
 
 class Contour:
-	def __init__(self):
-		self.vprev_vind = None
 
-	def f(self, obj, vlist_vind, vlist_tind, vlist_nind):
-		cnt = len(vlist_vind)
-		for i1 in range(cnt):
-			i2 = i1-1
-			obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % ( 
-				vlist_vind[i2], vlist_tind[i2], vlist_nind[i2],
-				vlist_vind[i1], vlist_tind[i1], vlist_nind[i1],
-				self.vprev_vind[i1], self.vprev_tind[i1], self.vprev_nind[i1],
-				) )
-			obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % ( 
-				vlist_vind[i2], vlist_tind[i2], vlist_nind[i2],
-				self.vprev_vind[i1], self.vprev_tind[i1], self.vprev_nind[i1],
-				self.vprev_vind[i2], self.vprev_tind[i2], self.vprev_nind[i2],
-				) )
+  def __init__(self):
+    self.vprev_vind = None
 
-	def belt(self, obj, vlist, nlist, tlist):
-		vlist_vind = [obj.push_v(xyz)  for xyz in vlist]
-		vlist_tind = [obj.push_vt(xyz) for xyz in tlist]
-		vlist_nind = [obj.push_vn(xyz) for xyz in nlist]
-		if self.vprev_vind:
-			self.f(obj, vlist_vind, vlist_tind, vlist_nind)
-		else:
-			self.first_vind = vlist_vind
-			self.first_tind = vlist_tind
-			self.first_nind = vlist_nind
-		self.vprev_vind = vlist_vind
-		self.vprev_tind = vlist_tind
-		self.vprev_nind = vlist_nind
+  def f(self, obj, vlist_vind, vlist_tind, vlist_nind):
+    cnt = len(vlist_vind)
+    for i1 in range(cnt):
+      i2 = i1 - 1
+      obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % (
+          vlist_vind[i2],
+          vlist_tind[i2],
+          vlist_nind[i2],
+          vlist_vind[i1],
+          vlist_tind[i1],
+          vlist_nind[i1],
+          self.vprev_vind[i1],
+          self.vprev_tind[i1],
+          self.vprev_nind[i1],
+      ))
+      obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % (
+          vlist_vind[i2],
+          vlist_tind[i2],
+          vlist_nind[i2],
+          self.vprev_vind[i1],
+          self.vprev_tind[i1],
+          self.vprev_nind[i1],
+          self.vprev_vind[i2],
+          self.vprev_tind[i2],
+          self.vprev_nind[i2],
+      ))
+
+  def belt(self, obj, vlist, nlist, tlist):
+    vlist_vind = [obj.push_v(xyz) for xyz in vlist]
+    vlist_tind = [obj.push_vt(xyz) for xyz in tlist]
+    vlist_nind = [obj.push_vn(xyz) for xyz in nlist]
+    if self.vprev_vind:
+      self.f(obj, vlist_vind, vlist_tind, vlist_nind)
+    else:
+      self.first_vind = vlist_vind
+      self.first_tind = vlist_tind
+      self.first_nind = vlist_nind
+    self.vprev_vind = vlist_vind
+    self.vprev_tind = vlist_tind
+    self.vprev_nind = vlist_nind
+
+  def finish(self, obj):
+    self.f(obj, self.first_vind, self.first_tind, self.first_nind)
 
-	def finish(self, obj):
-		self.f(obj, self.first_vind, self.first_tind, self.first_nind)
 
 def test_contour():
-	RAD1 = 2.0
-	RAD2 = 1.5
-	obj = Obj("torus.obj")
-	obj.out.write("usemtl porcelain\n")
-	contour = Contour()
-	for step in range(100):
-		angle = step/100.0*2*np.pi
-		belt_v = []
-		belt_n = []
-		belt_t = []
-		for b in range(50):
-			beta = b/50.0*2*np.pi
-			r = RAD2*np.cos(beta) + RAD1
-			z = RAD2*np.sin(beta)
-			belt_v.append( np.array( [
-				np.cos(angle)*r,
-				np.sin(angle)*r,
-				z] ) )
-			belt_n.append( np.array( [
-				np.cos(angle)*np.cos(beta),
-				np.sin(angle)*np.cos(beta),
-				np.sin(beta)] ) )
-			belt_t.append( (0,0) )
-		contour.belt(obj, belt_v, belt_n, belt_t)
-	contour.finish(obj)
+  RAD1 = 2.0
+  RAD2 = 1.5
+  obj = Obj("torus.obj")
+  obj.out.write("usemtl porcelain\n")
+  contour = Contour()
+  for step in range(100):
+    angle = step / 100.0 * 2 * np.pi
+    belt_v = []
+    belt_n = []
+    belt_t = []
+    for b in range(50):
+      beta = b / 50.0 * 2 * np.pi
+      r = RAD2 * np.cos(beta) + RAD1
+      z = RAD2 * np.sin(beta)
+      belt_v.append(np.array([np.cos(angle) * r, np.sin(angle) * r, z]))
+      belt_n.append(
+          np.array([np.cos(angle) * np.cos(beta),
+                    np.sin(angle) * np.cos(beta),
+                    np.sin(beta)]))
+      belt_t.append((0, 0))
+    contour.belt(obj, belt_v, belt_n, belt_t)
+  contour.finish(obj)
+
 
 #test_convex_hull()
 #test_contour()
 
+
 class RotationFigureParams:
-	pass
+  pass
+
 
 def generate_plate(p, obj, collision_prefix):
-	contour = Contour()
-	belt_vlist_3d_prev = None
+  contour = Contour()
+  belt_vlist_3d_prev = None
 
-	for step in range(p.N_VIZ+1):
-		angle = step/float(p.N_VIZ)*2*np.pi
+  for step in range(p.N_VIZ + 1):
+    angle = step / float(p.N_VIZ) * 2 * np.pi
 
-		if step % p.COLLISION_EVERY == 0:
-			vlist_3d = []
-			for x,y in p.belt_simple:
-				vlist_3d.append( [
-					np.cos(angle)*x*1.06,
-					np.sin(angle)*x*1.06,
-					y
-					] )
-			if belt_vlist_3d_prev:
-				obj2 = Obj(collision_prefix % (step / p.COLLISION_EVERY))
-				obj2.out.write("usemtl pan_tefal\n")
-				vlist = np.array( vlist_3d + belt_vlist_3d_prev )
-				vlist[len(vlist_3d):] *= 1.01  # break points on one plane
-				vlist[0,0:2] += 0.01*vlist[len(vlist_3d),0:2]
-				vlist[len(vlist_3d),0:2] += 0.01*vlist[0,0:2]
-				nlist = np.random.uniform( low=-1, high=+1, size=vlist.shape )
-				tlist = np.random.uniform( low=0, high=+1, size=(len(vlist),2) )
-				vind = [obj2.push_v(xyz) for xyz in vlist]
-				nind = [obj2.push_vn(xyz) for xyz in nlist]
-				convex_hull(vlist, vind, nind, None, obj2)
-			belt_vlist_3d_prev = vlist_3d
-		if step==p.N_VIZ: break
+    if step % p.COLLISION_EVERY == 0:
+      vlist_3d = []
+      for x, y in p.belt_simple:
+        vlist_3d.append([np.cos(angle) * x * 1.06, np.sin(angle) * x * 1.06, y])
+      if belt_vlist_3d_prev:
+        obj2 = Obj(collision_prefix % (step / p.COLLISION_EVERY))
+        obj2.out.write("usemtl pan_tefal\n")
+        vlist = np.array(vlist_3d + belt_vlist_3d_prev)
+        vlist[len(vlist_3d):] *= 1.01  # break points on one plane
+        vlist[0, 0:2] += 0.01 * vlist[len(vlist_3d), 0:2]
+        vlist[len(vlist_3d), 0:2] += 0.01 * vlist[0, 0:2]
+        nlist = np.random.uniform(low=-1, high=+1, size=vlist.shape)
+        tlist = np.random.uniform(low=0, high=+1, size=(len(vlist), 2))
+        vind = [obj2.push_v(xyz) for xyz in vlist]
+        nind = [obj2.push_vn(xyz) for xyz in nlist]
+        convex_hull(vlist, vind, nind, None, obj2)
+      belt_vlist_3d_prev = vlist_3d
+    if step == p.N_VIZ: break
 
-		belt_v = []
-		belt_n = []
-		belt_t = []
-		for x,y,nx,ny in p.belt:
-			belt_v.append( np.array( [
-				np.cos(angle)*x,
-				np.sin(angle)*x,
-				y
-				] ) )
-			belt_n.append( np.array( [
-				np.cos(angle)*nx,
-				np.sin(angle)*nx,
-				ny
-				] ) )
-			if ny-nx >= 0:
-				belt_t.append( (
-					127.0/512 + np.cos(angle)*x/p.RAD_HIGH*105/512,
-					(512-135.0)/512 + np.sin(angle)*x/p.RAD_HIGH*105/512) )
-			else:
-				belt_t.append( (
-					382.0/512 + np.cos(angle)*x/p.RAD_HIGH*125/512,
-					(512-380.0)/512 + np.sin(angle)*x/p.RAD_HIGH*125/512) )
-		contour.belt(obj, belt_v, belt_n, belt_t)
+    belt_v = []
+    belt_n = []
+    belt_t = []
+    for x, y, nx, ny in p.belt:
+      belt_v.append(np.array([np.cos(angle) * x, np.sin(angle) * x, y]))
+      belt_n.append(np.array([np.cos(angle) * nx, np.sin(angle) * nx, ny]))
+      if ny - nx >= 0:
+        belt_t.append((127.0 / 512 + np.cos(angle) * x / p.RAD_HIGH * 105 / 512,
+                       (512 - 135.0) / 512 + np.sin(angle) * x / p.RAD_HIGH * 105 / 512))
+      else:
+        belt_t.append((382.0 / 512 + np.cos(angle) * x / p.RAD_HIGH * 125 / 512,
+                       (512 - 380.0) / 512 + np.sin(angle) * x / p.RAD_HIGH * 125 / 512))
+    contour.belt(obj, belt_v, belt_n, belt_t)
+
+  contour.finish(obj)
 
-	contour.finish(obj)
 
 def tefal():
-	p = RotationFigureParams()
-	p.RAD_LOW  = 0.240/2
-	p.RAD_HIGH = 0.255/2
-	p.H   = 0.075
-	p.THICK = 0.005
-	p.N_VIZ = 30
-	p.COLLISION_EVERY = 5
-	p.belt = [
-		(p.RAD_HIGH-p.THICK, p.H,     -1,0),  # x y norm
-		(p.RAD_HIGH      , p.H,      0,1),
-		(p.RAD_HIGH+p.THICK, p.H,     +1,0),
-		(p.RAD_LOW+p.THICK, p.THICK,   +1,0),
-		(p.RAD_LOW      , 0,       0,-1),
-		(            0, 0,       0,-1),
-		(            0, p.THICK,   0,1),
-		(p.RAD_LOW-p.THICK, p.THICK,   0,1),
-		(p.RAD_LOW-p.THICK, 3*p.THICK,-1,0),
-		]
-	p.belt.reverse()
-	p.belt_simple = [
-		(p.RAD_HIGH-p.THICK, p.H),
-		(p.RAD_HIGH+p.THICK, p.H),
-		(p.RAD_LOW       , 0),
-		(p.RAD_LOW-p.THICK , 0)
-		]
-	obj = Obj("pan_tefal.obj")
-	obj.out.write("usemtl pan_tefal\n")
-	generate_plate(p, obj, "pan_tefal-collision%02i.obj")
+  p = RotationFigureParams()
+  p.RAD_LOW = 0.240 / 2
+  p.RAD_HIGH = 0.255 / 2
+  p.H = 0.075
+  p.THICK = 0.005
+  p.N_VIZ = 30
+  p.COLLISION_EVERY = 5
+  p.belt = [
+      (p.RAD_HIGH - p.THICK, p.H, -1, 0),  # x y norm
+      (p.RAD_HIGH, p.H, 0, 1),
+      (p.RAD_HIGH + p.THICK, p.H, +1, 0),
+      (p.RAD_LOW + p.THICK, p.THICK, +1, 0),
+      (p.RAD_LOW, 0, 0, -1),
+      (0, 0, 0, -1),
+      (0, p.THICK, 0, 1),
+      (p.RAD_LOW - p.THICK, p.THICK, 0, 1),
+      (p.RAD_LOW - p.THICK, 3 * p.THICK, -1, 0),
+  ]
+  p.belt.reverse()
+  p.belt_simple = [(p.RAD_HIGH - p.THICK, p.H), (p.RAD_HIGH + p.THICK, p.H), (p.RAD_LOW, 0),
+                   (p.RAD_LOW - p.THICK, 0)]
+  obj = Obj("pan_tefal.obj")
+  obj.out.write("usemtl pan_tefal\n")
+  generate_plate(p, obj, "pan_tefal-collision%02i.obj")
+
 
 def plate():
-	p = RotationFigureParams()
-	p.RAD_LOW  = 0.110/2
-	p.RAD_HIGH = 0.190/2
-	p.H   = 0.060
-	p.THICK = 0.003
-	p.N_VIZ = 30
-	p.COLLISION_EVERY = 5
-	p.belt = [
-		(p.RAD_HIGH-p.THICK, p.H,     -0.9,0.5),  # x y norm
-		(p.RAD_HIGH      , p.H,      0,1),
-		(p.RAD_HIGH+p.THICK, p.H,     +1,0),
-		(p.RAD_LOW+p.THICK, p.THICK,   +1,0),
-		(p.RAD_LOW      , 0,       0,-1),
-		(            0, 0,       0,-1),
-		(            0, p.THICK,   0,1),
-		(p.RAD_LOW-3*p.THICK, p.THICK,   0,1),
-		(p.RAD_LOW-p.THICK, 3*p.THICK,-0.5,1.0),
-		]
-	p.belt.reverse()
-	p.belt_simple = [
-		(p.RAD_HIGH-p.THICK, p.H),
-		(p.RAD_HIGH+p.THICK, p.H),
-		(p.RAD_LOW       , 0),
-		(p.RAD_LOW-p.THICK , 0)
-		]
-	obj = Obj("plate.obj")
-	obj.out.write("usemtl solid_color\n")
-	generate_plate(p, obj, "plate-collision%02i.obj")
+  p = RotationFigureParams()
+  p.RAD_LOW = 0.110 / 2
+  p.RAD_HIGH = 0.190 / 2
+  p.H = 0.060
+  p.THICK = 0.003
+  p.N_VIZ = 30
+  p.COLLISION_EVERY = 5
+  p.belt = [
+      (p.RAD_HIGH - p.THICK, p.H, -0.9, 0.5),  # x y norm
+      (p.RAD_HIGH, p.H, 0, 1),
+      (p.RAD_HIGH + p.THICK, p.H, +1, 0),
+      (p.RAD_LOW + p.THICK, p.THICK, +1, 0),
+      (p.RAD_LOW, 0, 0, -1),
+      (0, 0, 0, -1),
+      (0, p.THICK, 0, 1),
+      (p.RAD_LOW - 3 * p.THICK, p.THICK, 0, 1),
+      (p.RAD_LOW - p.THICK, 3 * p.THICK, -0.5, 1.0),
+  ]
+  p.belt.reverse()
+  p.belt_simple = [(p.RAD_HIGH - p.THICK, p.H), (p.RAD_HIGH + p.THICK, p.H), (p.RAD_LOW, 0),
+                   (p.RAD_LOW - p.THICK, 0)]
+  obj = Obj("plate.obj")
+  obj.out.write("usemtl solid_color\n")
+  generate_plate(p, obj, "plate-collision%02i.obj")
+
 
 plate()
-
-

data/quadruped/vision/vision.py

@@ -3,51 +3,44 @@ import time
 import math
 
 pi = 3.14159264
-limitVal = 2*pi
-legpos = 3./4.*pi
+limitVal = 2 * pi
+legpos = 3. / 4. * pi
 legposS = 0
-legposSright = 0#-0.3
-legposSleft = 0#0.3
+legposSright = 0  #-0.3
+legposSleft = 0  #0.3
 
-defaultERP=0.4
+defaultERP = 0.4
 maxMotorForce = 5000
 maxGearForce = 10000
 jointFriction = 0.1
 
-
 p.connect(p.GUI)
 
+amplitudeId = p.addUserDebugParameter("amplitude", 0, 3.14, 0.5)
+timeScaleId = p.addUserDebugParameter("timeScale", 0, 10, 1)
 
-amplitudeId= p.addUserDebugParameter("amplitude",0,3.14,0.5)
-timeScaleId = p.addUserDebugParameter("timeScale",0,10,1)
-
-
-jointTypeNames={}
-jointTypeNames[p.JOINT_REVOLUTE]="JOINT_REVOLUTE"
-jointTypeNames[p.JOINT_FIXED]="JOINT_FIXED"
+jointTypeNames = {}
+jointTypeNames[p.JOINT_REVOLUTE] = "JOINT_REVOLUTE"
+jointTypeNames[p.JOINT_FIXED] = "JOINT_FIXED"
 p.setPhysicsEngineParameter(numSolverIterations=100)
 p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
 #disable rendering during creation.
 #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
-p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
+p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION, 1)
 
+jointNamesToIndex = {}
 
-jointNamesToIndex={}
-
-
-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
-vision = p.loadURDF("vision60.urdf",[0,0,0.4],useFixedBase=False)
-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
-
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+vision = p.loadURDF("vision60.urdf", [0, 0, 0.4], useFixedBase=False)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
 
 for j in range(p.getNumJoints(vision)):
-	jointInfo = p.getJointInfo(vision,j)
-	jointInfoName = jointInfo[1].decode("utf-8") 
-	print("joint ",j," = ",jointInfoName, "type=",jointTypeNames[jointInfo[2]])
-	jointNamesToIndex[jointInfoName ]=j
-	#print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName])
-	p.setJointMotorControl2(vision,j,p.VELOCITY_CONTROL,targetVelocity=0, force=jointFriction)
-
+  jointInfo = p.getJointInfo(vision, j)
+  jointInfoName = jointInfo[1].decode("utf-8")
+  print("joint ", j, " = ", jointInfoName, "type=", jointTypeNames[jointInfo[2]])
+  jointNamesToIndex[jointInfoName] = j
+  #print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName])
+  p.setJointMotorControl2(vision, j, p.VELOCITY_CONTROL, targetVelocity=0, force=jointFriction)
 
 chassis_right_center = jointNamesToIndex['chassis_right_center']
 motor_front_rightR_joint = jointNamesToIndex['motor_front_rightR_joint']
@@ -72,149 +65,277 @@ knee_back_leftL_joint = jointNamesToIndex['knee_back_leftL_joint']
 motor_back_leftR_joint = jointNamesToIndex['motor_back_leftR_joint']
 motor_back_leftS_joint = jointNamesToIndex['motor_back_leftS_joint']
 
-motA_rf_Id= p.addUserDebugParameter("motA_rf",-limitVal,limitVal,legpos)
-motB_rf_Id= p.addUserDebugParameter("motB_rf",-limitVal,limitVal,legpos)
-motC_rf_Id= p.addUserDebugParameter("motC_rf",-limitVal,limitVal,legposSright)
-erp_rf_Id= p.addUserDebugParameter("erp_rf",0,1,defaultERP)
-relPosTarget_rf_Id= p.addUserDebugParameter("relPosTarget_rf",-limitVal,limitVal,0)
+motA_rf_Id = p.addUserDebugParameter("motA_rf", -limitVal, limitVal, legpos)
+motB_rf_Id = p.addUserDebugParameter("motB_rf", -limitVal, limitVal, legpos)
+motC_rf_Id = p.addUserDebugParameter("motC_rf", -limitVal, limitVal, legposSright)
+erp_rf_Id = p.addUserDebugParameter("erp_rf", 0, 1, defaultERP)
+relPosTarget_rf_Id = p.addUserDebugParameter("relPosTarget_rf", -limitVal, limitVal, 0)
 
+motA_lf_Id = p.addUserDebugParameter("motA_lf", -limitVal, limitVal, -legpos)
+motB_lf_Id = p.addUserDebugParameter("motB_lf", -limitVal, limitVal, -legpos)
+motC_lf_Id = p.addUserDebugParameter("motC_lf", -limitVal, limitVal, legposSleft)
 
-motA_lf_Id= p.addUserDebugParameter("motA_lf",-limitVal,limitVal,-legpos)
-motB_lf_Id= p.addUserDebugParameter("motB_lf",-limitVal,limitVal,-legpos)
-motC_lf_Id= p.addUserDebugParameter("motC_lf",-limitVal,limitVal,legposSleft)
+erp_lf_Id = p.addUserDebugParameter("erp_lf", 0, 1, defaultERP)
+relPosTarget_lf_Id = p.addUserDebugParameter("relPosTarget_lf", -limitVal, limitVal, 0)
 
-erp_lf_Id= p.addUserDebugParameter("erp_lf",0,1,defaultERP)
-relPosTarget_lf_Id= p.addUserDebugParameter("relPosTarget_lf",-limitVal,limitVal,0)
+motA_rb_Id = p.addUserDebugParameter("motA_rb", -limitVal, limitVal, legpos)
+motB_rb_Id = p.addUserDebugParameter("motB_rb", -limitVal, limitVal, legpos)
+motC_rb_Id = p.addUserDebugParameter("motC_rb", -limitVal, limitVal, legposSright)
 
-motA_rb_Id= p.addUserDebugParameter("motA_rb",-limitVal,limitVal,legpos)
-motB_rb_Id= p.addUserDebugParameter("motB_rb",-limitVal,limitVal,legpos)
-motC_rb_Id= p.addUserDebugParameter("motC_rb",-limitVal,limitVal,legposSright)
+erp_rb_Id = p.addUserDebugParameter("erp_rb", 0, 1, defaultERP)
+relPosTarget_rb_Id = p.addUserDebugParameter("relPosTarget_rb", -limitVal, limitVal, 0)
 
-erp_rb_Id= p.addUserDebugParameter("erp_rb",0,1,defaultERP)
-relPosTarget_rb_Id= p.addUserDebugParameter("relPosTarget_rb",-limitVal,limitVal,0)
+motA_lb_Id = p.addUserDebugParameter("motA_lb", -limitVal, limitVal, -legpos)
+motB_lb_Id = p.addUserDebugParameter("motB_lb", -limitVal, limitVal, -legpos)
+motC_lb_Id = p.addUserDebugParameter("motC_lb", -limitVal, limitVal, legposSleft)
 
+erp_lb_Id = p.addUserDebugParameter("erp_lb", 0, 1, defaultERP)
+relPosTarget_lb_Id = p.addUserDebugParameter("relPosTarget_lb", -limitVal, limitVal, 0)
 
-motA_lb_Id= p.addUserDebugParameter("motA_lb",-limitVal,limitVal,-legpos)
-motB_lb_Id= p.addUserDebugParameter("motB_lb",-limitVal,limitVal,-legpos)
-motC_lb_Id= p.addUserDebugParameter("motC_lb",-limitVal,limitVal,legposSleft)
-
-erp_lb_Id= p.addUserDebugParameter("erp_lb",0,1,defaultERP)
-relPosTarget_lb_Id= p.addUserDebugParameter("relPosTarget_lb",-limitVal,limitVal,0)
-
-camTargetPos=[0.25,0.62,-0.15]
+camTargetPos = [0.25, 0.62, -0.15]
 camDist = 2
 camYaw = -2
-camPitch=-16
+camPitch = -16
 p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos)
 
+c_rf = p.createConstraint(vision,
+                          knee_front_rightR_joint,
+                          vision,
+                          motor_front_rightL_joint,
+                          jointType=p.JOINT_GEAR,
+                          jointAxis=[0, 1, 0],
+                          parentFramePosition=[0, 0, 0],
+                          childFramePosition=[0, 0, 0])
+p.changeConstraint(c_rf, gearRatio=-1, gearAuxLink=motor_front_rightR_joint, maxForce=maxGearForce)
 
+c_lf = p.createConstraint(vision,
+                          knee_front_leftL_joint,
+                          vision,
+                          motor_front_leftR_joint,
+                          jointType=p.JOINT_GEAR,
+                          jointAxis=[0, 1, 0],
+                          parentFramePosition=[0, 0, 0],
+                          childFramePosition=[0, 0, 0])
+p.changeConstraint(c_lf, gearRatio=-1, gearAuxLink=motor_front_leftL_joint, maxForce=maxGearForce)
 
+c_rb = p.createConstraint(vision,
+                          knee_back_rightR_joint,
+                          vision,
+                          motor_back_rightL_joint,
+                          jointType=p.JOINT_GEAR,
+                          jointAxis=[0, 1, 0],
+                          parentFramePosition=[0, 0, 0],
+                          childFramePosition=[0, 0, 0])
+p.changeConstraint(c_rb, gearRatio=-1, gearAuxLink=motor_back_rightR_joint, maxForce=maxGearForce)
 
-
-c_rf = p.createConstraint(vision,knee_front_rightR_joint,vision,motor_front_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
-p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,maxForce=maxGearForce)
-
-c_lf = p.createConstraint(vision,knee_front_leftL_joint,vision,motor_front_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
-p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,maxForce=maxGearForce)
-
-c_rb = p.createConstraint(vision,knee_back_rightR_joint,vision,motor_back_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
-p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,maxForce=maxGearForce)
-
-c_lb = p.createConstraint(vision,knee_back_leftL_joint,vision,motor_back_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
-p.changeConstraint(c_lb,gearRatio=-1, gearAuxLink = motor_back_leftL_joint,maxForce=maxGearForce)
-
-
-
+c_lb = p.createConstraint(vision,
+                          knee_back_leftL_joint,
+                          vision,
+                          motor_back_leftR_joint,
+                          jointType=p.JOINT_GEAR,
+                          jointAxis=[0, 1, 0],
+                          parentFramePosition=[0, 0, 0],
+                          childFramePosition=[0, 0, 0])
+p.changeConstraint(c_lb, gearRatio=-1, gearAuxLink=motor_back_leftL_joint, maxForce=maxGearForce)
 
 p.setRealTimeSimulation(1)
-for i in range (1):
-#while (1):
-	motA_rf = p.readUserDebugParameter(motA_rf_Id)
-	motB_rf = p.readUserDebugParameter(motB_rf_Id)
-	motC_rf = p.readUserDebugParameter(motC_rf_Id)
-	erp_rf = p.readUserDebugParameter(erp_rf_Id)
-	relPosTarget_rf = p.readUserDebugParameter(relPosTarget_rf_Id)
-	#motC_rf
-	p.setJointMotorControl2(vision,motor_front_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rf,force=maxMotorForce)
-	p.setJointMotorControl2(vision,motor_front_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rf,force=maxMotorForce)
-	p.setJointMotorControl2(vision,motor_front_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rf,force=maxMotorForce)
-	p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,erp=erp_rf, relativePositionTarget=relPosTarget_rf,maxForce=maxGearForce)
+for i in range(1):
+  #while (1):
+  motA_rf = p.readUserDebugParameter(motA_rf_Id)
+  motB_rf = p.readUserDebugParameter(motB_rf_Id)
+  motC_rf = p.readUserDebugParameter(motC_rf_Id)
+  erp_rf = p.readUserDebugParameter(erp_rf_Id)
+  relPosTarget_rf = p.readUserDebugParameter(relPosTarget_rf_Id)
+  #motC_rf
+  p.setJointMotorControl2(vision,
+                          motor_front_rightR_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motA_rf,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_front_rightL_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motB_rf,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_front_rightS_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motC_rf,
+                          force=maxMotorForce)
+  p.changeConstraint(c_rf,
+                     gearRatio=-1,
+                     gearAuxLink=motor_front_rightR_joint,
+                     erp=erp_rf,
+                     relativePositionTarget=relPosTarget_rf,
+                     maxForce=maxGearForce)
 
-	motA_lf = p.readUserDebugParameter(motA_lf_Id)
-	motB_lf = p.readUserDebugParameter(motB_lf_Id)
-	motC_lf = p.readUserDebugParameter(motC_lf_Id)
-	erp_lf = p.readUserDebugParameter(erp_lf_Id)
-	relPosTarget_lf = p.readUserDebugParameter(relPosTarget_lf_Id)
-	p.setJointMotorControl2(vision,motor_front_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lf,force=maxMotorForce)
-	p.setJointMotorControl2(vision,motor_front_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lf,force=maxMotorForce)
-	p.setJointMotorControl2(vision,motor_front_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lf,force=maxMotorForce)
-	p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,erp=erp_lf, relativePositionTarget=relPosTarget_lf,maxForce=maxGearForce)
+  motA_lf = p.readUserDebugParameter(motA_lf_Id)
+  motB_lf = p.readUserDebugParameter(motB_lf_Id)
+  motC_lf = p.readUserDebugParameter(motC_lf_Id)
+  erp_lf = p.readUserDebugParameter(erp_lf_Id)
+  relPosTarget_lf = p.readUserDebugParameter(relPosTarget_lf_Id)
+  p.setJointMotorControl2(vision,
+                          motor_front_leftL_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motA_lf,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_front_leftR_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motB_lf,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_front_leftS_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motC_lf,
+                          force=maxMotorForce)
+  p.changeConstraint(c_lf,
+                     gearRatio=-1,
+                     gearAuxLink=motor_front_leftL_joint,
+                     erp=erp_lf,
+                     relativePositionTarget=relPosTarget_lf,
+                     maxForce=maxGearForce)
 
+  motA_rb = p.readUserDebugParameter(motA_rb_Id)
+  motB_rb = p.readUserDebugParameter(motB_rb_Id)
+  motC_rb = p.readUserDebugParameter(motC_rb_Id)
+  erp_rb = p.readUserDebugParameter(erp_rb_Id)
+  relPosTarget_rb = p.readUserDebugParameter(relPosTarget_rb_Id)
+  p.setJointMotorControl2(vision,
+                          motor_back_rightR_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motA_rb,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_back_rightL_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motB_rb,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_back_rightS_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motC_rb,
+                          force=maxMotorForce)
+  p.changeConstraint(c_rb,
+                     gearRatio=-1,
+                     gearAuxLink=motor_back_rightR_joint,
+                     erp=erp_rb,
+                     relativePositionTarget=relPosTarget_rb,
+                     maxForce=maxGearForce)
 
-	motA_rb = p.readUserDebugParameter(motA_rb_Id)
-	motB_rb = p.readUserDebugParameter(motB_rb_Id)
-	motC_rb = p.readUserDebugParameter(motC_rb_Id)
-	erp_rb = p.readUserDebugParameter(erp_rb_Id)
-	relPosTarget_rb = p.readUserDebugParameter(relPosTarget_rb_Id)
-	p.setJointMotorControl2(vision,motor_back_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rb,force=maxMotorForce)
-	p.setJointMotorControl2(vision,motor_back_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rb,force=maxMotorForce)
-	p.setJointMotorControl2(vision,motor_back_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rb,force=maxMotorForce)
-	p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,erp=erp_rb, relativePositionTarget=relPosTarget_rb,maxForce=maxGearForce)
+  motA_lb = p.readUserDebugParameter(motA_lb_Id)
+  motB_lb = p.readUserDebugParameter(motB_lb_Id)
+  motC_lb = p.readUserDebugParameter(motC_lb_Id)
+  erp_lb = p.readUserDebugParameter(erp_lb_Id)
+  relPosTarget_lb = p.readUserDebugParameter(relPosTarget_lb_Id)
+  p.setJointMotorControl2(vision,
+                          motor_back_leftL_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motA_lb,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_back_leftR_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motB_lb,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_back_leftS_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motC_lb,
+                          force=maxMotorForce)
+  p.changeConstraint(c_lb,
+                     gearRatio=-1,
+                     gearAuxLink=motor_back_leftL_joint,
+                     erp=erp_lb,
+                     relativePositionTarget=relPosTarget_lb,
+                     maxForce=maxGearForce)
 
-	motA_lb = p.readUserDebugParameter(motA_lb_Id)
-	motB_lb = p.readUserDebugParameter(motB_lb_Id)
-	motC_lb = p.readUserDebugParameter(motC_lb_Id)
-	erp_lb = p.readUserDebugParameter(erp_lb_Id)
-	relPosTarget_lb = p.readUserDebugParameter(relPosTarget_lb_Id)
-	p.setJointMotorControl2(vision,motor_back_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lb,force=maxMotorForce)
-	p.setJointMotorControl2(vision,motor_back_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lb,force=maxMotorForce)
-	p.setJointMotorControl2(vision,motor_back_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lb,force=maxMotorForce)
-	p.changeConstraint(c_lb,gearRatio=-1, gearAuxLink = motor_back_leftL_joint,erp=erp_lb, relativePositionTarget=relPosTarget_lb,maxForce=maxGearForce)
-
-
-
-	p.setGravity(0,0,-10)
-	time.sleep(1./240.)
+  p.setGravity(0, 0, -10)
+  time.sleep(1. / 240.)
 t = 0
 prevTime = time.time()
 while (1):
-        timeScale = p.readUserDebugParameter(timeScaleId)
-        amplitude = p.readUserDebugParameter(amplitudeId)
-        newTime = time.time()
-        dt = (newTime-prevTime)*timeScale
-        t = t+dt
-        prevTime = newTime
+  timeScale = p.readUserDebugParameter(timeScaleId)
+  amplitude = p.readUserDebugParameter(amplitudeId)
+  newTime = time.time()
+  dt = (newTime - prevTime) * timeScale
+  t = t + dt
+  prevTime = newTime
 
-        amp=amplitude
-        motA_rf = math.sin(t)*amp+legpos
-        motA_rb = math.sin(t)*amp+legpos
-        motA_lf = -(math.sin(t)*amp+legpos)
-        motA_lb = -(math.sin(t)*amp+legpos)
+  amp = amplitude
+  motA_rf = math.sin(t) * amp + legpos
+  motA_rb = math.sin(t) * amp + legpos
+  motA_lf = -(math.sin(t) * amp + legpos)
+  motA_lb = -(math.sin(t) * amp + legpos)
 
-        motB_rf = math.sin(t)*amp+legpos
-        motB_rb = math.sin(t)*amp+legpos
-        motB_lf = -(math.sin(t)*amp+legpos)
-        motB_lb = -(math.sin(t)*amp+legpos)
-	
-        p.setJointMotorControl2(vision,motor_front_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rf,force=maxMotorForce)
-        p.setJointMotorControl2(vision,motor_front_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rf,force=maxMotorForce)
-        p.setJointMotorControl2(vision,motor_front_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rf,force=maxMotorForce)
-      
+  motB_rf = math.sin(t) * amp + legpos
+  motB_rb = math.sin(t) * amp + legpos
+  motB_lf = -(math.sin(t) * amp + legpos)
+  motB_lb = -(math.sin(t) * amp + legpos)
 
-        p.setJointMotorControl2(vision,motor_front_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lf,force=maxMotorForce)
-        p.setJointMotorControl2(vision,motor_front_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lf,force=maxMotorForce)
-        p.setJointMotorControl2(vision,motor_front_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lf,force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_front_rightR_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motA_rf,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_front_rightL_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motB_rf,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_front_rightS_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motC_rf,
+                          force=maxMotorForce)
 
+  p.setJointMotorControl2(vision,
+                          motor_front_leftL_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motA_lf,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_front_leftR_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motB_lf,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_front_leftS_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motC_lf,
+                          force=maxMotorForce)
 
-        p.setJointMotorControl2(vision,motor_back_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rb,force=maxMotorForce)
-        p.setJointMotorControl2(vision,motor_back_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rb,force=maxMotorForce)
-        p.setJointMotorControl2(vision,motor_back_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rb,force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_back_rightR_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motA_rb,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_back_rightL_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motB_rb,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_back_rightS_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motC_rb,
+                          force=maxMotorForce)
 
+  p.setJointMotorControl2(vision,
+                          motor_back_leftL_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motA_lb,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_back_leftR_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motB_lb,
+                          force=maxMotorForce)
+  p.setJointMotorControl2(vision,
+                          motor_back_leftS_joint,
+                          p.POSITION_CONTROL,
+                          targetPosition=motC_lb,
+                          force=maxMotorForce)
 
-        p.setJointMotorControl2(vision,motor_back_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lb,force=maxMotorForce)
-        p.setJointMotorControl2(vision,motor_back_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lb,force=maxMotorForce)
-        p.setJointMotorControl2(vision,motor_back_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lb,force=maxMotorForce)
-
-        p.setGravity(0,0,-10)
-        time.sleep(1./240.)
-
+  p.setGravity(0, 0, -10)
+  time.sleep(1. / 240.)

data/terrain.py

@@ -2,58 +2,57 @@ import math
 
 NUM_VERTS_X = 30
 NUM_VERTS_Y = 30
-totalVerts = NUM_VERTS_X*NUM_VERTS_Y
-totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1)
+totalVerts = NUM_VERTS_X * NUM_VERTS_Y
+totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1)
 offset = -50.0
 TRIANGLE_SIZE = 1.
-waveheight=0.1
-gGroundVertices = [None] * totalVerts*3
-gGroundIndices = [None] * totalTriangles*3
+waveheight = 0.1
+gGroundVertices = [None] * totalVerts * 3
+gGroundIndices = [None] * totalTriangles * 3
 
-i=0
+i = 0
 
-for i in range (NUM_VERTS_X):		
-	for j in range (NUM_VERTS_Y):
-		gGroundVertices[(i+j*NUM_VERTS_X)*3+0] = (i-NUM_VERTS_X*0.5)*TRIANGLE_SIZE
-		gGroundVertices[(i+j*NUM_VERTS_X)*3+1] = (j-NUM_VERTS_Y*0.5)*TRIANGLE_SIZE
-		gGroundVertices[(i+j*NUM_VERTS_X)*3+2] = waveheight*math.sin(float(i))*math.cos(float(j)+offset)
-		                         
-index=0
-for i in range (NUM_VERTS_X-1):
-	for j in range (NUM_VERTS_Y-1):
-		gGroundIndices[index] = 1+j*NUM_VERTS_X+i
-		index+=1
-		gGroundIndices[index] = 1+j*NUM_VERTS_X+i+1
-		index+=1
-		gGroundIndices[index] = 1+(j+1)*NUM_VERTS_X+i+1
-		index+=1
-		gGroundIndices[index] = 1+j*NUM_VERTS_X+i
-		index+=1
-		gGroundIndices[index] = 1+(j+1)*NUM_VERTS_X+i+1
-		index+=1
-		gGroundIndices[index] = 1+(j+1)*NUM_VERTS_X+i
-		index+=1
+for i in range(NUM_VERTS_X):
+  for j in range(NUM_VERTS_Y):
+    gGroundVertices[(i + j * NUM_VERTS_X) * 3 + 0] = (i - NUM_VERTS_X * 0.5) * TRIANGLE_SIZE
+    gGroundVertices[(i + j * NUM_VERTS_X) * 3 + 1] = (j - NUM_VERTS_Y * 0.5) * TRIANGLE_SIZE
+    gGroundVertices[(i + j * NUM_VERTS_X) * 3 +
+                    2] = waveheight * math.sin(float(i)) * math.cos(float(j) + offset)
+
+index = 0
+for i in range(NUM_VERTS_X - 1):
+  for j in range(NUM_VERTS_Y - 1):
+    gGroundIndices[index] = 1 + j * NUM_VERTS_X + i
+    index += 1
+    gGroundIndices[index] = 1 + j * NUM_VERTS_X + i + 1
+    index += 1
+    gGroundIndices[index] = 1 + (j + 1) * NUM_VERTS_X + i + 1
+    index += 1
+    gGroundIndices[index] = 1 + j * NUM_VERTS_X + i
+    index += 1
+    gGroundIndices[index] = 1 + (j + 1) * NUM_VERTS_X + i + 1
+    index += 1
+    gGroundIndices[index] = 1 + (j + 1) * NUM_VERTS_X + i
+    index += 1
 
 #print(gGroundVertices)
 #print(gGroundIndices)
 
 print("o Terrain")
 
-for i in range (totalVerts):
-	print("v "),
-	print(gGroundVertices[i*3+0]),
-	print(" "),
-	print(gGroundVertices[i*3+1]),
-	print(" "),
-	print(gGroundVertices[i*3+2])
-		
-for i in range (totalTriangles):
-	print("f "),
-	print(gGroundIndices[i*3+0]),
-	print(" "),
-	print(gGroundIndices[i*3+1]),
-	print(" "),
-	print(gGroundIndices[i*3+2]),
-	print(" ")
-	
+for i in range(totalVerts):
+  print("v "),
+  print(gGroundVertices[i * 3 + 0]),
+  print(" "),
+  print(gGroundVertices[i * 3 + 1]),
+  print(" "),
+  print(gGroundVertices[i * 3 + 2])
 
+for i in range(totalTriangles):
+  print("f "),
+  print(gGroundIndices[i * 3 + 0]),
+  print(" "),
+  print(gGroundIndices[i * 3 + 1]),
+  print(" "),
+  print(gGroundIndices[i * 3 + 2]),
+  print(" ")

data/threecubes/threecubes.py

@@ -3,6 +3,5 @@ p.connect(p.DIRECT)
 p.loadPlugin("eglRendererPlugin")
 p.loadSDF("newsdf.sdf")
 while (1):
-	p.getCameraImage(320,240, flags=p.ER_NO_SEGMENTATION_MASK)
-	p.stepSimulation()
-	
+  p.getCameraImage(320, 240, flags=p.ER_NO_SEGMENTATION_MASK)
+  p.stepSimulation()

data/threecubes/threecubes_org.py

@@ -3,20 +3,30 @@ useEGL = False
 useEGLGUI = False
 
 if useEGL:
-	if useEGLGUI:
-		p.connect(p.GUI, "window_backend=2")
-	else:	
-		p.connect(p.DIRECT)
-		p.loadPlugin("eglRendererPlugin")
+  if useEGLGUI:
+    p.connect(p.GUI, "window_backend=2")
+  else:
+    p.connect(p.DIRECT)
+    p.loadPlugin("eglRendererPlugin")
 else:
-	p.connect(p.GUI)
+  p.connect(p.GUI)
 
 p.loadURDF("threecubes.urdf", flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
 while (1):
-	
-	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]
 
-	p.getCameraImage(64,64, viewMatrix=viewmat, projectionMatrix=projmat, flags=p.ER_NO_SEGMENTATION_MASK )
-	p.stepSimulation()
-	
+  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
+  ]
+
+  p.getCameraImage(64,
+                   64,
+                   viewMatrix=viewmat,
+                   projectionMatrix=projmat,
+                   flags=p.ER_NO_SEGMENTATION_MASK)
+  p.stepSimulation()

docs/pybullet_quickstart_guide/PlainPreview/BuildMarkdeepUtility.py

@@ -1,43 +1,44 @@
 import re
 
-if(__name__=="__main__"):
-    # Assemble the script which embeds the Markdeep page into the preview blog
-    PreviewBlogPage=open("PreviewBlogPage.htm","rb").read().decode("utf-8");
-    HeadMatch=re.search("<head(.*?)>(.*?)</head>",PreviewBlogPage,re.DOTALL);
-    HeadAttributes=HeadMatch.group(1);
-    FullDocumentHead=HeadMatch.group(2);
-    BodyMatch=re.search("<body(.*?)>(.*?)</body>",PreviewBlogPage,re.DOTALL);
-    BodyAttributes=BodyMatch.group(1);
-    FullPreviewBody=BodyMatch.group(2);
-    ArticleHTMLCodeMacro="$(ARTICLE_HTML_CODE)";
-    iArticleHTMLCodeMacro=FullPreviewBody.find(ArticleHTMLCodeMacro);
-    DocumentBodyPrefix=FullPreviewBody[0:iArticleHTMLCodeMacro];
-    DocumentBodySuffix=FullPreviewBody[iArticleHTMLCodeMacro+len(ArticleHTMLCodeMacro):];
-    FullPrepareHTMLCode=open("PrepareHTML.js","rb").read().decode("utf-8");
-    ReplacementList=[
-        ("$(FULL_DOCUMENT_HEAD)",FullDocumentHead),
-        ("$(DOCUMENT_BODY_PREFIX)",DocumentBodyPrefix),
-        ("$(DOCUMENT_BODY_SUFFIX)",DocumentBodySuffix)
-    ];
-    for Macro,Replacement in ReplacementList:
-        FullPrepareHTMLCode=FullPrepareHTMLCode.replace(Macro,Replacement.replace("\r\n","\\r\\n\\\r\n").replace("'","\\'"));
-    # Generate code which sets body and head attributes appropriately
-    for Element,AttributeCode in [("head",HeadAttributes),("body",BodyAttributes)]:
-        FullPrepareHTMLCode+="\r\n// Setting "+Element+" attributes\r\n";
-        for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"",AttributeCode):
-            FullPrepareHTMLCode+="document."+Element+".setAttribute(\""+Match.group(1)+"\",\""+Match.group(2)+"\");\r\n";
-    open("PrepareHTML.full.js","wb").write(FullPrepareHTMLCode.encode("utf-8"));
+if (__name__ == "__main__"):
+  # Assemble the script which embeds the Markdeep page into the preview blog
+  PreviewBlogPage = open("PreviewBlogPage.htm", "rb").read().decode("utf-8")
+  HeadMatch = re.search("<head(.*?)>(.*?)</head>", PreviewBlogPage, re.DOTALL)
+  HeadAttributes = HeadMatch.group(1)
+  FullDocumentHead = HeadMatch.group(2)
+  BodyMatch = re.search("<body(.*?)>(.*?)</body>", PreviewBlogPage, re.DOTALL)
+  BodyAttributes = BodyMatch.group(1)
+  FullPreviewBody = BodyMatch.group(2)
+  ArticleHTMLCodeMacro = "$(ARTICLE_HTML_CODE)"
+  iArticleHTMLCodeMacro = FullPreviewBody.find(ArticleHTMLCodeMacro)
+  DocumentBodyPrefix = FullPreviewBody[0:iArticleHTMLCodeMacro]
+  DocumentBodySuffix = FullPreviewBody[iArticleHTMLCodeMacro + len(ArticleHTMLCodeMacro):]
+  FullPrepareHTMLCode = open("PrepareHTML.js", "rb").read().decode("utf-8")
+  ReplacementList = [("$(FULL_DOCUMENT_HEAD)", FullDocumentHead),
+                     ("$(DOCUMENT_BODY_PREFIX)", DocumentBodyPrefix),
+                     ("$(DOCUMENT_BODY_SUFFIX)", DocumentBodySuffix)]
+  for Macro, Replacement in ReplacementList:
+    FullPrepareHTMLCode = FullPrepareHTMLCode.replace(
+        Macro,
+        Replacement.replace("\r\n", "\\r\\n\\\r\n").replace("'", "\\'"))
+  # Generate code which sets body and head attributes appropriately
+  for Element, AttributeCode in [("head", HeadAttributes), ("body", BodyAttributes)]:
+    FullPrepareHTMLCode += "\r\n// Setting " + Element + " attributes\r\n"
+    for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"", AttributeCode):
+      FullPrepareHTMLCode += "document." + Element + ".setAttribute(\"" + Match.group(
+          1) + "\",\"" + Match.group(2) + "\");\r\n"
+  open("PrepareHTML.full.js", "wb").write(FullPrepareHTMLCode.encode("utf-8"))
 
-    # Concatenate all the scripts together
-    SourceFileList=[
-                    "PrepareHTML.full.js",
-                    "SetMarkdeepMode.js",
-                    "markdeep.min.js",
-                    "DisplayMarkdeepOutput.js",
-                    "InvokeMathJax.js"
-    ];
-    OutputCode="\r\n\r\n".join(["// "+SourceFile+"\r\n\r\n"+open(SourceFile,"rb").read().decode("utf-8") for SourceFile in SourceFileList]);
-    OutputFile=open("MarkdeepUtility.js","wb");
-    OutputFile.write(OutputCode.encode("utf-8"));
-    OutputFile.close();
-    print("Done.");
+  # Concatenate all the scripts together
+  SourceFileList = [
+      "PrepareHTML.full.js", "SetMarkdeepMode.js", "markdeep.min.js", "DisplayMarkdeepOutput.js",
+      "InvokeMathJax.js"
+  ]
+  OutputCode = "\r\n\r\n".join([
+      "// " + SourceFile + "\r\n\r\n" + open(SourceFile, "rb").read().decode("utf-8")
+      for SourceFile in SourceFileList
+  ])
+  OutputFile = open("MarkdeepUtility.js", "wb")
+  OutputFile.write(OutputCode.encode("utf-8"))
+  OutputFile.close()
+  print("Done.")

docs/pybullet_quickstart_guide/WordpressPreview/BuildMarkdeepUtility.py

@@ -1,43 +1,44 @@
 import re
 
-if(__name__=="__main__"):
-    # Assemble the script which embeds the Markdeep page into the preview blog
-    PreviewBlogPage=open("PreviewBlogPage.htm","rb").read().decode("utf-8");
-    HeadMatch=re.search("<head(.*?)>(.*?)</head>",PreviewBlogPage,re.DOTALL);
-    HeadAttributes=HeadMatch.group(1);
-    FullDocumentHead=HeadMatch.group(2);
-    BodyMatch=re.search("<body(.*?)>(.*?)</body>",PreviewBlogPage,re.DOTALL);
-    BodyAttributes=BodyMatch.group(1);
-    FullPreviewBody=BodyMatch.group(2);
-    ArticleHTMLCodeMacro="$(ARTICLE_HTML_CODE)";
-    iArticleHTMLCodeMacro=FullPreviewBody.find(ArticleHTMLCodeMacro);
-    DocumentBodyPrefix=FullPreviewBody[0:iArticleHTMLCodeMacro];
-    DocumentBodySuffix=FullPreviewBody[iArticleHTMLCodeMacro+len(ArticleHTMLCodeMacro):];
-    FullPrepareHTMLCode=open("PrepareHTML.js","rb").read().decode("utf-8");
-    ReplacementList=[
-        ("$(FULL_DOCUMENT_HEAD)",FullDocumentHead),
-        ("$(DOCUMENT_BODY_PREFIX)",DocumentBodyPrefix),
-        ("$(DOCUMENT_BODY_SUFFIX)",DocumentBodySuffix)
-    ];
-    for Macro,Replacement in ReplacementList:
-        FullPrepareHTMLCode=FullPrepareHTMLCode.replace(Macro,Replacement.replace("\r\n","\\r\\n\\\r\n").replace("'","\\'"));
-    # Generate code which sets body and head attributes appropriately
-    for Element,AttributeCode in [("head",HeadAttributes),("body",BodyAttributes)]:
-        FullPrepareHTMLCode+="\r\n// Setting "+Element+" attributes\r\n";
-        for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"",AttributeCode):
-            FullPrepareHTMLCode+="document."+Element+".setAttribute(\""+Match.group(1)+"\",\""+Match.group(2)+"\");\r\n";
-    open("PrepareHTML.full.js","wb").write(FullPrepareHTMLCode.encode("utf-8"));
+if (__name__ == "__main__"):
+  # Assemble the script which embeds the Markdeep page into the preview blog
+  PreviewBlogPage = open("PreviewBlogPage.htm", "rb").read().decode("utf-8")
+  HeadMatch = re.search("<head(.*?)>(.*?)</head>", PreviewBlogPage, re.DOTALL)
+  HeadAttributes = HeadMatch.group(1)
+  FullDocumentHead = HeadMatch.group(2)
+  BodyMatch = re.search("<body(.*?)>(.*?)</body>", PreviewBlogPage, re.DOTALL)
+  BodyAttributes = BodyMatch.group(1)
+  FullPreviewBody = BodyMatch.group(2)
+  ArticleHTMLCodeMacro = "$(ARTICLE_HTML_CODE)"
+  iArticleHTMLCodeMacro = FullPreviewBody.find(ArticleHTMLCodeMacro)
+  DocumentBodyPrefix = FullPreviewBody[0:iArticleHTMLCodeMacro]
+  DocumentBodySuffix = FullPreviewBody[iArticleHTMLCodeMacro + len(ArticleHTMLCodeMacro):]
+  FullPrepareHTMLCode = open("PrepareHTML.js", "rb").read().decode("utf-8")
+  ReplacementList = [("$(FULL_DOCUMENT_HEAD)", FullDocumentHead),
+                     ("$(DOCUMENT_BODY_PREFIX)", DocumentBodyPrefix),
+                     ("$(DOCUMENT_BODY_SUFFIX)", DocumentBodySuffix)]
+  for Macro, Replacement in ReplacementList:
+    FullPrepareHTMLCode = FullPrepareHTMLCode.replace(
+        Macro,
+        Replacement.replace("\r\n", "\\r\\n\\\r\n").replace("'", "\\'"))
+  # Generate code which sets body and head attributes appropriately
+  for Element, AttributeCode in [("head", HeadAttributes), ("body", BodyAttributes)]:
+    FullPrepareHTMLCode += "\r\n// Setting " + Element + " attributes\r\n"
+    for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"", AttributeCode):
+      FullPrepareHTMLCode += "document." + Element + ".setAttribute(\"" + Match.group(
+          1) + "\",\"" + Match.group(2) + "\");\r\n"
+  open("PrepareHTML.full.js", "wb").write(FullPrepareHTMLCode.encode("utf-8"))
 
-    # Concatenate all the scripts together
-    SourceFileList=[
-                    "PrepareHTML.full.js",
-                    "SetMarkdeepMode.js",
-                    "markdeep.min.js",
-                    "DisplayMarkdeepOutput.js",
-                    "InvokeMathJax.js"
-    ];
-    OutputCode="\r\n\r\n".join(["// "+SourceFile+"\r\n\r\n"+open(SourceFile,"rb").read().decode("utf-8") for SourceFile in SourceFileList]);
-    OutputFile=open("MarkdeepUtility.js","wb");
-    OutputFile.write(OutputCode.encode("utf-8"));
-    OutputFile.close();
-    print("Done.");
+  # Concatenate all the scripts together
+  SourceFileList = [
+      "PrepareHTML.full.js", "SetMarkdeepMode.js", "markdeep.min.js", "DisplayMarkdeepOutput.js",
+      "InvokeMathJax.js"
+  ]
+  OutputCode = "\r\n\r\n".join([
+      "// " + SourceFile + "\r\n\r\n" + open(SourceFile, "rb").read().decode("utf-8")
+      for SourceFile in SourceFileList
+  ])
+  OutputFile = open("MarkdeepUtility.js", "wb")
+  OutputFile.write(OutputCode.encode("utf-8"))
+  OutputFile.close()
+  print("Done.")

examples/SharedMemory/grpc/pybullet_client.py

@@ -10,69 +10,86 @@ import pybullet_pb2_grpc
 #todo: how to add this?
 MJCF_COLORS_FROM_FILE = 512
 
+
 def run():
-	print("grpc.insecure_channel")
-	channel = grpc.insecure_channel('localhost:6667')
-	print("pybullet_pb2_grpc.PyBulletAPIStub")
-	stub = pybullet_pb2_grpc.PyBulletAPIStub(channel)
-	response=0
+  print("grpc.insecure_channel")
+  channel = grpc.insecure_channel('localhost:6667')
+  print("pybullet_pb2_grpc.PyBulletAPIStub")
+  stub = pybullet_pb2_grpc.PyBulletAPIStub(channel)
+  response = 0
 
-	print("submit CheckVersionCommand")
-	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(checkVersionCommand=pybullet_pb2.CheckVersionCommand(clientVersion=123)))
-	print("PyBullet client received: " , response)
+  print("submit CheckVersionCommand")
+  response = stub.SubmitCommand(
+      pybullet_pb2.PyBulletCommand(checkVersionCommand=pybullet_pb2.CheckVersionCommand(
+          clientVersion=123)))
+  print("PyBullet client received: ", response)
+
+  print("submit_ResetSimulationCommand")
+  response = stub.SubmitCommand(
+      pybullet_pb2.PyBulletCommand(resetSimulationCommand=pybullet_pb2.ResetSimulationCommand()))
+  print("PyBullet client received: ", response)
+
+  print("submit LoadUrdfCommand ")
+  response = stub.SubmitCommand(
+      pybullet_pb2.PyBulletCommand(loadUrdfCommand=pybullet_pb2.LoadUrdfCommand(
+          fileName="door.urdf",
+          initialPosition=pybullet_pb2.vec3(x=0, y=0, z=0),
+          useMultiBody=True,
+          useFixedBase=True,
+          globalScaling=2,
+          flags=1)))
+  print("PyBullet client received: ", response)
+  bodyUniqueId = response.urdfStatus.bodyUniqueId
+
+  print("submit LoadSdfCommand")
+  response = stub.SubmitCommand(
+      pybullet_pb2.PyBulletCommand(loadSdfCommand=pybullet_pb2.LoadSdfCommand(
+          fileName="two_cubes.sdf", useMultiBody=True, globalScaling=2)))
+  print("PyBullet client received: ", response)
+
+  print("submit LoadMjcfCommand")
+  response = stub.SubmitCommand(
+      pybullet_pb2.PyBulletCommand(loadMjcfCommand=pybullet_pb2.LoadMjcfCommand(
+          fileName="mjcf/humanoid.xml", flags=MJCF_COLORS_FROM_FILE)))
+  print("PyBullet client received: ", response)
+
+  print("submit ChangeDynamicsCommand ")
+  response = stub.SubmitCommand(
+      pybullet_pb2.PyBulletCommand(changeDynamicsCommand=pybullet_pb2.ChangeDynamicsCommand(
+          bodyUniqueId=bodyUniqueId, linkIndex=-1, mass=10)))
+  print("PyBullet client received: ", response)
+
+  print("submit GetDynamicsCommand ")
+  response = stub.SubmitCommand(
+      pybullet_pb2.PyBulletCommand(getDynamicsCommand=pybullet_pb2.GetDynamicsCommand(
+          bodyUniqueId=bodyUniqueId, linkIndex=-1)))
+  print("PyBullet client received: ", response)
+
+  print("submit InitPoseCommand")
+  response = stub.SubmitCommand(
+      pybullet_pb2.PyBulletCommand(initPoseCommand=pybullet_pb2.InitPoseCommand(
+          bodyUniqueId=bodyUniqueId, initialStateQ=[1, 2, 3], hasInitialStateQ=[1, 1, 1])))
+  print("PyBullet client received: ", response)
+
+  print("submit RequestActualStateCommand")
+  response = stub.SubmitCommand(
+      pybullet_pb2.
+      PyBulletCommand(requestActualStateCommand=pybullet_pb2.RequestActualStateCommand(
+          bodyUniqueId=bodyUniqueId, computeForwardKinematics=True, computeLinkVelocities=True)))
+  print("PyBullet client received: ", response)
+
+  i = 0
+  while (True):
+    i = i + 1
+    print("submit StepSimulationCommand: ", i)
+    response = stub.SubmitCommand(
+        pybullet_pb2.PyBulletCommand(stepSimulationCommand=pybullet_pb2.StepSimulationCommand()))
+    print("PyBullet client received: ", response.statusType)
 
 
-        print("submit_ResetSimulationCommand")
-        response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(resetSimulationCommand=pybullet_pb2.ResetSimulationCommand()))
-        print("PyBullet client received: ", response)
-
-
-	print("submit LoadUrdfCommand ")
-	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadUrdfCommand=pybullet_pb2.LoadUrdfCommand(fileName="door.urdf", initialPosition=pybullet_pb2.vec3(x=0,y=0,z=0), useMultiBody=True, useFixedBase=True, globalScaling=2, flags = 1)))
-	print("PyBullet client received: " , response)
-	bodyUniqueId = response.urdfStatus.bodyUniqueId
-	
-	
-
-	print("submit LoadSdfCommand")
-	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadSdfCommand=pybullet_pb2.LoadSdfCommand(fileName="two_cubes.sdf", useMultiBody=True, globalScaling=2)))
-	print("PyBullet client received: " , response)
-	
-
-	print("submit LoadMjcfCommand")
-	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadMjcfCommand=pybullet_pb2.LoadMjcfCommand(fileName="mjcf/humanoid.xml",flags=MJCF_COLORS_FROM_FILE)))
-	print("PyBullet client received: " , response)
-
-	
-	
-	print("submit ChangeDynamicsCommand ")
-	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(changeDynamicsCommand=pybullet_pb2.ChangeDynamicsCommand(bodyUniqueId=bodyUniqueId, linkIndex=-1, mass=10)))
-	print("PyBullet client received: " , response)
-	
-	print("submit GetDynamicsCommand ")
-	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(getDynamicsCommand=pybullet_pb2.GetDynamicsCommand(bodyUniqueId=bodyUniqueId, linkIndex=-1)))
-	print("PyBullet client received: " , response)
-
-	print("submit InitPoseCommand")
-	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(initPoseCommand=pybullet_pb2.InitPoseCommand(bodyUniqueId=bodyUniqueId, initialStateQ=[1,2,3],hasInitialStateQ=[1,1,1])))
-	print("PyBullet client received: " , response)
-	
-	print("submit RequestActualStateCommand")
-	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(requestActualStateCommand=pybullet_pb2.RequestActualStateCommand(bodyUniqueId=bodyUniqueId, computeForwardKinematics=True, computeLinkVelocities=True )))
-	print("PyBullet client received: " , response)
-	
-	
-        i=0
-        while(True):
-                        i=i+1
-			print("submit StepSimulationCommand: ", i)
-			response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(stepSimulationCommand=pybullet_pb2.StepSimulationCommand()))
-			print("PyBullet client received: " , response.statusType)
-  
-  #print("TerminateServerCommand")
-	#response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(terminateServerCommand=pybullet_pb2.TerminateServerCommand()))
-	#print("PyBullet client received: " , response.statusType)
-
+#print("TerminateServerCommand")
+#response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(terminateServerCommand=pybullet_pb2.TerminateServerCommand()))
+#print("PyBullet client received: " , response.statusType)
 
 if __name__ == '__main__':
-	run()
+  run()

examples/pybullet/examples/addPlanarReflection.py

@@ -2,74 +2,100 @@ import pybullet as p
 import time
 import math
 
-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
+
+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)
+if (cid < 0):
+  p.connect(p.GUI)
 p.setPhysicsEngineParameter(numSolverIterations=10)
-p.setTimeStep(1./120.)
+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_RENDERING, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION, 1)
 
-p.configureDebugVisualizer(p.COV_ENABLE_GUI,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)
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
 
-shift = [0,-0.02,0]
-meshScale=[0.1,0.1,0.1]
+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)
+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)
+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.setGravity(0, 0, -10)
 p.setRealTimeSimulation(1)
 
-colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]]
+colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
 currentColor = 0
 
 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>=1):
-					#p.removeBody(objectUid)
-					p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor])
-					currentColor+=1
-					if (currentColor>=len(colors)):
-						currentColor=0
+  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

examples/pybullet/examples/batchRayCast.py

@@ -5,73 +5,70 @@ import math
 useGui = True
 
 if (useGui):
-	p.connect(p.GUI)
+  p.connect(p.GUI)
 else:
-	p.connect(p.DIRECT)
+  p.connect(p.DIRECT)
 
-p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
+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])
+p.loadURDF("r2d2.urdf", [3, 3, 1])
 
-
-rayFrom=[]
-rayTo=[]
-rayIds=[]
+rayFrom = []
+rayTo = []
+rayIds = []
 
 numRays = 1024
-	
+
 rayLen = 13
 
-
-rayHitColor = [1,0,0]
-rayMissColor = [0,1,0]
+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)
+
+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")
+  timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "rayCastBench.json")
 
 numSteps = 10
 if (useGui):
-	numSteps = 327680
+  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.)
 
-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)
+  p.stopStateLogging(timingLog)

examples/pybullet/examples/biped2d_pybullet.py

@@ -4,28 +4,26 @@ import os
 import time
 GRAVITY = -9.8
 dt = 1e-3
-iters=2000
+iters = 2000
 
 physicsClient = p.connect(p.GUI)
 p.setAdditionalSearchPath(pybullet_data.getDataPath())
 p.resetSimulation()
 #p.setRealTimeSimulation(True)
-p.setGravity(0,0,GRAVITY)
+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)
+cubeStartPos = [0, 0, 1.13]
+cubeStartOrientation = p.getQuaternionFromEuler([0., 0, 0])
+botId = p.loadURDF("biped/biped2d_pybullet.urdf", cubeStartPos, cubeStartOrientation)
 
-#for i in range(10000):     
+#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
@@ -33,8 +31,8 @@ for joint in range (p.getNumJoints(botId)):
 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.)
+  #p.stepSimulation()
+  #p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
+  p.setGravity(0, 0, GRAVITY)
+  time.sleep(1 / 240.)
 time.sleep(1000)

examples/pybullet/examples/changeDynamicsMass.py

@@ -2,14 +2,14 @@ import pybullet as p
 import time
 
 p.connect(p.GUI)
-cube2 = p.loadURDF("cube.urdf",[0,0,3], useFixedBase=True)
+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.setGravity(0, 0, -10)
+timeStep = 1. / 240.
 p.setTimeStep(timeStep)
-p.changeDynamics(cube2,-1,mass=1)
+p.changeDynamics(cube2, -1, mass=1)
 #now cube2 will have a floating base and move
 
 while (p.isConnected()):
-	p.stepSimulation()
-	time.sleep(timeStep)
+  p.stepSimulation()
+  time.sleep(timeStep)

examples/pybullet/examples/changeTexture.py

@@ -1,43 +1,41 @@
 import pybullet as p
 import time
 p.connect(p.GUI)
-planeUidA = p.loadURDF("plane_transparent.urdf",[0,0,0])
-planeUid = p.loadURDF("plane_transparent.urdf",[0,0,-1])
+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)
+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
+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
+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 duraction")
-	print(end-start)
+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 duraction")
+  print(end - start)
 p.stopStateLogging(logId)
 #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
 #p.configureDebugVisualizer(p.COV_ENABLE_GUI,1)
-

examples/pybullet/examples/collisionFilter.py

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

examples/pybullet/examples/commandLogAndPlayback.py

@@ -2,14 +2,13 @@ import pybullet as p
 import time
 
 p.connect(p.GUI)
-logId = p.startStateLogging(p.STATE_LOGGING_ALL_COMMANDS,"commandLog.bin")
+logId = p.startStateLogging(p.STATE_LOGGING_ALL_COMMANDS, "commandLog.bin")
 p.loadURDF("plane.urdf")
-p.loadURDF("r2d2.urdf",[0,0,1])
+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.)
+p.resetSimulation()
 
+logId = p.startStateLogging(p.STATE_REPLAY_ALL_COMMANDS, "commandLog.bin")
+while (p.isConnected()):
+  time.sleep(1. / 240.)

examples/pybullet/examples/constraint.py

@@ -5,22 +5,22 @@ import math
 p.connect(p.GUI)
 
 p.loadURDF("plane.urdf")
-cubeId = p.loadURDF("cube_small.urdf",0,0,1)
-p.setGravity(0,0,-10)
+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))
-prev=[0,0,1]
-a=-math.pi
+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))
+prev = [0, 0, 1]
+a = -math.pi
 while 1:
-	a=a+0.01
-	if (a>math.pi):
-		a=-math.pi
-	time.sleep(.01)
-	p.setGravity(0,0,-10)
-	pivot=[a,0,1]
-	orn = p.getQuaternionFromEuler([a,0,0])
-	p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50)
+  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)

examples/pybullet/examples/contactFriction.py

@@ -1,34 +1,29 @@
 import pybullet as p
 p.connect(p.GUI)
 useMaximalCoordinates = False
-p.loadURDF("plane.urdf", useMaximalCoordinates=useMaximalCoordinates )
+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]
+p.loadURDF("cube.urdf", [0, 0, 1], useMaximalCoordinates=useMaximalCoordinates)
+p.setGravity(0, 3, -10)
+while (1):
+  p.stepSimulation()
+  pts = p.getContactPoints()
 
-	
-	print("totalNormalForce=",totalNormalForce)
-	print("totalLateralFrictionForce=",totalLateralFrictionForce)
-	
-	
-		
-		
+  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)

examples/pybullet/examples/createMesh.py

@@ -8,114 +8,129 @@ 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)
+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)
+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]
+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
 ]
-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)
-
-
+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])
+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
+for i in range(segmentLength):
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1])
+  segmentStart = segmentStart - 1
 
-baseOrientation = p.getQuaternionFromEuler([math.pi/2.,0,math.pi/2.])
+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
 
-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)
+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])
-	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
+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]]
+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
+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)
+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)
-	
+  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)

examples/pybullet/examples/createMultiBodyBatch.py

@@ -3,145 +3,140 @@ import time
 import math
 
 cid = p.connect(p.SHARED_MEMORY)
-if (cid<0):
-	p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
-	
-	
+if (cid < 0):
+  p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
+
 p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
-p.setTimeStep(1./120.)
+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)
+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)
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
 
-shift = [0,-0.02,0]
-meshScale=[0.1,0.1,0.1]
+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]
+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
 ]
 
-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)
+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 = []
 
-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])
 
-
-
-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])
-
-bodyUid = p.createMultiBody(baseMass=0,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition =[0,0,2], batchPositions=batchPositions,useMaximalCoordinates=True)
-p.changeVisualShape(bodyUid,-1, textureUniqueId = texUid)		
+bodyUid = p.createMultiBody(baseMass=0,
+                            baseInertialFramePosition=[0, 0, 0],
+                            baseCollisionShapeIndex=collisionShapeId,
+                            baseVisualShapeIndex=visualShapeId,
+                            basePosition=[0, 0, 2],
+                            batchPositions=batchPositions,
+                            useMaximalCoordinates=True)
+p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
 
 p.syncBodyInfo()
-print("numBodies=",p.getNumBodies())
+print("numBodies=", p.getNumBodies())
 p.stopStateLogging(logId)
-p.setGravity(0,0,-10)
+p.setGravity(0, 0, -10)
 
-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
 
-colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,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)
-  

examples/pybullet/examples/createMultiBodyLinks.py

@@ -1,56 +1,73 @@
 import pybullet as p
 import time
 
-
 p.connect(p.GUI)
 p.createCollisionShape(p.GEOM_PLANE)
-p.createMultiBody(0,0)
+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])
+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]]
+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)
+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.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)
+for i in range(p.getNumJoints(sphereUid)):
+  p.getJointInfo(sphereUid, i)
 
-	time.sleep(0.01)
-	
+while (1):
+  keys = p.getKeyboardEvents()
+  print(keys)
+
+  time.sleep(0.01)

examples/pybullet/examples/createObstacleCourse.py

@@ -8,93 +8,121 @@ 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)
+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)
+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")
-
-
+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])
+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
+for i in range(segmentLength):
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1])
+  segmentStart = segmentStart - 1
 
-baseOrientation = p.getQuaternionFromEuler([math.pi/2.,0,math.pi/2.])
+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
 
-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)
+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])
-	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
+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]]
+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
+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)
+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)
-	
+  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)

examples/pybullet/examples/createSphereMultiBodies.py

@@ -5,34 +5,45 @@ useMaximalCoordinates = 0
 
 p.connect(p.GUI)
 #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)
+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])
+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)
 
-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.setGravity(0, 0, -10)
 p.setRealTimeSimulation(1)
 
 while (1):
-	keys = p.getKeyboardEvents()
-	#print(keys)
-	time.sleep(0.01)
-	
+  keys = p.getKeyboardEvents()
+  #print(keys)
+  time.sleep(0.01)

examples/pybullet/examples/createTexturedMeshVisualShape.py

@@ -2,167 +2,180 @@ import pybullet as p
 import time
 import math
 
-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
+
+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)
+if (cid < 0):
+  p.connect(p.GUI)
 p.setPhysicsEngineParameter(numSolverIterations=10)
-p.setTimeStep(1./120.)
+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)
+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)
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
 
-shift = [0,-0.02,0]
-meshScale=[0.1,0.1,0.1]
+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]
+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
 ]
 
-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_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)
+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)
+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.setGravity(0, 0, -10)
 p.setRealTimeSimulation(1)
 
-colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,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
+  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

examples/pybullet/examples/createVisualShape.py

@@ -2,72 +2,98 @@ import pybullet as p
 import time
 import math
 
-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 = [rayToCenter[0]  - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0],
-			rayToCenter[1]  - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1],
-				rayToCenter[2]  - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]]
-	return rayFrom,rayTo
+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 = [
+      rayToCenter[0] - 0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] -
+      float(mouseY) * dVer[0], rayToCenter[1] - 0.5 * horizon[1] + 0.5 * vertical[1] +
+      float(mouseX) * dHor[1] - float(mouseY) * dVer[1], rayToCenter[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)
+if (cid < 0):
+  p.connect(p.GUI)
 p.setPhysicsEngineParameter(numSolverIterations=10)
-p.setTimeStep(1./120.)
+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)
+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)
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
 
-shift = [0,-0.02,0]
-meshScale=[0.1,0.1,0.1]
+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)
+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 = 5
 rangey = 5
-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)
+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.setGravity(0, 0, -10)
 p.setRealTimeSimulation(1)
 
-colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]]
+colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
 currentColor = 0
 
 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>=1):
-					p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor])
-					currentColor+=1
-					if (currentColor>=len(colors)):
-						currentColor=0
+  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.changeVisualShape(objectUid, -1, rgbaColor=colors[currentColor])
+          currentColor += 1
+          if (currentColor >= len(colors)):
+            currentColor = 0

examples/pybullet/examples/createVisualShapeArray.py

@@ -2,80 +2,111 @@ import pybullet as p
 import time
 import math
 
-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
+
+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)
+if (cid < 0):
+  p.connect(p.GUI)
 p.setPhysicsEngineParameter(numSolverIterations=10)
-p.setTimeStep(1./120.)
+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)
+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)
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
 
-shift = [0,-0.02,0]
-shift1 = [0,0.1,0]
-shift2 = [0,0,0]
+shift = [0, -0.02, 0]
+shift1 = [0, 0.1, 0]
+shift2 = [0, 0, 0]
 
-meshScale=[0.1,0.1,0.1]
+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])
+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
+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)
+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.setGravity(0, 0, -10)
 p.setRealTimeSimulation(1)
 
-colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,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)
+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
+
+  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

examples/pybullet/examples/debugDrawItems.py

@@ -2,18 +2,22 @@ import pybullet as p
 import time
 
 cid = p.connect(p.SHARED_MEMORY)
-if (cid<0):
-	p.connect(p.GUI)
+if (cid < 0):
+  p.connect(p.GUI)
 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.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
+angle = 0
 while (True):
-	time.sleep(0.01)
-	p.resetJointState(kuka,2,angle)
-	p.resetJointState(kuka,3,angle)
-	angle+=0.01
+  time.sleep(0.01)
+  p.resetJointState(kuka, 2, angle)
+  p.resetJointState(kuka, 3, angle)
+  angle += 0.01

examples/pybullet/examples/dumpLog.py

@@ -7,7 +7,8 @@ import os, fnmatch
 import argparse
 from time import sleep
 
-def readLogFile(filename, verbose = True):
+
+def readLogFile(filename, verbose=True):
   f = open(filename, 'rb')
 
   print('Opened'),
@@ -44,19 +45,19 @@ def readLogFile(filename, verbose = True):
       if verbose:
         print("num chunks:")
         print(len(chunks))
-      
+
       for chunk in chunks:
-        print("len(chunk)=",len(chunk)," sz = ", sz)
+        print("len(chunk)=", len(chunk), " sz = ", sz)
         if len(chunk) == sz:
-          print("chunk #",chunkIndex)
-          chunkIndex=chunkIndex+1
+          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])
-    
+              print("    ", keys[i], "=", values[i])
+
           log.append(record)
     else:
       print("Error, expected aabb terminal")
@@ -65,16 +66,16 @@ def readLogFile(filename, verbose = True):
 
 numArgs = len(sys.argv)
 
-print ('Number of arguments:', numArgs, 'arguments.')
-print ('Argument List:', str(sys.argv))
+print('Number of arguments:', numArgs, 'arguments.')
+print('Argument List:', str(sys.argv))
 fileName = "log.bin"
 
-if (numArgs>1):
+if (numArgs > 1):
   fileName = sys.argv[1]
 
 print("filename=")
 print(fileName)
 
 verbose = True
-  
-readLogFile(fileName,verbose)
+
+readLogFile(fileName, verbose)

examples/pybullet/examples/dumpVrLog.py

@@ -7,7 +7,8 @@ import os, fnmatch
 import argparse
 from time import sleep
 
-def readLogFile(filename, verbose = True):
+
+def readLogFile(filename, verbose=True):
   f = open(filename, 'rb')
 
   print('Opened'),
@@ -37,20 +38,20 @@ def readLogFile(filename, verbose = True):
   chunks = wholeFile.split(b'\xaa\xbb')
   log = list()
   if verbose:
-  	print("num chunks:")
-  	print(len(chunks))
+    print("num chunks:")
+    print(len(chunks))
   chunkIndex = 0
   for chunk in chunks:
-    print("len(chunk)=",len(chunk)," sz = ", sz)
+    print("len(chunk)=", len(chunk), " sz = ", sz)
     if len(chunk) == sz:
-      print("chunk #",chunkIndex)
-      chunkIndex=chunkIndex+1
+      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])
+          print("    ", keys[i], "=", values[i])
 
       log.append(record)
 
@@ -59,19 +60,19 @@ def readLogFile(filename, verbose = True):
 
 numArgs = len(sys.argv)
 
-print ('Number of arguments:', numArgs, 'arguments.')
-print ('Argument List:', str(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]
+if (numArgs > 1):
+  fileName = sys.argv[1]
 
 print("filename=")
 print(fileName)
 
 verbose = True
-	
-log = readLogFile(fileName,verbose)
+
+log = readLogFile(fileName, verbose)
 
 # the index of the first integer in the vr log file for packed buttons
 firstPackedButtonIndex = 13
@@ -83,26 +84,27 @@ numPackedButtons = 7
 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)
+  # 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)

examples/pybullet/examples/eglRenderTest.py

@@ -1,4 +1,3 @@
-
 import pybullet as p
 import time
 import pkgutil
@@ -7,41 +6,48 @@ egl = pkgutil.get_loader('eglRenderer')
 p.connect(p.DIRECT)
 
 plugin = p.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
-print("plugin=",plugin)
+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.setGravity(0, 0, -10)
+p.loadURDF("plane.urdf", [0, 0, -1])
 p.loadURDF("r2d2.urdf")
 
 pixelWidth = 320
 pixelHeight = 220
-camTargetPos = [0,0,0]
+camTargetPos = [0, 0, 0]
 camDistance = 4
 pitch = -10.0
-roll=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))
+  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)
+    #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)

examples/pybullet/examples/experimentalCcdSphereRadius.py

@@ -4,48 +4,50 @@ import time
 p.connect(p.GUI)
 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)
 
-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])
+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)
 
-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)
+p.setGravity(0, 0, -10)
 
 pts = p.getContactPoints()
-print("num points=",len(pts))
+print("num points=", len(pts))
 print(pts)
 while (p.isConnected()):
-	time.sleep(1./240.)
-	p.stepSimulation()
-	
+  time.sleep(1. / 240.)
+  p.stepSimulation()

examples/pybullet/examples/externalTorqueControlledSphere.py

@@ -5,57 +5,55 @@ import time
 p.connect(p.GUI)
 p.setAdditionalSearchPath(pybullet_data.getDataPath())
 p.loadSDF("stadium.sdf")
-p.setGravity(0,0,-10)
+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])
+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]
+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.)
+  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.)

examples/pybullet/examples/fileIOPlugin.py

@@ -3,18 +3,17 @@ import time
 
 p.connect(p.GUI)
 fileIO = p.loadPlugin("fileIOPlugin")
-if (fileIO>=0):
-	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.")
+if (fileIO >= 0):
+  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.)
+  p.stepSimulation()
+  time.sleep(1. / 240.)

examples/pybullet/examples/forcetorquesensor.py

@@ -6,21 +6,20 @@ 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.setJointMotorControl2(hinge, hingeJointIndex, p.VELOCITY_CONTROL, 0, 0, 0)
 
-p.setGravity(0,0,-10)
+p.setGravity(0, 0, -10)
 p.stepSimulation()
 print("joint state without sensor")
 
-print(p.getJointState(0,0))
-p.enableJointForceTorqueSensor(hinge,hingeJointIndex)
+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)
+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))
+print(p.getJointState(0, 0))
 
 p.disconnect()
-

examples/pybullet/examples/frictionCone.py

@@ -5,42 +5,42 @@ import math
 p.connect(p.GUI)
 useMaximalCoordinates = False
 
-p.setGravity(0,0,-10)
-plane=p.loadURDF("plane.urdf",[0,0,-1],useMaximalCoordinates=useMaximalCoordinates)
+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.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])
+for i in range(num):
+  print("progress:", i, num)
 
-	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
+  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])
 
-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
+  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)
+  time.sleep(0.01)

examples/pybullet/examples/getAABB.py

@@ -1,78 +1,79 @@
 import pybullet as p
-draw=1
+draw = 1
 printtext = 0
 
 if (draw):
-	p.connect(p.GUI)
+  p.connect(p.GUI)
 else:
-	p.connect(p.DIRECT)
+  p.connect(p.DIRECT)
 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], 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 = [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 = [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], 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])
 
-	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)
+  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)
+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()
+while (1):
+  a = 0
+  p.stepSimulation()

examples/pybullet/examples/getCameraImageTest.py

@@ -3,16 +3,14 @@ import numpy as np
 import pybullet as p
 import time
 
-
-direct = p.connect(p.GUI)#, options="--window_backend=2 --render_device=0")
+direct = p.connect(p.GUI)  #, options="--window_backend=2 --render_device=0")
 #egl = p.loadPlugin("eglRendererPlugin")
 
-
 p.loadURDF('plane.urdf')
-p.loadURDF("r2d2.urdf",[0,0,1])
+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])
+p.changeVisualShape(cube_trans, -1, rgbaColor=[1, 1, 1, 0.1])
 width = 128
 height = 128
 
@@ -25,42 +23,71 @@ 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)
-rgb_opengl= np.reshape(images[2], (height, width, 4))*1./255.
+images = p.getCameraImage(width,
+                          height,
+                          view_matrix,
+                          projection_matrix,
+                          shadow=True,
+                          renderer=p.ER_BULLET_HARDWARE_OPENGL)
+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.
+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)
+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.
+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])
+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)
+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)
+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.
+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.
+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)
@@ -71,30 +98,29 @@ 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.subplot(4, 2, 3)
 plt.imshow(rgb_opengl)
 plt.title('RGB OpenGL3')
 
-plt.subplot(4,2,4)
+plt.subplot(4, 2, 4)
 plt.imshow(rgb_tiny)
 plt.title('RGB Tiny')
 
-plt.subplot(4,2,5)
+plt.subplot(4, 2, 5)
 plt.imshow(seg_opengl)
 plt.title('Seg OpenGL3')
 
-plt.subplot(4,2,6)
+plt.subplot(4, 2, 6)
 plt.imshow(seg_tiny)
 plt.title('Seg Tiny')
 
-plt.subplot(4,2,7)
+plt.subplot(4, 2, 7)
 plt.imshow(proj_opengl)
 plt.title('Proj OpenGL')
 
-plt.subplot(4,2,8)
+plt.subplot(4, 2, 8)
 plt.imshow(proj_tiny)
 plt.title('Proj Tiny')
 plt.subplots_adjust(hspace=0.7)
 
-
 plt.show()

examples/pybullet/examples/getClosestPoints.py

@@ -2,51 +2,68 @@ import pybullet as p
 import time
 p.connect(p.GUI)
 useCollisionShapeQuery = True
-p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
+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
+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 )
+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
+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)
+  pitch += 0.01
+  if (pitch >= 3.1415 * 2.):
+    pitch = 0
+  yaw += 0.01
+  if (yaw >= 3.1415 * 2.):
+    yaw = 0
 
-	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)
+  baseOrientationB = p.getQuaternionFromEuler([yaw, pitch, 0])
+  if (obB >= 0):
+    p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
 
-	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.)
-	
+  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

examples/pybullet/examples/getTextureUid.py

@@ -6,14 +6,13 @@ print(visualData)
 curTexUid = visualData[0][8]
 print(curTexUid)
 texUid = p.loadTexture("tex256.png")
-print("texUid=",texUid)
+print("texUid=", texUid)
 
-p.changeVisualShape(plane,-1,textureUniqueId=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)
+for i in range(100):
+  p.getCameraImage(320, 200)
+p.changeVisualShape(plane, -1, textureUniqueId=curTexUid)
 
+for i in range(100):
+  p.getCameraImage(320, 200)

examples/pybullet/examples/grpcClient.py

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

examples/pybullet/examples/grpcServer.py

@@ -10,20 +10,20 @@ id = p.loadPlugin("grpcPlugin")
 #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 (id < 0):
+  print("Cannot load grpcPlugin")
+  exit(0)
+
 if usePort:
-	p.executePluginCommand(id, "localhost:12345")
+  p.executePluginCommand(id, "localhost:12345")
 else:
-	p.executePluginCommand(id, "localhost")
+  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)
+  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)

examples/pybullet/examples/hand.py

@@ -12,18 +12,18 @@ import pybullet as p
 #first try to connect to shared memory (VR), if it fails use local GUI
 c = p.connect(p.SHARED_MEMORY)
 print(c)
-if (c<0):
-		p.connect(p.GUI)
-		
+if (c < 0):
+  p.connect(p.GUI)
+
 #load the MuJoCo MJCF hand
 objects = p.loadMJCF("MPL/MPL.xml")
 
-hand=objects[0]
+hand = objects[0]
 #clamp in range 400-600
 #minV = 400
 #maxV = 600
-minVarray = [275,280,350,290]
-maxVarray = [450,550,500,400]
+minVarray = [275, 280, 350, 290]
+maxVarray = [450, 550, 500, 400]
 
 pinkId = 0
 middleId = 1
@@ -32,82 +32,93 @@ 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
+  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)
+  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
+  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
+  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)
+  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,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)
+        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)
 
-				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)
+        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")
+  print("Cannot find port")

examples/pybullet/examples/hello_pybullet.py

@@ -3,21 +3,21 @@ from time import sleep
 
 physicsClient = p.connect(p.GUI)
 
-p.setGravity(0,0,-10)
+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)
+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)
+  p.setRealTimeSimulation(1)
 
 while 1:
-	if (useRealTimeSimulation):
-		p.setGravity(0,0,-10)
-		sleep(0.01) # Time in seconds.
-	else:
-		p.stepSimulation()
+  if (useRealTimeSimulation):
+    p.setGravity(0, 0, -10)
+    sleep(0.01)  # Time in seconds.
+  else:
+    p.stepSimulation()

examples/pybullet/examples/humanoidMotionCapture.py

@@ -1,182 +1,233 @@
-import pybullet	as p
+import pybullet as p
 import json
 import time
 
 useGUI = True
 if useGUI:
-	p.connect(p.GUI)
+  p.connect(p.GUI)
 else:
-	p.connect(p.DIRECT)
+  p.connect(p.DIRECT)
 
 useZUp = False
 useYUp = not useZUp
-showJointMotorTorques	=	False
+showJointMotorTorques = False
 
 if useYUp:
-	p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP	,	1)
+  p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1)
 
-from pdControllerExplicit	import PDControllerExplicitMultiDof
-from pdControllerStable	import PDControllerStableMultiDof
+from pdControllerExplicit import PDControllerExplicitMultiDof
+from pdControllerStable import PDControllerStableMultiDof
 
 explicitPD = PDControllerExplicitMultiDof(p)
 stablePD = PDControllerStableMultiDof(p)
 
-p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94,	cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16])
+p.resetDebugVisualizerCamera(cameraDistance=7,
+                             cameraYaw=-94,
+                             cameraPitch=-14,
+                             cameraTargetPosition=[0.31, 0.03, -1.16])
 
 import pybullet_data
 p.setTimeOut(10000)
-useMotionCapture=False
-useMotionCaptureReset=False#not	useMotionCapture
-useExplicitPD	=	True
+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. / 600.
 #timeStep	=	1./240.
 
 p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
 
-path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt"
+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)
+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)
+numFrames = len(motion_dict['Frames'])
+print("#frames = ", numFrames)
 
+frameId = p.addUserDebugParameter("frame", 0, numFrames - 1, 0)
 
-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)
 
-erpId	=	p.addUserDebugParameter("erp",0,1,0.2)
+jointTypes = [
+    "JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED"
+]
 
-kpMotorId	=	p.addUserDebugParameter("kpMotor",0,1,.2)
-forceMotorId = p.addUserDebugParameter("forceMotor",0,2000,1000)
+startLocations = [[0, 0, 2], [0, 0, 0], [0, 0, -2], [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])
 
+humanoid = p.loadURDF("humanoid/humanoid.urdf",
+                      startLocations[0],
+                      globalScaling=0.25,
+                      useFixedBase=False,
+                      flags=p.URDF_MAINTAIN_LINK_ORDER)
+humanoid2 = p.loadURDF("humanoid/humanoid.urdf",
+                       startLocations[1],
+                       globalScaling=0.25,
+                       useFixedBase=False,
+                       flags=p.URDF_MAINTAIN_LINK_ORDER)
+humanoid3 = p.loadURDF("humanoid/humanoid.urdf",
+                       startLocations[2],
+                       globalScaling=0.25,
+                       useFixedBase=False,
+                       flags=p.URDF_MAINTAIN_LINK_ORDER)
+humanoid4 = p.loadURDF("humanoid/humanoid.urdf",
+                       startLocations[3],
+                       globalScaling=0.25,
+                       useFixedBase=False,
+                       flags=p.URDF_MAINTAIN_LINK_ORDER)
 
+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])
 
-jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
-							"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
-							
-startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]]
+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
+]
 
-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])
+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
+]
 
-humanoid =	p.loadURDF("humanoid/humanoid.urdf", startLocations[0],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
-humanoid2	=	p.loadURDF("humanoid/humanoid.urdf", startLocations[1],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
-humanoid3	=	p.loadURDF("humanoid/humanoid.urdf", startLocations[2],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
-humanoid4	=	p.loadURDF("humanoid/humanoid.urdf", startLocations[3],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
+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])
 
-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])
+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(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(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(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])
 
-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]
+  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)
 
-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]
+  #print(ji)
+  print("joint[", j, "].type=", jointTypes[ji[2]])
+  print("joint[", j, "].name=", ji[1])
 
-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])
+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)
 
-
-
-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(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(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(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)
-	
-	#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
+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
@@ -191,249 +242,320 @@ leftElbow=13
 
 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
+]
 
-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)
 
-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
 
-while	(p.isConnected()):
+  frameFraction = frameReal - frame
+  #print("frameFraction=",frameFraction)
+  #print("frame=",frame)
+  #print("frameNext=", frameNext)
 
-	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
+  #getQuaternionSlerp
 
-	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=humanoid, 
-			jointIndices = jointIndicesAll,
-			desiredPositions = pose,
-			desiredVelocities	=	[0]*totalDofs,
-			kps	=	kpOrg,
-			kds	=	kdOrg,
-			maxForces	=	[maxForce]*totalDofs,	
-			timeStep=timeStep)
-			
-		taus3	=	explicitPD.computePD(bodyUniqueId=humanoid3, 
-			jointIndices = jointIndicesAll,
-			desiredPositions = pose,
-			desiredVelocities	=	[0]*totalDofs,
-			kps	=	kpOrg,
-			kds	=	kdOrg,
-			maxForces	=	[maxForce*0.05]*totalDofs, 
-			timeStep=timeStep)
+  frameData = motion_dict['Frames'][frame]
+  frameDataNext = motion_dict['Frames'][frameNext]
 
-		#taus=[0]*43
-		dofIndex=7
-		for	index	in range (len(jointIndicesAll)):
-			jointIndex = jointIndicesAll[index]
-			if jointDofCounts[index]==4:
-				
-				p.setJointMotorControlMultiDof(humanoid,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(humanoid, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus[dofIndex]])
-				p.setJointMotorControlMultiDof(humanoid3,	jointIndex,	controlMode=p.TORQUE_CONTROL,	force=[taus3[dofIndex]])
+  #print("duration=",frameData[0])
+  #print(pos=[frameData])
 
-			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)
+  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=humanoid,
+                              jointIndices=jointIndicesAll,
+                              desiredPositions=pose,
+                              desiredVelocities=[0] * totalDofs,
+                              kps=kpOrg,
+                              kds=kdOrg,
+                              maxForces=[maxForce] * totalDofs,
+                              timeStep=timeStep)
+
+    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(
+            humanoid,
+            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(humanoid,
+                                       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)

examples/pybullet/examples/humanoid_benchmark.py

@@ -1,20 +1,25 @@
 import pybullet as p
 import time
 p.connect(p.DIRECT)
-p.setGravity(0,0,-10)
+p.setGravity(0, 0, -10)
 p.setPhysicsEngineParameter(numSolverIterations=5)
-p.setPhysicsEngineParameter(fixedTimeStep=1./240.)
+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])
-
+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)
@@ -26,11 +31,10 @@ p.setRealTimeSimulation(0)
 print("Starting benchmark")
 fileName = "pybullet_humanoid_timings.json"
 
-logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,fileName)
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, fileName)
 for i in range(1000):
-	p.stepSimulation()
+  p.stepSimulation()
 p.stopStateLogging(logId)
 
 print("ended benchmark")
 print("Use Chrome browser, visit about://tracing, and load the %s file" % fileName)
-

examples/pybullet/examples/humanoid_knee_position_control.py

@@ -2,8 +2,8 @@ import pybullet as p
 import time
 
 cid = p.connect(p.SHARED_MEMORY)
-if (cid<0):
-	cid = p.connect(p.GUI)
+if (cid < 0):
+  cid = p.connect(p.GUI)
 
 p.resetSimulation()
 
@@ -11,36 +11,41 @@ useRealTime = 0
 
 p.setRealTimeSimulation(useRealTime)
 
-p.setGravity(0,0,-10)
+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)
 
-for i in range (p.getNumJoints(human)):
-	p.setJointMotorControl2(human,i,p.POSITION_CONTROL,targetPosition=0,force=500)
+kneeAngleTargetLeftId = p.addUserDebugParameter("kneeAngleL", -4, 4, -1)
+maxForceLeftId = p.addUserDebugParameter("maxForceL", 0, 500, 10)
 
-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
+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)
+  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()	
+  if (useRealTime == 0):
+    p.stepSimulation()

examples/pybullet/examples/humanoid_manual_control.py

@@ -5,28 +5,28 @@ p.connect(p.GUI)
 obUids = p.loadMJCF("mjcf/humanoid.xml")
 humanoid = obUids[1]
 
-gravId = p.addUserDebugParameter("gravity",-10,10,-10)
-jointIds=[]
-paramIds=[]
+gravId = p.addUserDebugParameter("gravity", -10, 10, -10)
+jointIds = []
+paramIds = []
 
 p.setPhysicsEngineParameter(numSolverIterations=10)
-p.changeDynamics(humanoid,-1,linearDamping=0, angularDamping=0)
+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))
+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)
+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)

examples/pybullet/examples/ik_end_effector_orientation.py

@@ -5,26 +5,37 @@ from datetime import datetime
 from time import sleep
 
 p.connect(p.GUI)
-p.loadURDF("plane.urdf",[0,0,-0.3])
-kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
-p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
+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 = [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)
+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])
+  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)
+    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)

examples/pybullet/examples/integrate.py

@@ -1,13 +1,12 @@
 import pybullet as p
 p.connect(p.GUI)
-cube  = p.loadURDF("cube.urdf")
+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)
+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)
-

examples/pybullet/examples/internalEdge.py

@@ -4,39 +4,37 @@ import time
 p.connect(p.GUI)
 
 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]
+  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)
-	
+  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)
-            
+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)
+p.setGravity(0, 0, -10)
 
 pts = p.getContactPoints()
-print("num points=",len(pts))
+print("num points=", len(pts))
 print(pts)
 while (p.isConnected()):
-	time.sleep(1./240.)
-	p.stepSimulation()
-	
+  time.sleep(1. / 240.)
+  p.stepSimulation()

examples/pybullet/examples/inverse_dynamics.py

@@ -3,7 +3,7 @@ plot = True
 import time
 
 if (plot):
-	import matplotlib.pyplot as plt
+  import matplotlib.pyplot as plt
 import math
 verbose = False
 
@@ -19,137 +19,146 @@ 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)
+  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)
+  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)
 
-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"
+]
 
-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])
+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]
+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])
-	 
+  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_pos = [[0.]* steps,[0.]* steps]
-q_vel = [[0.]* steps,[0.]* steps]
-q_tor = [[0.]* steps,[0.]* steps]
+  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]
+  # Read Sensor States:
+  joint_states = bullet.getJointStates(id_robot, id_revolute_joints)
 
-    # 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]]
+  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
 
-    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)
+  q_vel[0][i] = joint_states[0][1]
+  q_vel[1][i] = joint_states[1][1]
 
-    # Set the Joint Torques:
-    bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.TORQUE_CONTROL, forces=[torque[0], torque[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]]
 
-    # Step Simulation
-    bullet.stepSimulation()
+  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()
+  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)
 
-	plt.pause(0.01)	
+  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)	
+  bullet.stepSimulation()
+  time.sleep(0.01)

examples/pybullet/examples/inverse_kinematics.py

@@ -4,35 +4,34 @@ import math
 from datetime import datetime
 
 clid = p.connect(p.SHARED_MEMORY)
-if (clid<0):
-	p.connect(p.GUI)
-p.loadURDF("plane.urdf",[0,0,-0.3])
-kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
-p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
+if (clid < 0):
+  p.connect(p.GUI)
+p.loadURDF("plane.urdf", [0, 0, -0.3])
+kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0])
+p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
 kukaEndEffectorIndex = 6
 numJoints = p.getNumJoints(kukaId)
-if (numJoints!=7):
-	exit()
-	
+if (numJoints != 7):
+  exit()
 
 #lower limits for null space
-ll=[-.967,-2	,-2.96,0.19,-2.96,-2.09,-3.05]
+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]
+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]
+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]
+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]
+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])
+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]
+p.setGravity(0, 0, 0)
+t = 0.
+prevPose = [0, 0, 0]
+prevPose1 = [0, 0, 0]
 hasPrevPose = 0
 useNullSpace = 1
 
@@ -46,46 +45,73 @@ p.setRealTimeSimulation(useRealTimeSimulation)
 #trailDuration is duration (in seconds) after debug lines will be removed automatically
 #use 0 for no-removal
 trailDuration = 15
-	
-while 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.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)]
-		#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		
+while 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.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)]
+    #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

examples/pybullet/examples/inverse_kinematics_husky_kuka.py

@@ -5,57 +5,57 @@ from datetime import datetime
 from datetime import datetime
 
 clid = p.connect(p.SHARED_MEMORY)
-if (clid<0):
-	p.connect(p.GUI)
-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)
+if (clid < 0):
+  p.connect(p.GUI)
+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])
-
+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])
+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]
+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()
-	
+if (numJoints != 7):
+  exit()
 
 #lower limits for null space
-ll=[-.967,-2	,-2.96,0.19,-2.96,-2.09,-3.05]
+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]
+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]
+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]
+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]
+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])
+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]
+p.setGravity(0, 0, -10)
+t = 0.
+prevPose = [0, 0, 0]
+prevPose1 = [0, 0, 0]
 hasPrevPose = 0
 useNullSpace = 0
 
-useOrientation =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 = 0
@@ -64,106 +64,130 @@ 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]	
+basepos = [0, 0, 0]
+ang = 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]
+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]
+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]]
+  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]
+    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])
+  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
 
-	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		
+  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

examples/pybullet/examples/inverse_kinematics_pole.py

@@ -4,24 +4,23 @@ import math
 from datetime import datetime
 
 clid = p.connect(p.SHARED_MEMORY)
-if (clid<0):
-	p.connect(p.GUI)
-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])
-
+if (clid < 0):
+  p.connect(p.GUI)
+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]
+jd = [0.1, 0.1, 0.1, 0.1]
 
-p.setGravity(0,0,0)
-t=0.
-prevPose=[0,0,0]
-prevPose1=[0,0,0]
+p.setGravity(0, 0, 0)
+t = 0.
+prevPose = [0, 0, 0]
+prevPose1 = [0, 0, 0]
 hasPrevPose = 0
 
 ikSolver = 0
@@ -30,30 +29,35 @@ 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)
+  if (useRealTimeSimulation):
+    dt = datetime.now()
+    t = (dt.second / 60.) * 2. * math.pi
+  else:
+    t = t + 0.01
+    time.sleep(0.01)
 
-		#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])
+  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)
 
-	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		
+    #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

examples/pybullet/examples/jacobian.py

@@ -2,47 +2,54 @@ import pybullet as p
 
 
 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
+  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
+  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)))
+  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
+  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)
+if (clid < 0):
+  p.connect(p.DIRECT)
 
 time_step = 0.001
 gravity_constant = -9.81
@@ -50,14 +57,14 @@ p.resetSimulation()
 p.setTimeStep(time_step)
 p.setGravity(0.0, 0.0, gravity_constant)
 
-p.loadURDF("plane.urdf",[0,0,-0.3])
+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])
+p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
 numJoints = p.getNumJoints(kukaId)
 kukaEndEffectorIndex = numJoints - 1
 
@@ -69,7 +76,10 @@ p.stepSimulation()
 pos, vel, torq = getJointStates(kukaId)
 mpos, mvel, mtorq = getMotorJointStates(kukaId)
 
-result = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1)
+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.
@@ -78,11 +88,11 @@ link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = r
 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))
+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))

examples/pybullet/examples/jointFrictionAndMotor.py

@@ -2,25 +2,23 @@ import pybullet as p
 import time
 
 p.connect(p.GUI)
-door=p.loadURDF("door.urdf")
+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))
+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)
+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)
-	
+  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)

examples/pybullet/examples/jointFrictionDamping.py

@@ -2,27 +2,34 @@ import pybullet as p
 import time
 
 p.connect(p.GUI)
-p.loadURDF("plane.urdf",[0,0,-0.25])
-minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf",useFixedBase=True)
+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])
+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.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)
+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])
+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)
+  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) 
+  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)

examples/pybullet/examples/kuka_grasp_block_playback.py

@@ -2,15 +2,15 @@ import pybullet as p
 import struct
 
 
-def readLogFile(filename, verbose = True):
+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
@@ -41,11 +41,12 @@ def readLogFile(filename, verbose = True):
 
   return log
 
+
 #clid = p.connect(p.SHARED_MEMORY)
 p.connect(p.GUI)
 p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf")
-p.loadURDF("tray/tray.urdf",[0,0,0])
-p.loadURDF("block.urdf",[0,0,2])
+p.loadURDF("tray/tray.urdf", [0, 0, 0])
+p.loadURDF("block.urdf", [0, 0, 2])
 
 log = readLogFile("data/block_grasp_log.bin")
 
@@ -58,26 +59,26 @@ 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])
+  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)
+stepIndexId = p.addUserDebugParameter("stepIndex", 0, recordNum / objectNum - 1, 0)
 
 while True:
-	stepIndex = int(p.readUserDebugParameter(stepIndexId))
-	Step(stepIndex)
-	p.stepSimulation()
-	Step(stepIndex)
-
+  stepIndex = int(p.readUserDebugParameter(stepIndexId))
+  Step(stepIndex)
+  p.stepSimulation()
+  Step(stepIndex)

examples/pybullet/examples/kuka_with_cube.py

@@ -5,36 +5,36 @@ from datetime import datetime
 
 #clid = p.connect(p.SHARED_MEMORY)
 p.connect(p.GUI)
-p.loadURDF("plane.urdf",[0,0,-0.3],useFixedBase=True)
-kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0],useFixedBase=True)
-p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
+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()
+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])
+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]
+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]
+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]
+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]
+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]
+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])
+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]
+p.setGravity(0, 0, -10)
+t = 0.
+prevPose = [0, 0, 0]
+prevPose1 = [0, 0, 0]
 hasPrevPose = 0
 useNullSpace = 0
 
@@ -47,50 +47,68 @@ p.setRealTimeSimulation(useRealTimeSimulation)
 #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)
+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])
+  print("Body %d's name is %s." % (i, p.getBodyInfo(i)[1]))
 
-	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
+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

examples/pybullet/examples/kuka_with_cube_playback.py

@@ -10,15 +10,16 @@ import os, fnmatch
 import argparse
 from time import sleep
 
-def readLogFile(filename, verbose = True):
+
+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
@@ -49,13 +50,14 @@ def readLogFile(filename, verbose = True):
 
   return log
 
+
 #clid = p.connect(p.SHARED_MEMORY)
 p.connect(p.GUI)
-p.loadURDF("plane.urdf",[0,0,-0.3])
-p.loadURDF("kuka_iiwa/model.urdf",[0,0,1])
-p.loadURDF("cube.urdf",[2,2,5])
-p.loadURDF("cube.urdf",[-2,-2,5])
-p.loadURDF("cube.urdf",[2,-2,5])
+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")
 
@@ -67,14 +69,14 @@ 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)
+  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)

examples/pybullet/examples/load_soft_body.py

@@ -3,20 +3,20 @@ from time import sleep
 
 physicsClient = p.connect(p.GUI)
 
-p.setGravity(0,0,-10)
+p.setGravity(0, 0, -10)
 planeId = p.loadURDF("plane.urdf")
 bunnyId = p.loadSoftBody("bunny.obj")
-p.loadURDF("cube_small.urdf",[1,0,1])
+p.loadURDF("cube_small.urdf", [1, 0, 1])
 useRealTimeSimulation = 1
 
 if (useRealTimeSimulation):
-	p.setRealTimeSimulation(1)
+  p.setRealTimeSimulation(1)
 
 while p.isConnected():
-	p.setGravity(0,0,-10)
-	if (useRealTimeSimulation):
+  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()
+    sleep(0.01)  # Time in seconds.
+    #p.getCameraImage(320,200,renderer=p.ER_BULLET_HARDWARE_OPENGL )
+  else:
+    p.stepSimulation()

examples/pybullet/examples/loadingBench.py

@@ -4,25 +4,23 @@ p.connect(p.GUI)
 
 p.resetSimulation()
 timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json")
-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+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)
+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)
+for i in range(100):
+  p.loadURDF("dinnerware/plate.urdf", 0, i, 1)
 
-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
 
 p.stopStateLogging(timinglog)
 print("stopped state logging")
-p.getCameraImage(320,200)
+p.getCameraImage(320, 200)
 
 while (1):
-	p.stepSimulation()
-
-
+  p.stepSimulation()

examples/pybullet/examples/logMinitaur.py

@@ -1,11 +1,11 @@
 import pybullet as p
 cid = p.connect(p.SHARED_MEMORY)
-if (cid < 0) :
-	p.connect(p.GUI)
+if (cid < 0):
+  p.connect(p.GUI)
 p.loadURDF("plane.urdf")
 
 quadruped = p.loadURDF("quadruped/quadruped.urdf")
-logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"LOG00048.TXT",[quadruped])
+logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "LOG00048.TXT", [quadruped])
 p.stepSimulation()
 p.stepSimulation()
 p.stepSimulation()

examples/pybullet/examples/manyspheres.py

@@ -2,34 +2,34 @@ import pybullet as p
 import time
 
 conid = p.connect(p.SHARED_MEMORY)
-if (conid<0):
-	p.connect(p.GUI)
-	
+if (conid < 0):
+  p.connect(p.GUI)
+
 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.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, 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.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)
-
+  gravX = p.readUserDebugParameter(gravXid)
+  gravY = p.readUserDebugParameter(gravYid)
+  gravZ = p.readUserDebugParameter(gravZid)
+  p.setGravity(gravX, gravY, gravZ)
+  time.sleep(0.01)

examples/pybullet/examples/mimicJointConstraint.py

@@ -4,26 +4,44 @@
 import pybullet as p
 import time
 p.connect(p.GUI)
-p.loadURDF("plane.urdf",0,0,-2)
-wheelA = p.loadURDF("differential/diff_ring.urdf",[0,0,0])
+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)
+  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)
 
-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)
-
-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)
-
-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)
+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)
 
+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)
 
 p.setRealTimeSimulation(1)
-while(1):
-	p.setGravity(0,0,-10)
-	time.sleep(0.01)
+while (1):
+  p.setGravity(0, 0, -10)
+  time.sleep(0.01)
 #p.removeConstraint(c)
-	

examples/pybullet/examples/minitaur.py

@@ -1,7 +1,9 @@
 import pybullet as p
 import numpy as np
 
+
 class Minitaur:
+
   def __init__(self, urdfRootPath=''):
     self.urdfRootPath = urdfRootPath
     self.reset()
@@ -26,9 +28,8 @@ class Minitaur:
     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.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, 0, 0, .2)
     self.kp = 1
     self.kd = 0.1
     self.maxForce = 3.5
@@ -38,10 +39,14 @@ class Minitaur:
     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)
+    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)
@@ -49,53 +54,107 @@ class Minitaur:
   def resetPose(self):
     kneeFrictionForce = 0
     halfpi = 1.57079632679
-    kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)
+    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)
+    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)
- 
+    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)
- 
+    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)
-
+    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)

examples/pybullet/examples/minitaur_evaluate.py

@@ -15,6 +15,7 @@ def current_position():
   position = minitaur.getBasePosition()
   return np.asarray(position)
 
+
 def is_fallen():
   global minitaur
   orientation = minitaur.getBaseOrientation()
@@ -22,13 +23,16 @@ def is_fallen():
   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
+    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]
@@ -43,29 +47,37 @@ def evaluate_desired_motorAngle_2Amplitude4Phase(i, params):
   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
+  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_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):
+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)
+  p.setGravity(0, 0, -10)
 
   global minitaur
   minitaur = Minitaur(urdfRoot)
@@ -95,5 +107,6 @@ def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep
   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)
+  print("trial for ", params, " final_distance", final_distance, "total_energy", total_energy,
+        "finalReturn", finalReturn, "elapsed_time", elapsedTime)
   return finalReturn

examples/pybullet/examples/minitaur_test.py

@@ -10,18 +10,27 @@ import time
 import math
 import numpy as np
 
+
 def main(unused_args):
   timeStep = 0.01
   c = p.connect(p.SHARED_MEMORY)
-  if (c<0):
-      c = p.connect(p.GUI)
+  if (c < 0):
+    c = p.connect(p.GUI)
 
-  params = [0.1903581461951056, 0.0006732219568880068, 0.05018085615283363, 3.219916795483583, 6.2406418167980595, 4.189869754607539]
+  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)
+  finalReturn = evaluate_params(evaluateFunc=evaluate_func,
+                                params=params,
+                                objectiveParams=[energy_weight],
+                                timeStep=timeStep,
+                                sleepTime=timeStep)
 
   print(finalReturn)
 
+
 main(0)

examples/pybullet/examples/motorMaxVelocity.py

@@ -2,11 +2,19 @@ import pybullet as p
 import time
 
 p.connect(p.GUI)
-cartpole=p.loadURDF("cartpole.urdf")
+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)
+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)
+  p.setGravity(0, 0, -10)
+  js = p.getJointState(cartpole, 1)
+  print("position=", js[0], "velocity=", js[1])
+  time.sleep(0.01)

examples/pybullet/examples/otherPhysicsEngine.py

@@ -6,43 +6,47 @@ import math
 usePhysX = True
 useMaximalCoordinates = True
 if usePhysX:
-  p.connect(p.PhysX,options="--numCores=8 --solver=pgs")
+  p.connect(p.PhysX, options="--numCores=8 --solver=pgs")
   p.loadPlugin("eglRendererPlugin")
 else:
   p.connect(p.GUI)
 
-p.setPhysicsEngineParameter(fixedTimeStep=1./240.,numSolverIterations=4, minimumSolverIslandSize=1024)
+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")
+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
+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)
+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
 
-print("numDominoes=",numDominoes)
+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):
@@ -51,48 +55,51 @@ print("numDominoes=",numDominoes)
 #        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!")
 
+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)
+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)
+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)
+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
+angle = math.pi * 0.5
 
-count=0
+count = 0
 while (1):
-  count+=1
-  if (count==12):
-      p.stopStateLogging(logId)
-      p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,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
+  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)
+    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()

examples/pybullet/examples/pdControl.py

@@ -1,4 +1,3 @@
-
 import pybullet as p
 from pdControllerExplicit import PDControllerExplicitMultiDof
 from pdControllerExplicit import PDControllerExplicit
@@ -6,107 +5,145 @@ from pdControllerStable import PDControllerStable
 
 import time
 
-
-useMaximalCoordinates=False
+useMaximalCoordinates = False
 p.connect(p.GUI)
-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)
+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)
 
-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))
+  #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)
 
-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]
+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)
+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)
+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)
+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)
+  #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)
+  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]])
+  desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
+  desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
+  kpPole = p.readUserDebugParameter(kpPoleId)
+  kdPole = p.readUserDebugParameter(kdPoleId)
+  maxForcePole = p.readUserDebugParameter(maxForcePoleId)
 
-		
-	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)
-	
-	
+  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)

examples/pybullet/examples/pdControllerExplicit.py

@@ -2,88 +2,97 @@ 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
+  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
-			
-			
+  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

examples/pybullet/examples/pdControllerStable.py

@@ -1,144 +1,148 @@
 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)
-			
-			p =  Kp.dot(qError)
-			
-			#np.savetxt("pb_qError.csv", qError, delimiter=",")
-			#np.savetxt("pb_p.csv", p, delimiter=",")
-			
-			d = Kd.dot(qdoterr)
+  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)
+
+    p = Kp.dot(qError)
+
+    #np.savetxt("pb_qError.csv", qError, delimiter=",")
+    #np.savetxt("pb_p.csv", p, delimiter=",")
+
+    d = Kd.dot(qdoterr)
+
+    #np.savetxt("pb_d.csv", d, delimiter=",")
+    #np.savetxt("pbqdoterr.csv", qdoterr, delimiter=",")
+
+    M1 = self._pb.calculateMassMatrix(bodyUniqueId, q1, flags=1)
+
+    M2 = np.array(M1)
+    #np.savetxt("M2.csv", M2, delimiter=",")
+
+    M = (M2 + Kd * timeStep)
+
+    #np.savetxt("pbM_tKd.csv",M, delimiter=",")
+
+    c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)
+
+    c = np.array(c1)
+    #np.savetxt("pbC.csv",c, delimiter=",")
+    A = M
+    #p = [0]*43
+    b = p + d - c
+    #np.savetxt("pb_acc.csv",b, delimiter=",")
+    qddot = np.linalg.solve(A, b)
+    tau = p + d - Kd.dot(qddot) * timeStep
+    #print("len(tau)=",len(tau))
+    maxF = np.array(maxForces)
+    forces = np.clip(tau, -maxF, maxF)
+    return forces
+
 
-			#np.savetxt("pb_d.csv", d, delimiter=",")
-			#np.savetxt("pbqdoterr.csv", qdoterr, delimiter=",")
-			
-			
-			M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1, flags=1)
-			
-			
-			M2 = np.array(M1)
-			#np.savetxt("M2.csv", M2, delimiter=",")
-			
-			M = (M2 + Kd * timeStep)
-			
-			#np.savetxt("pbM_tKd.csv",M, delimiter=",")
-			
-		
-			
-			c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)
-			
-			
-			c = np.array(c1)
-			#np.savetxt("pbC.csv",c, delimiter=",")
-			A = M
-			#p = [0]*43
-			b =  p + d -c
-			#np.savetxt("pb_acc.csv",b, delimiter=",")
-			qddot = np.linalg.solve(A, b)
-			tau = p + d - Kd.dot(qddot) * timeStep
-			#print("len(tau)=",len(tau))
-			maxF = np.array(maxForces)
-			forces = np.clip(tau, -maxF , maxF )
-			return forces
-			
-		
-			
 class PDControllerStable(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 = []
-			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)
-			p =  Kp.dot(qError)
-			d = Kd.dot(qdotError)
-			forces = p + d
-			
-			M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1)
-			M2 = np.array(M1)
-			M = (M2 + Kd * timeStep)
-			c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
-			c = np.array(c1)
-			A = M
-			b = -c + p + d
-			qddot = np.linalg.solve(A, b)
-			tau = p + d - Kd.dot(qddot) * timeStep
-			maxF = np.array(maxForces)
-			forces = np.clip(tau, -maxF , maxF )
-			#print("c=",c)
-			return tau
+  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)
+    p = Kp.dot(qError)
+    d = Kd.dot(qdotError)
+    forces = p + d
+
+    M1 = self._pb.calculateMassMatrix(bodyUniqueId, q1)
+    M2 = np.array(M1)
+    M = (M2 + Kd * timeStep)
+    c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
+    c = np.array(c1)
+    A = M
+    b = -c + p + d
+    qddot = np.linalg.solve(A, b)
+    tau = p + d - Kd.dot(qddot) * timeStep
+    maxF = np.array(maxForces)
+    forces = np.clip(tau, -maxF, maxF)
+    #print("c=",c)
+    return tau

examples/pybullet/examples/pointCloudFromCameraImage.py

@@ -1,123 +1,136 @@
-
 import pybullet as p
 import math
 import numpy as np
 
-
-
 p.connect(p.GUI)
 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
+cube = p.loadURDF("cube.urdf", [0, 0, 1])
 
 
-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]]
+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])
+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]
 
-cornersX = [0,width,width,0]
-cornersY = [0,0,height,height]
-corners3D=[]
+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])
 
-imgW = int(width/10)
-imgH = int(height/10)
+cornersX = [0, width, width, 0]
+cornersY = [0, 0, height, height]
+corners3D = []
 
-img = p.getCameraImage(imgW,imgH, renderer=p.ER_BULLET_HARDWARE_OPENGL)
-rgbBuffer=img[2]
+imgW = int(width / 10)
+imgH = int(height / 10)
+
+img = p.getCameraImage(imgW, imgH, renderer=p.ER_BULLET_HARDWARE_OPENGL)
+rgbBuffer = img[2]
 depthBuffer = img[3]
-print("rgbBuffer.shape=",rgbBuffer.shape)
-print("depthBuffer.shape=",depthBuffer.shape)
+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)
+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)
+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)
+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)
-
-		
-
-
+  p.setGravity(0, 0, -10)

examples/pybullet/examples/profileTiming.py

@@ -3,10 +3,9 @@ import time
 
 p.connect(p.GUI)
 
-t = time.time()+0.1
+t = time.time() + 0.1
 
 logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "haha")
-while (time.time()<t):
-	p.submitProfileTiming("pythontest")
+while (time.time() < t):
+  p.submitProfileTiming("pythontest")
 p.stopStateLogging(logId)
-		

examples/pybullet/examples/projective_texture.py

@@ -5,31 +5,35 @@ import numpy as np
 
 physicsClient = p.connect(p.GUI)
 
-p.setGravity(0,0,0)
-bearStartPos1 = [-3.3,0,0]
-bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
+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)
+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)
+  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()
+  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()

examples/pybullet/examples/quadruped.py

@@ -4,43 +4,102 @@ import math
 
 
 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)
+  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
@@ -48,12 +107,12 @@ useMaximalCoordinates = False
 useRealTime = 1
 
 #the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance
-fixedTimeStep = 1./100
+fixedTimeStep = 1. / 100
 numSolverIterations = 50
 
 if (useMaximalCoordinates):
-	fixedTimeStep = 1./500
-	numSolverIterations = 200
+  fixedTimeStep = 1. / 500
+  numSolverIterations = 200
 
 speed = 10
 amplitude = 0.8
@@ -64,32 +123,37 @@ kp = 1
 kd = .5
 maxKneeForce = 1000
 
-
 physId = p.connect(p.SHARED_MEMORY)
-if (physId<0):
-	p.connect(p.GUI)
+if (physId < 0):
+  p.connect(p.GUI)
 #p.resetSimulation()
 
-angle = 0 # pick in range 0..0.2 radians
-orn = p.getQuaternionFromEuler([0,angle,0])
-p.loadURDF("plane.urdf",[0,0,0],orn)
+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.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.setGravity(0, 0, 0)
 p.setTimeStep(fixedTimeStep)
 
-orn = p.getQuaternionFromEuler([0,0,0.4])
+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)
+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]
-
+  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']
@@ -116,183 +180,286 @@ 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]
+motordir = [-1, -1, -1, -1, 1, 1, 1, 1]
 halfpi = 1.57079632679
-twopi = 4*halfpi
+twopi = 4 * halfpi
 kneeangle = -2.1834
 
-dyn = p.getDynamicsInfo(quadruped,-1)
-mass=dyn[0]
-friction=dyn[1]
+dyn = p.getDynamicsInfo(quadruped, -1)
+mass = dyn[0]
+friction = dyn[1]
 localInertiaDiagonal = dyn[2]
 
-print("localInertiaDiagonal",localInertiaDiagonal)
+print("localInertiaDiagonal", localInertiaDiagonal)
 
 #this is a no-op, just to show the API
-p.changeDynamics(quadruped,-1,localInertiaDiagonal=localInertiaDiagonal)
+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, -1, [1, 0, 0])
 #drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
 
-for i in range (nJoints):
-	drawInertiaBox(quadruped,i, [0,1,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)
+  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.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)
-	
+  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.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)
+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
+]
 
-
-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])
+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)
+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)
+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])
-
-
-
-
-
+logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "quadrupedLog.bin", [quadruped])
 
 #jump
 t = 0.0
 t_end = t + 100
-i=0
+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)
+  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)

examples/pybullet/examples/quadruped_playback.py

@@ -10,15 +10,16 @@ import os, fnmatch
 import argparse
 from time import sleep
 
-def readLogFile(filename, verbose = True):
+
+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
@@ -38,9 +39,9 @@ def readLogFile(filename, verbose = True):
   wholeFile = f.read()
   # split by alignment word
   chunks = wholeFile.split(b'\xaa\xbb')
-  print ("num chunks")
-  print (len(chunks))
-  
+  print("num chunks")
+  print(len(chunks))
+
   log = list()
   for chunk in chunks:
     if len(chunk) == sz:
@@ -52,9 +53,10 @@ def readLogFile(filename, verbose = True):
 
   return log
 
+
 clid = p.connect(p.SHARED_MEMORY)
 
-log = readLogFile("LOG00076.TXT");
+log = readLogFile("LOG00076.TXT")
 
 recordNum = len(log)
 print('record num:'),
@@ -72,8 +74,6 @@ maxForce = 3.5
 kp = .05
 kd = .5
 
-
-
 quadruped = 1
 nJoints = p.getNumJoints(quadruped)
 jointNameToId = {}
@@ -106,21 +106,71 @@ 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]
-
+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.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)
-  

examples/pybullet/examples/quadruped_setup_playback.py

@@ -1,21 +1,43 @@
 import pybullet as p
 p.connect(p.SHARED_MEMORY)
-objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,-.300000,0.000000,0.000000,0.000000,1.000000)]
-objects = [p.loadURDF("quadruped/minitaur.urdf", [-0.000046,-0.000068,0.200774],[-0.000701,0.000387,-0.000252,1.000000],useFixedBase=False)]
+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)
+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)
+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()

examples/pybullet/examples/racecar.py

@@ -1,9 +1,9 @@
 import os, inspect
 currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
-print ("current_dir=" + currentdir)
-parentdir = os.path.join(currentdir,"../gym")
+print("current_dir=" + currentdir)
+parentdir = os.path.join(currentdir, "../gym")
 
-os.sys.path.insert(0,parentdir)
+os.sys.path.insert(0, parentdir)
 
 import pybullet as p
 import pybullet_data
@@ -11,48 +11,52 @@ import pybullet_data
 import time
 
 cid = p.connect(p.SHARED_MEMORY)
-if (cid<0):
-	p.connect(p.GUI)
-	
+if (cid < 0):
+  p.connect(p.GUI)
+
 p.resetSimulation()
-p.setGravity(0,0,-10)
+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.setRealTimeSimulation(useRealTimeSim)  # either this
 #p.loadURDF("plane.urdf")
-p.loadSDF(os.path.join(pybullet_data.getDataPath(),"stadium.sdf"))
+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))
+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]
+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]
+  p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
 
-targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-10,10,0)
-maxForceSlider = p.addUserDebugParameter("maxForce",0,10,10)
-steeringSlider = p.addUserDebugParameter("steering",-0.5,0.5,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)
+  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)

examples/pybullet/examples/racecar_differential.py

@@ -2,73 +2,131 @@ import pybullet as p
 import time
 
 cid = p.connect(p.SHARED_MEMORY)
-if (cid<0):
-	p.connect(p.GUI)
-	
+if (cid < 0):
+  p.connect(p.GUI)
+
 p.resetSimulation()
-p.setGravity(0,0,-10)
+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.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))
+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)	
+  p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
+  p.getJointInfo(car, wheel)
 
-wheels = [8,15]
+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,
+                       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,
+                       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,
+                       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,
+                       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,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,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)
 
-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]
 
-
-steering = [0,2]
-
-targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-50,50,0)
-maxForceSlider = p.addUserDebugParameter("maxForce",0,50,20)
-steeringSlider = p.addUserDebugParameter("steering",-1,1,0)
+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)
+  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)

examples/pybullet/examples/renderPlugin.py

@@ -1,9 +1,9 @@
-
 import pybullet as p
 p.connect(p.GUI)
-plugin = p.loadPlugin("d:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll","_testPlugin")
-print("plugin=",plugin)
+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)
+  p.getCameraImage(320, 200)

examples/pybullet/examples/rendertest.py

@@ -11,16 +11,16 @@ import numpy as np
 import pybullet
 from multiprocessing import Process
 
-camTargetPos = [0,0,0]
-cameraUp = [0,0,1]
-cameraPos = [1,1,1]
+camTargetPos = [0, 0, 0]
+cameraUp = [0, 0, 1]
+cameraPos = [1, 1, 1]
 
 pitch = -10.0
-roll=0
+roll = 0
 upAxisIndex = 2
 camDistance = 4
-pixelWidth = 84 # 320
-pixelHeight = 84 # 200
+pixelWidth = 84  # 320
+pixelHeight = 84  # 200
 nearPlane = 0.01
 farPlane = 100
 fov = 60
@@ -29,118 +29,122 @@ 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'
+  def __init__(self, connection_mode, *argv):
+    self.connection_mode = connection_mode
+    self.argv = argv
 
-        print(self.connection_mode, optionstring,*self.argv)
-        cid = pybullet.connect(self.connection_mode, options=optionstring,*self.argv)
-        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)
+  def __enter__(self):
+    print("connecting")
+    optionstring = '--width={} --height={}'.format(pixelWidth, pixelHeight)
+    optionstring += ' --window_backend=2 --render_device=0'
 
-        pybullet.resetSimulation()
-        pybullet.loadURDF("plane.urdf",[0,0,-1])
-        pybullet.loadURDF("r2d2.urdf")
-        pybullet.loadURDF("duck_vhacd.urdf")
-        pybullet.setGravity(0,0,-10)
+    print(self.connection_mode, optionstring, *self.argv)
+    cid = pybullet.connect(self.connection_mode, options=optionstring, *self.argv)
+    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 __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 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:
-        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
+      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 = []
 
-    res = []
+  with BulletSim(pybullet.DIRECT):
+    print("\nTesting DIRECT")
+    mean_time = test(log=False, plot=True)
+    res.append(("tiny", mean_time))
 
-    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.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]))
+  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]))

examples/pybullet/examples/rendertest_sync.py

@@ -15,11 +15,11 @@ import pybullet as p
 from itertools import cycle
 import numpy as np
 
-camTargetPos = [0,0,0]
-cameraUp = [0,0,1]
-cameraPos = [1,1,1]
+camTargetPos = [0, 0, 0]
+cameraUp = [0, 0, 1]
+cameraPos = [1, 1, 1]
 pitch = -10.0
-roll=0
+roll = 0
 upAxisIndex = 2
 camDistance = 4
 pixelWidth = 320
@@ -30,113 +30,126 @@ 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)
+  def __init__(
+      self,
+      renderer='tiny',  # ('tiny', 'egl', 'debug')
+  ):
+    self.action_space = spaces.Discrete(2)
+    self.iter = cycle(range(0, 360, 10))
 
-            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)
+    # 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)
 
-        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)
+      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 == "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])
+    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)
 
-        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)
+    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 __del__(self):
+    p.disconnect()
 
-    def reset(self):
-        pass
+  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()
+  def step(self, action):
+    p.stepSimulation()
     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
+    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 = []
+  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]))
+  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]))

examples/pybullet/examples/reset_dynamic_info.py

@@ -3,64 +3,124 @@ import time
 import math
 
 p.connect(p.GUI)
-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])
+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.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
+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()
+  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()

examples/pybullet/examples/restitution.py

@@ -5,36 +5,36 @@ import pybullet as p
 import time
 
 cid = p.connect(p.SHARED_MEMORY)
-if (cid<0):
-	cid = p.connect(p.GUI)
-restitutionId = p.addUserDebugParameter("restitution",0,1,1)
-restitutionVelocityThresholdId = p.addUserDebugParameter("res. vel. threshold",0,3,0.2)
+if (cid < 0):
+  cid = p.connect(p.GUI)
+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)
+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])
+sphere = p.loadURDF("sphere_with_restitution.urdf", [0, 0, 2])
 
 p.setRealTimeSimulation(1)
-p.setGravity(0,0,-10)
+p.setGravity(0, 0, -10)
 while (1):
-	restitution = p.readUserDebugParameter(restitutionId)
-	restitutionVelocityThreshold = p.readUserDebugParameter(restitutionVelocityThresholdId)
-	p.setPhysicsEngineParameter(restitutionVelocityThreshold=restitutionVelocityThreshold)
+  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)
+  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)

examples/pybullet/examples/robotcontrol.py

@@ -1,30 +1,28 @@
 import pybullet as p
-p.connect(p.GUI) #or p.SHARED_MEMORY or p.DIRECT
+p.connect(p.GUI)  #or p.SHARED_MEMORY or p.DIRECT
 
 p.loadURDF("plane.urdf")
-p.setGravity(0,0,-10)
-huskypos = [0,0,0.1]
+p.setGravity(0, 0, -10)
+huskypos = [0, 0, 0.1]
 
-husky = p.loadURDF("husky/husky.urdf",huskypos[0],huskypos[1],huskypos[2])
+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(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()
+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()
+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()
-
-

examples/pybullet/examples/rollPitchYaw.py

@@ -2,25 +2,24 @@ import pybullet as p
 import time
 
 cid = p.connect(p.SHARED_MEMORY)
-if (cid<0):
-	p.connect(p.GUI)
-q = p.loadURDF("quadruped/quadruped.urdf",useFixedBase=True)
-rollId = p.addUserDebugParameter("roll",-1.5,1.5,0)
-pitchId = p.addUserDebugParameter("pitch",-1.5,1.5,0)
-yawId = p.addUserDebugParameter("yaw",-1.5,1.5,0)
-fwdxId = p.addUserDebugParameter("fwd_x",-1,1,0)
-fwdyId = p.addUserDebugParameter("fwd_y",-1,1,0)
-fwdzId = p.addUserDebugParameter("fwd_z",-1,1,0)
+if (cid < 0):
+  p.connect(p.GUI)
+q = p.loadURDF("quadruped/quadruped.urdf", useFixedBase=True)
+rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0)
+pitchId = p.addUserDebugParameter("pitch", -1.5, 1.5, 0)
+yawId = p.addUserDebugParameter("yaw", -1.5, 1.5, 0)
+fwdxId = p.addUserDebugParameter("fwd_x", -1, 1, 0)
+fwdyId = p.addUserDebugParameter("fwd_y", -1, 1, 0)
+fwdzId = p.addUserDebugParameter("fwd_z", -1, 1, 0)
 
 while True:
-	roll = p.readUserDebugParameter(rollId)
-	pitch = p.readUserDebugParameter(pitchId)
-	yaw = p.readUserDebugParameter(yawId)
-	x = p.readUserDebugParameter(fwdxId)
-	y = p.readUserDebugParameter(fwdyId)
-	z = p.readUserDebugParameter(fwdzId)
-
-	orn = p.getQuaternionFromEuler([roll,pitch,yaw])
-	p.resetBasePositionAndOrientation(q,[x,y,z],orn)
-	#p.stepSimulation()#not really necessary for this demo, no physics used
+  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

examples/pybullet/examples/satCollision.py

@@ -1,16 +1,18 @@
 import pybullet as p
-import time 
+import time
 
 p.connect(p.GUI)
-p.setGravity(0,0,-10)
+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)
+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.)
-	
+  p.stepSimulation()
+  pts = p.getContactPoints()
+  #print("num contacts = ", len(pts))
+  time.sleep(1. / 240.)

examples/pybullet/examples/saveRestoreState.py

@@ -1,6 +1,6 @@
 import pybullet as p
 import math, time
-import difflib,sys
+import difflib, sys
 
 numSteps = 500
 numSteps2 = 30
@@ -10,69 +10,71 @@ verbose = 0
 
 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)
+  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")
-	
+  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()	
+for i in range(numSteps):
+  p.stepSimulation()
 p.saveBullet("state.bullet")
 if verbose:
-	p.setInternalSimFlags(1)
+  p.setInternalSimFlags(1)
 p.stepSimulation()
 if verbose:
-	p.setInternalSimFlags(0)
-	print("contact points=")
-	for q in p.getContactPoints():
-		print(q)
+  p.setInternalSimFlags(0)
+  print("contact points=")
+  for q in p.getContactPoints():
+    print(q)
 
-for i in range (numSteps2):
-	p.stepSimulation()
+for i in range(numSteps2):
+  p.stepSimulation()
 
-
-file = open("saveFile.txt","w") 
+file = open("saveFile.txt", "w")
 dumpStateToFile(file)
-file.close() 
+file.close()
 
 #################################
 setupWorld()
@@ -80,58 +82,55 @@ setupWorld()
 #both restore from file or from in-memory state should work
 p.restoreState(fileName="state.bullet")
 stateId = p.saveState()
-print("stateId=",stateId)
+print("stateId=", stateId)
 p.removeState(stateId)
 stateId = p.saveState()
-print("stateId=",stateId)
+print("stateId=", stateId)
 
 if verbose:
-	p.setInternalSimFlags(1)
+  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()
+  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")
+file = open("restoreFile.txt", "w")
 dumpStateToFile(file)
-file.close() 
+file.close()
 
 p.restoreState(stateId)
 if verbose:
-	p.setInternalSimFlags(1)
+  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()
+  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")
+file = open("restoreFile2.txt", "w")
 dumpStateToFile(file)
-file.close() 
+file.close()
 
-file1 = open("saveFile.txt","r") 
-file2 = open("restoreFile.txt","r") 
-compareFiles(file1,file2)
+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 = open("saveFile.txt", "r")
+file2 = open("restoreFile2.txt", "r")
+compareFiles(file1, file2)
 file1.close()
-file2.close()        
+file2.close()
 
 p.stopStateLogging(logId)
 
 #while (p.getConnectionInfo()["isConnected"]):
 #	time.sleep(1)
-

examples/pybullet/examples/segmask_linkindex.py

@@ -1,42 +1,41 @@
 import pybullet as p
 p.connect(p.GUI)
-r2d2 = p.loadURDF("r2d2.urdf",[0,0,1])
-for l in range (p.getNumJoints(r2d2)):
-	print(p.getJointInfo(r2d2,l))
+r2d2 = p.loadURDF("r2d2.urdf", [0, 0, 1])
+for l in range(p.getNumJoints(r2d2)):
+  print(p.getJointInfo(r2d2, l))
 
-p.loadURDF("r2d2.urdf",[2,0,1])
-p.loadURDF("r2d2.urdf",[4,0,1])
+p.loadURDF("r2d2.urdf", [2, 0, 1])
+p.loadURDF("r2d2.urdf", [4, 0, 1])
 
-p.getCameraImage(320,200,flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)
-segLinkIndex=1
+p.getCameraImage(320, 200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)
+segLinkIndex = 1
 verbose = 0
 
 while (1):
-	keys = p.getKeyboardEvents()
-	#for k in keys:
-	#	print("key=",k,"state=", keys[k])
-	if ord('1') in keys:
-		state = keys[ord('1')]
-		if (state & p.KEY_WAS_RELEASED):
-			verbose = 1-verbose
-	if ord('s') in keys:
-		state = keys[ord('s')]
-		if (state & p.KEY_WAS_RELEASED):
-			segLinkIndex = 1-segLinkIndex
-			#print("segLinkIndex=",segLinkIndex)
-	flags = 0
-	if (segLinkIndex):
-		flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX
-		
-	img = p.getCameraImage(320,200,flags=flags)
-	#print(img[0],img[1])
-	seg=img[4]
-	if (verbose):
-		for pixel in seg:
-			if (pixel>=0):
-				obUid = pixel & ((1<<24)-1)
-				linkIndex = (pixel >> 24)-1
-				print("obUid=",obUid,"linkIndex=",linkIndex)
-	
-	
-	p.stepSimulation()
+  keys = p.getKeyboardEvents()
+  #for k in keys:
+  #	print("key=",k,"state=", keys[k])
+  if ord('1') in keys:
+    state = keys[ord('1')]
+    if (state & p.KEY_WAS_RELEASED):
+      verbose = 1 - verbose
+  if ord('s') in keys:
+    state = keys[ord('s')]
+    if (state & p.KEY_WAS_RELEASED):
+      segLinkIndex = 1 - segLinkIndex
+      #print("segLinkIndex=",segLinkIndex)
+  flags = 0
+  if (segLinkIndex):
+    flags = p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX
+
+  img = p.getCameraImage(320, 200, flags=flags)
+  #print(img[0],img[1])
+  seg = img[4]
+  if (verbose):
+    for pixel in seg:
+      if (pixel >= 0):
+        obUid = pixel & ((1 << 24) - 1)
+        linkIndex = (pixel >> 24) - 1
+        print("obUid=", obUid, "linkIndex=", linkIndex)
+
+  p.stepSimulation()

examples/pybullet/examples/shiftCenterOfMass.py

@@ -6,17 +6,29 @@ p.connect(p.GUI)
 p.setAdditionalSearchPath(pybullet_data.getDataPath())
 
 p.loadURDF("plane.urdf")
-p.setGravity(0,0,-10)
-visualShift = [0,0,0]
-collisionShift = [0,0,0]
-inertiaShift = [0,0,-0.5]
+p.setGravity(0, 0, -10)
+visualShift = [0, 0, 0]
+collisionShift = [0, 0, 0]
+inertiaShift = [0, 0, -0.5]
 
-meshScale=[1,1,1]
-visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="cube.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=visualShift, meshScale=meshScale)
-collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="cube.obj", collisionFramePosition=collisionShift,meshScale=meshScale)
+meshScale = [1, 1, 1]
+visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
+                                    fileName="cube.obj",
+                                    rgbaColor=[1, 1, 1, 1],
+                                    specularColor=[0.4, .4, 0],
+                                    visualFramePosition=visualShift,
+                                    meshScale=meshScale)
+collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
+                                          fileName="cube.obj",
+                                          collisionFramePosition=collisionShift,
+                                          meshScale=meshScale)
 
-p.createMultiBody(baseMass=1,baseInertialFramePosition=inertiaShift,baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [0,0,1], useMaximalCoordinates=False)
+p.createMultiBody(baseMass=1,
+                  baseInertialFramePosition=inertiaShift,
+                  baseCollisionShapeIndex=collisionShapeId,
+                  baseVisualShapeIndex=visualShapeId,
+                  basePosition=[0, 0, 1],
+                  useMaximalCoordinates=False)
 while (1):
-	p.stepSimulation()
-	time.sleep(1./240.)
-		
+  p.stepSimulation()
+  time.sleep(1. / 240.)

examples/pybullet/examples/signedDistanceField.py

@@ -1,15 +1,14 @@
 import pybullet as p
-import pybullet 
+import pybullet
 import time
 p.connect(p.GUI)
 p.loadURDF("toys/concave_box.urdf")
-p.setGravity(0,0,-10)
-for i in range (10):
-	p.loadURDF("sphere_1cm.urdf",[i*0.02,0,0.5])
+p.setGravity(0, 0, -10)
+for i in range(10):
+  p.loadURDF("sphere_1cm.urdf", [i * 0.02, 0, 0.5])
 p.loadURDF("duck_vhacd.urdf")
-timeStep = 1./240.
+timeStep = 1. / 240.
 p.setTimeStep(timeStep)
 while (1):
-	p.stepSimulation()
-	time.sleep(timeStep)
-	
+  p.stepSimulation()
+  time.sleep(timeStep)

examples/pybullet/examples/sleeping.py

@@ -1,34 +1,34 @@
 import pybullet as p
 import time
-useMaximalCoordinates=False
+useMaximalCoordinates = False
 
 flags = p.URDF_ENABLE_SLEEPING
 
 p.connect(p.GUI)
-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
 
-p.loadURDF("plane100.urdf",flags=flags, useMaximalCoordinates=useMaximalCoordinates)
+p.loadURDF("plane100.urdf", flags=flags, useMaximalCoordinates=useMaximalCoordinates)
 #p.loadURDF("cube_small.urdf", [0,0,0.5], flags=flags)
 r2d2 = -1
-for k in range (5):
-	for i in range (5):
-		r2d2=p.loadURDF("r2d2.urdf",[k*2,i*2,1], useMaximalCoordinates=useMaximalCoordinates, flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES+flags)
+for k in range(5):
+  for i in range(5):
+    r2d2 = p.loadURDF("r2d2.urdf", [k * 2, i * 2, 1],
+                      useMaximalCoordinates=useMaximalCoordinates,
+                      flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES + flags)
 
-		#enable sleeping: you can pass the flag during URDF loading, or do it afterwards
-		#p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING)
-		
-		
-		for j in range (p.getNumJoints(r2d2)):
-			p.setJointMotorControl2(r2d2,j,p.VELOCITY_CONTROL,targetVelocity=0)
-print("r2d2=",r2d2)
-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
-timestep = 1./240.
+    #enable sleeping: you can pass the flag during URDF loading, or do it afterwards
+    #p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING)
+
+    for j in range(p.getNumJoints(r2d2)):
+      p.setJointMotorControl2(r2d2, j, p.VELOCITY_CONTROL, targetVelocity=0)
+print("r2d2=", r2d2)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+timestep = 1. / 240.
 p.setTimeStep(timestep)
-p.setGravity(0,0,-10)
+p.setGravity(0, 0, -10)
 
 while p.isConnected():
-	p.stepSimulation()
-	time.sleep(timestep)
-	#force the object to wake up
-	p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_WAKE_UP)
-	
+  p.stepSimulation()
+  time.sleep(timestep)
+  #force the object to wake up
+  p.changeDynamics(r2d2, -1, activationState=p.ACTIVATION_STATE_WAKE_UP)

examples/pybullet/examples/snake.py

@@ -2,125 +2,141 @@ import pybullet as p
 import time
 import math
 
-
 # This simple snake logic is from some 15 year old Havok C++ demo
 # Thanks to Michael Ewert!
-  
-  
+
 p.connect(p.GUI)
 plane = p.createCollisionShape(p.GEOM_PLANE)
 
-p.createMultiBody(0,plane)
+p.createMultiBody(0, plane)
 
 useMaximalCoordinates = True
 sphereRadius = 0.25
 #colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]])
-colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+                                  halfExtents=[sphereRadius, sphereRadius, sphereRadius])
 
 mass = 1
 visualShapeId = -1
-  
-link_Masses=[]
-linkCollisionShapeIndices=[]
-linkVisualShapeIndices=[]
-linkPositions=[]
-linkOrientations=[]
-linkInertialFramePositions=[]
-linkInertialFrameOrientations=[]
-indices=[]
-jointTypes=[]
-axis=[]
 
-for i in range (36):
+link_Masses = []
+linkCollisionShapeIndices = []
+linkVisualShapeIndices = []
+linkPositions = []
+linkOrientations = []
+linkInertialFramePositions = []
+linkInertialFrameOrientations = []
+indices = []
+jointTypes = []
+axis = []
+
+for i in range(36):
   link_Masses.append(1)
   linkCollisionShapeIndices.append(colBoxId)
   linkVisualShapeIndices.append(-1)
-  linkPositions.append([0,sphereRadius*2.0+0.01,0])
-  linkOrientations.append([0,0,0,1])
-  linkInertialFramePositions.append([0,0,0])
-  linkInertialFrameOrientations.append([0,0,0,1])
+  linkPositions.append([0, sphereRadius * 2.0 + 0.01, 0])
+  linkOrientations.append([0, 0, 0, 1])
+  linkInertialFramePositions.append([0, 0, 0])
+  linkInertialFrameOrientations.append([0, 0, 0, 1])
   indices.append(i)
   jointTypes.append(p.JOINT_REVOLUTE)
-  axis.append([0,0,1])
+  axis.append([0, 0, 1])
 
-basePosition = [0,0,1]
-baseOrientation = [0,0,0,1]
-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, useMaximalCoordinates=useMaximalCoordinates)
+basePosition = [0, 0, 1]
+baseOrientation = [0, 0, 0, 1]
+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,
+                              useMaximalCoordinates=useMaximalCoordinates)
 
-p.setGravity(0,0,-10)
+p.setGravity(0, 0, -10)
 p.setRealTimeSimulation(0)
 
-anistropicFriction = [1,0.01,0.01]
-p.changeDynamics(sphereUid,-1,lateralFriction=2,anisotropicFriction=anistropicFriction)
+anistropicFriction = [1, 0.01, 0.01]
+p.changeDynamics(sphereUid, -1, lateralFriction=2, anisotropicFriction=anistropicFriction)
 p.getNumJoints(sphereUid)
-for i in range (p.getNumJoints(sphereUid)):
-  p.getJointInfo(sphereUid,i)
-  p.changeDynamics(sphereUid,i,lateralFriction=2,anisotropicFriction=anistropicFriction)
-  
-dt = 1./240.
-SNAKE_NORMAL_PERIOD=0.1#1.5
+for i in range(p.getNumJoints(sphereUid)):
+  p.getJointInfo(sphereUid, i)
+  p.changeDynamics(sphereUid, i, lateralFriction=2, anisotropicFriction=anistropicFriction)
+
+dt = 1. / 240.
+SNAKE_NORMAL_PERIOD = 0.1  #1.5
 m_wavePeriod = SNAKE_NORMAL_PERIOD
 
-m_waveLength=4
-m_wavePeriod=1.5
-m_waveAmplitude=0.4
+m_waveLength = 4
+m_wavePeriod = 1.5
+m_waveAmplitude = 0.4
 m_waveFront = 0.0
 #our steering value
 m_steering = 0.0
-m_segmentLength = sphereRadius*2.0
+m_segmentLength = sphereRadius * 2.0
 forward = 0
 
 while (1):
   keys = p.getKeyboardEvents()
-  for k,v in keys.items():
-    
-    if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_TRIGGERED)):
+  for k, v in keys.items():
+
+    if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
       m_steering = -.2
-    if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_RELEASED)):
+    if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
       m_steering = 0
-    if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_TRIGGERED)):
+    if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
       m_steering = .2
-    if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_RELEASED)):
+    if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
       m_steering = 0
-    
+
   amp = 0.2
   offset = 0.6
   numMuscles = p.getNumJoints(sphereUid)
   scaleStart = 1.0
 
-  #start of the snake with smaller waves.  
+  #start of the snake with smaller waves.
   #I think starting the wave at the tail would work better ( while it still goes from head to tail )
-  if( m_waveFront < m_segmentLength*4.0 ):
-    scaleStart = m_waveFront/(m_segmentLength*4.0)
+  if (m_waveFront < m_segmentLength * 4.0):
+    scaleStart = m_waveFront / (m_segmentLength * 4.0)
 
-  segment = numMuscles-1
+  segment = numMuscles - 1
 
   #we simply move a sin wave down the body of the snake.
   #this snake may be going backwards, but who can tell ;)
-  for joint in range (p.getNumJoints(sphereUid)):
-    segment = joint#numMuscles-1-joint
+  for joint in range(p.getNumJoints(sphereUid)):
+    segment = joint  #numMuscles-1-joint
     #map segment to phase
-    phase = (m_waveFront - (segment+1)*m_segmentLength)/ m_waveLength
+    phase = (m_waveFront - (segment + 1) * m_segmentLength) / m_waveLength
     phase -= math.floor(phase)
     phase *= math.pi * 2.0
 
     #map phase to curvature
-    targetPos = math.sin( phase ) * scaleStart * m_waveAmplitude
+    targetPos = math.sin(phase) * scaleStart * m_waveAmplitude
 
     #// steer snake by squashing +ve or -ve side of sin curve
-    if( m_steering > 0 and targetPos < 0 ):
-      targetPos *= 1.0/( 1.0 + m_steering )
-      
-    if( m_steering < 0 and targetPos > 0 ):
-     targetPos *= 1.0/( 1.0 - m_steering )
-    
+    if (m_steering > 0 and targetPos < 0):
+      targetPos *= 1.0 / (1.0 + m_steering)
+
+    if (m_steering < 0 and targetPos > 0):
+      targetPos *= 1.0 / (1.0 - m_steering)
+
     #set our motor
-    p.setJointMotorControl2(sphereUid,joint,p.POSITION_CONTROL,targetPosition=targetPos+m_steering,force=30)
+    p.setJointMotorControl2(sphereUid,
+                            joint,
+                            p.POSITION_CONTROL,
+                            targetPosition=targetPos + m_steering,
+                            force=30)
 
   #wave keeps track of where the wave is in time
-  m_waveFront += dt/m_wavePeriod * m_waveLength;
+  m_waveFront += dt / m_wavePeriod * m_waveLength
   p.stepSimulation()
 
-  
   time.sleep(dt)
-  

examples/pybullet/examples/switchConstraintSolver.py

@@ -3,29 +3,28 @@ import time
 
 p.connect(p.GUI)
 #p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
-p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG, globalCFM = 0.000001)
+p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG,
+                            globalCFM=0.000001)
 #p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
 
-
 p.loadURDF("plane.urdf")
 radius = 0.025
 distance = 1.86
-yaw=135
-pitch=-11
-targetPos=[0,0,0]
+yaw = 135
+pitch = -11
+targetPos = [0, 0, 0]
 
 p.setPhysicsEngineParameter(solverResidualThreshold=0.001, numSolverIterations=200)
-p.resetDebugVisualizerCamera(distance, yaw,pitch, targetPos)
+p.resetDebugVisualizerCamera(distance, yaw, pitch, targetPos)
 objectId = -1
 
-for i in range (10):
-	objectId = p.loadURDF("cube_small.urdf",[1,1,radius+i*2*radius])
+for i in range(10):
+  objectId = p.loadURDF("cube_small.urdf", [1, 1, radius + i * 2 * radius])
 
-p.changeDynamics(objectId,-1,100)
+p.changeDynamics(objectId, -1, 100)
 
-timeStep = 1./240.
-p.setGravity(0,0,-10)
+timeStep = 1. / 240.
+p.setGravity(0, 0, -10)
 while (p.isConnected()):
-	p.stepSimulation()
-	time.sleep(timeStep)
-
+  p.stepSimulation()
+  time.sleep(timeStep)