Merge remote-tracking branch 'bp/master' into pullRequest

data/dinnerware/dinnerware.mtl

@@ -0,0 +1,21 @@
+newmtl porcelain
+Ka 0.000000 0.000000 0.000000
+Kd 1.000000 1.000000 1.000000
+Ks 0.200000 0.200000 0.200000
+d 1.000000
+illum 2
+Ns 0.000000
+
+newmtl solid_color
+Ka 0.000000 0.000000 0.000000
+Kd 1.000000 1.000000 1.000000
+Ks 0.200000 0.200000 0.200000
+d 1.000000
+illum 2
+Ns 0.000000
+
+newmtl pan_tefal
+Ka 0.000000 0.000000 0.000000
+Kd 0.609804 0.494118 0.486275
+Ks 0.330000 0.330000 0.330000
+map_Kd pan_tefal.jpg

data/dinnerware/generate.py

@@ -0,0 +1,252 @@
+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 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]) ) )
+
+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)
+
+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 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 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)
+
+#test_convex_hull()
+#test_contour()
+
+class RotationFigureParams:
+	pass
+
+def generate_plate(p, obj, collision_prefix):
+	contour = Contour()
+	belt_vlist_3d_prev = None
+
+	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
+
+		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)
+
+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")
+
+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")
+
+plate()
+
+

data/dinnerware/pan_tefal-collision01.obj

@@ -0,0 +1,36 @@
+mtllib dinnerware.mtl
+usemtl pan_tefal
+v 0.066236 0.112453 0.075000
+v 0.070225 0.121633 0.075000
+v 0.063600 0.110158 0.000000
+v 0.060950 0.105568 0.000000
+v 0.131811 0.001125 0.075750
+v 0.141855 0.000000 0.075750
+v 0.128472 0.000000 0.000000
+v 0.123119 0.000000 0.000000
+vn 0.166577 -0.705775 0.688573
+vn -0.727212 0.200240 0.656556
+vn 0.637357 -0.756451 -0.146827
+vn -0.340753 -0.711716 0.614286
+vn 0.122979 0.860640 0.494141
+vn 0.770289 -0.238643 0.591358
+vn -0.635571 -0.120420 0.762594
+vn -0.070367 0.649517 -0.757084
+f 1//1 2//2 4//4
+f 1//1 5//5 2//2
+f 1//1 6//6 2//2
+f 1//1 4//4 5//5
+f 1//1 4//4 8//8
+f 1//1 5//5 6//6
+f 1//1 8//8 5//5
+f 2//2 3//3 4//4
+f 2//2 7//7 3//3
+f 2//2 5//5 6//6
+f 2//2 6//6 7//7
+f 3//3 7//7 4//4
+f 3//3 8//8 4//4
+f 3//3 7//7 8//8
+f 4//4 8//8 5//5
+f 4//4 7//7 8//8
+f 5//5 8//8 6//6
+f 6//6 8//8 7//7

data/dinnerware/pan_tefal-collision02.obj

@@ -0,0 +1,36 @@
+mtllib dinnerware.mtl
+usemtl pan_tefal
+v -0.064269 0.113589 0.075000
+v -0.070225 0.121633 0.075000
+v -0.063600 0.110158 0.000000
+v -0.060950 0.105568 0.000000
+v 0.064932 0.114714 0.075750
+v 0.070927 0.122850 0.075750
+v 0.064236 0.111260 0.000000
+v 0.061560 0.106624 0.000000
+vn -0.693799 -0.019731 0.719898
+vn -0.782132 -0.410509 0.468776
+vn 0.057085 0.837906 0.542821
+vn 0.549444 0.695908 -0.462410
+vn -0.488898 0.761278 -0.425951
+vn 0.527121 -0.833525 0.165465
+vn 0.764499 0.413217 0.494766
+vn -0.190845 -0.280421 -0.940714
+f 1//1 2//2 4//4
+f 1//1 5//5 2//2
+f 1//1 6//6 2//2
+f 1//1 4//4 5//5
+f 1//1 4//4 8//8
+f 1//1 5//5 6//6
+f 1//1 8//8 5//5
+f 2//2 3//3 4//4
+f 2//2 7//7 3//3
+f 2//2 5//5 6//6
+f 2//2 6//6 7//7
+f 3//3 7//7 4//4
+f 3//3 8//8 4//4
+f 3//3 7//7 8//8
+f 4//4 8//8 5//5
+f 4//4 7//7 8//8
+f 5//5 8//8 6//6
+f 6//6 8//8 7//7

data/dinnerware/pan_tefal-collision03.obj

@@ -0,0 +1,36 @@
+mtllib dinnerware.mtl
+usemtl pan_tefal
+v -0.130506 0.001136 0.075000
+v -0.140450 0.000000 0.075000
+v -0.127200 0.000000 0.000000
+v -0.121900 0.000000 0.000000
+v -0.066879 0.113589 0.075750
+v -0.070927 0.122850 0.075750
+v -0.064236 0.111260 0.000000
+v -0.061559 0.106624 0.000000
+vn -0.005755 -0.840864 -0.541216
+vn -0.393829 -0.423425 0.815849
+vn 0.855454 0.357218 -0.374959
+vn -0.535483 -0.042852 -0.843458
+vn 0.593436 -0.341238 0.728966
+vn -0.684303 -0.643863 0.342301
+vn -0.543719 -0.112982 0.831628
+vn -0.432322 -0.733068 0.525080
+f 1//1 2//2 4//4
+f 1//1 5//5 2//2
+f 1//1 6//6 2//2
+f 1//1 4//4 5//5
+f 1//1 4//4 8//8
+f 1//1 5//5 6//6
+f 1//1 8//8 5//5
+f 2//2 3//3 4//4
+f 2//2 7//7 3//3
+f 2//2 5//5 6//6
+f 2//2 6//6 7//7
+f 3//3 7//7 4//4
+f 3//3 8//8 4//4
+f 3//3 7//7 8//8
+f 4//4 8//8 5//5
+f 4//4 7//7 8//8
+f 5//5 8//8 6//6
+f 6//6 8//8 7//7

data/dinnerware/pan_tefal-collision04.obj

@@ -0,0 +1,36 @@
+mtllib dinnerware.mtl
+usemtl pan_tefal
+v -0.066236 -0.112453 0.075000
+v -0.070225 -0.121633 0.075000
+v -0.063600 -0.110158 0.000000
+v -0.060950 -0.105568 0.000000
+v -0.131811 -0.001125 0.075750
+v -0.141855 0.000000 0.075750
+v -0.128472 0.000000 0.000000
+v -0.123119 0.000000 0.000000
+vn -0.706733 -0.191066 0.681192
+vn 0.444900 -0.691793 0.568758
+vn 0.088502 0.609907 0.787515
+vn -0.441942 -0.815296 0.374139
+vn -0.595750 0.064829 0.800549
+vn -0.683770 -0.459760 -0.566639
+vn -0.132581 -0.607583 0.783112
+vn -0.220415 -0.970551 0.097201
+f 1//1 2//2 4//4
+f 1//1 5//5 2//2
+f 1//1 6//6 2//2
+f 1//1 4//4 5//5
+f 1//1 4//4 8//8
+f 1//1 5//5 6//6
+f 1//1 8//8 5//5
+f 2//2 3//3 4//4
+f 2//2 7//7 3//3
+f 2//2 5//5 6//6
+f 2//2 6//6 7//7
+f 3//3 7//7 4//4
+f 3//3 8//8 4//4
+f 3//3 7//7 8//8
+f 4//4 8//8 5//5
+f 4//4 7//7 8//8
+f 5//5 8//8 6//6
+f 6//6 8//8 7//7

data/dinnerware/pan_tefal-collision05.obj

@@ -0,0 +1,36 @@
+mtllib dinnerware.mtl
+usemtl pan_tefal
+v 0.064269 -0.113589 0.075000
+v 0.070225 -0.121633 0.075000
+v 0.063600 -0.110158 0.000000
+v 0.060950 -0.105568 0.000000
+v -0.064932 -0.114714 0.075750
+v -0.070927 -0.122850 0.075750
+v -0.064236 -0.111260 0.000000
+v -0.061560 -0.106624 0.000000
+vn 0.337731 -0.812635 0.474934
+vn -0.151863 -0.910103 0.385551
+vn -0.786673 0.591186 -0.177887
+vn 0.661692 0.712532 0.233371
+vn -0.904799 0.159708 -0.394757
+vn 0.641963 -0.023667 -0.766370
+vn -0.472889 -0.878846 0.063294
+vn 0.722857 0.565784 -0.396694
+f 1//1 2//2 4//4
+f 1//1 5//5 2//2
+f 1//1 6//6 2//2
+f 1//1 4//4 5//5
+f 1//1 4//4 8//8
+f 1//1 5//5 6//6
+f 1//1 8//8 5//5
+f 2//2 3//3 4//4
+f 2//2 7//7 3//3
+f 2//2 5//5 6//6
+f 2//2 6//6 7//7
+f 3//3 7//7 4//4
+f 3//3 8//8 4//4
+f 3//3 7//7 8//8
+f 4//4 8//8 5//5
+f 4//4 7//7 8//8
+f 5//5 8//8 6//6
+f 6//6 8//8 7//7

data/dinnerware/pan_tefal-collision06.obj

@@ -0,0 +1,36 @@
+mtllib dinnerware.mtl
+usemtl pan_tefal
+v 0.130506 -0.001136 0.075000
+v 0.140450 -0.000000 0.075000
+v 0.127200 -0.000000 0.000000
+v 0.121900 -0.000000 0.000000
+v 0.066879 -0.113589 0.075750
+v 0.070927 -0.122850 0.075750
+v 0.064236 -0.111260 0.000000
+v 0.061560 -0.106624 0.000000
+vn -0.793944 -0.586742 -0.159329
+vn 0.466324 0.641735 -0.608867
+vn -0.911102 -0.220305 -0.348365
+vn -0.616532 -0.769008 0.168863
+vn -0.642390 0.561142 0.521973
+vn -0.527497 0.482140 0.699491
+vn -0.264570 0.491705 0.829596
+vn -0.595670 0.662065 0.454804
+f 1//1 2//2 4//4
+f 1//1 5//5 2//2
+f 1//1 6//6 2//2
+f 1//1 4//4 5//5
+f 1//1 4//4 8//8
+f 1//1 5//5 6//6
+f 1//1 8//8 5//5
+f 2//2 3//3 4//4
+f 2//2 7//7 3//3
+f 2//2 5//5 6//6
+f 2//2 6//6 7//7
+f 3//3 7//7 4//4
+f 3//3 8//8 4//4
+f 3//3 7//7 8//8
+f 4//4 8//8 5//5
+f 4//4 7//7 8//8
+f 5//5 8//8 6//6
+f 6//6 8//8 7//7

data/dinnerware/pan_tefal.urdf

@@ -0,0 +1,84 @@
+<?xml version="1.0"?>
+<robot name="cube">
+  <!-- Frying pan model, Copyright (c) 2016 Oleg Klimov       -->
+  <!-- LICENSE: CC-SA                                         -->
+  <link name="cube">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="pan_tefal.obj"/>
+      </geometry>
+    </visual>
+    <visual>  <!-- handle -->
+      <geometry>
+        <cylinder radius="0.011" length="0.185"/>
+      </geometry>
+      <origin rpy="0 1.4 0" xyz="0.22 0 0.07"/>
+      <material name="stainless">
+        <color rgba="0.3 0.3 .3 1"/>
+      </material>
+    </visual>
+
+    <collision>  <!-- handle -->
+      <geometry>
+        <cylinder radius="0.011" length="0.185"/>
+      </geometry>
+      <origin rpy="0 1.4 0" xyz="0.22 0 0.07"/>
+    </collision>
+    <collision>  <!-- bottom -->
+      <geometry>
+        <cylinder radius="0.12" length="0.01"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0.005"/>
+    </collision>
+
+    <collision>
+      <geometry>
+        <mesh filename="pan_tefal-collision01.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+
+    <collision>
+      <geometry>
+        <mesh filename="pan_tefal-collision02.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+
+    <collision>
+      <geometry>
+        <mesh filename="pan_tefal-collision03.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+
+    <collision>
+      <geometry>
+        <mesh filename="pan_tefal-collision04.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+
+    <collision>
+      <geometry>
+        <mesh filename="pan_tefal-collision05.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+
+    <collision>
+      <geometry>
+        <mesh filename="pan_tefal-collision06.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+
+    <inertial>
+      <mass value="2."/>  <!-- mr^2 = 0.5*0.12^2 = 0.0072 for Z axis, lighter along other axis -->
+      <inertia ixx="0.005" iyy="0.005" izz="0.0072"/>
+    </inertial>
+  </link>
+
+</robot>
+

data/dinnerware/plate-collision01.obj

@@ -0,0 +1,34 @@
+mtllib dinnerware.mtl
+usemtl pan_tefal
+v 0.049745 0.084455 0.060000
+v 0.051940 0.089963 0.060000
+v 0.029150 0.050489 0.000000
+v 0.027560 0.047735 0.000000
+v 0.098993 0.000845 0.060600
+v 0.104919 0.000000 0.060600
+v 0.058883 0.000000 0.000000
+v 0.055671 0.000000 0.000000
+vn -0.962255 -0.069189 0.263208
+vn 0.888900 -0.266051 0.372926
+vn -0.704117 -0.151605 -0.693711
+vn -0.096471 -0.952673 -0.288284
+vn 0.353510 0.535276 0.767144
+vn 0.049583 0.551162 -0.832924
+vn -0.080990 0.081838 -0.993349
+vn 0.250326 0.614585 -0.748080
+f 1//1 2//2 4//4
+f 1//1 5//5 2//2
+f 1//1 6//6 2//2
+f 1//1 4//4 5//5
+f 1//1 5//5 6//6
+f 2//2 3//3 4//4
+f 2//2 7//7 3//3
+f 2//2 5//5 6//6
+f 2//2 6//6 7//7
+f 3//3 7//7 4//4
+f 3//3 8//8 4//4
+f 3//3 7//7 8//8
+f 4//4 8//8 5//5
+f 4//4 7//7 8//8
+f 5//5 8//8 6//6
+f 6//6 8//8 7//7

data/dinnerware/plate-collision02.obj

@@ -0,0 +1,34 @@
+mtllib dinnerware.mtl
+usemtl pan_tefal
+v -0.048268 0.085308 0.060000
+v -0.051940 0.089963 0.060000
+v -0.029150 0.050489 0.000000
+v -0.027560 0.047735 0.000000
+v 0.048765 0.086152 0.060600
+v 0.052459 0.090862 0.060600
+v 0.029442 0.050994 0.000000
+v 0.027836 0.048213 0.000000
+vn -0.471567 -0.127056 -0.872629
+vn 0.616670 0.313448 -0.722127
+vn 0.434277 -0.589449 -0.681141
+vn -0.696898 0.673938 -0.245235
+vn -0.964280 0.142499 -0.223288
+vn -0.118273 0.245678 -0.962109
+vn 0.333505 0.310546 -0.890133
+vn 0.592935 -0.751851 -0.288355
+f 1//1 2//2 4//4
+f 1//1 5//5 2//2
+f 1//1 6//6 2//2
+f 1//1 4//4 5//5
+f 1//1 5//5 6//6
+f 2//2 3//3 4//4
+f 2//2 7//7 3//3
+f 2//2 5//5 6//6
+f 2//2 6//6 7//7
+f 3//3 7//7 4//4
+f 3//3 8//8 4//4
+f 3//3 7//7 8//8
+f 4//4 8//8 5//5
+f 4//4 7//7 8//8
+f 5//5 8//8 6//6
+f 6//6 8//8 7//7

data/dinnerware/plate-collision03.obj

@@ -0,0 +1,34 @@
+mtllib dinnerware.mtl
+usemtl pan_tefal
+v -0.098012 0.000853 0.060000
+v -0.103880 0.000000 0.060000
+v -0.058300 0.000000 0.000000
+v -0.055120 0.000000 0.000000
+v -0.050228 0.085308 0.060600
+v -0.052459 0.090862 0.060600
+v -0.029441 0.050994 0.000000
+v -0.027836 0.048213 0.000000
+vn -0.570326 0.565919 -0.595368
+vn -0.987481 0.122113 -0.099846
+vn 0.256877 0.509622 0.821157
+vn -0.149510 -0.272737 0.950401
+vn 0.250981 -0.885212 0.391675
+vn 0.623883 0.757497 -0.192269
+vn 0.116527 -0.876135 -0.467769
+vn -0.474452 0.562082 -0.677466
+f 1//1 2//2 4//4
+f 1//1 5//5 2//2
+f 1//1 6//6 2//2
+f 1//1 4//4 5//5
+f 1//1 5//5 6//6
+f 2//2 3//3 4//4
+f 2//2 7//7 3//3
+f 2//2 5//5 6//6
+f 2//2 6//6 7//7
+f 3//3 7//7 4//4
+f 3//3 8//8 4//4
+f 3//3 7//7 8//8
+f 4//4 8//8 5//5
+f 4//4 7//7 8//8
+f 5//5 8//8 6//6
+f 6//6 8//8 7//7

data/dinnerware/plate-collision04.obj

@@ -0,0 +1,34 @@
+mtllib dinnerware.mtl
+usemtl pan_tefal
+v -0.049745 -0.084455 0.060000
+v -0.051940 -0.089963 0.060000
+v -0.029150 -0.050489 0.000000
+v -0.027560 -0.047735 0.000000
+v -0.098993 -0.000845 0.060600
+v -0.104919 0.000000 0.060600
+v -0.058883 0.000000 0.000000
+v -0.055671 0.000000 0.000000
+vn -0.454516 0.625342 -0.634321
+vn -0.518286 -0.578386 0.629959
+vn -0.173259 0.226757 -0.958417
+vn -0.247982 -0.510538 -0.823320
+vn -0.417030 0.446958 0.791400
+vn -0.004783 -0.717725 -0.696310
+vn 0.273713 0.103751 0.956199
+vn 0.429490 0.309534 -0.848367
+f 1//1 2//2 4//4
+f 1//1 5//5 2//2
+f 1//1 6//6 2//2
+f 1//1 4//4 5//5
+f 1//1 5//5 6//6
+f 2//2 3//3 4//4
+f 2//2 7//7 3//3
+f 2//2 5//5 6//6
+f 2//2 6//6 7//7
+f 3//3 7//7 4//4
+f 3//3 8//8 4//4
+f 3//3 7//7 8//8
+f 4//4 8//8 5//5
+f 4//4 7//7 8//8
+f 5//5 8//8 6//6
+f 6//6 8//8 7//7

data/dinnerware/plate-collision05.obj

@@ -0,0 +1,34 @@
+mtllib dinnerware.mtl
+usemtl pan_tefal
+v 0.048268 -0.085308 0.060000
+v 0.051940 -0.089963 0.060000
+v 0.029150 -0.050489 0.000000
+v 0.027560 -0.047735 0.000000
+v -0.048765 -0.086152 0.060600
+v -0.052459 -0.090862 0.060600
+v -0.029442 -0.050994 0.000000
+v -0.027836 -0.048213 0.000000
+vn 0.404852 0.441710 0.800617
+vn -0.882548 -0.029017 0.469326
+vn 0.182767 0.981935 0.048984
+vn -0.768458 0.392626 -0.505290
+vn 0.592713 0.805382 -0.007215
+vn 0.400028 0.869068 0.291031
+vn -0.642772 0.691166 0.330355
+vn 0.104670 0.652086 -0.750885
+f 1//1 2//2 4//4
+f 1//1 5//5 2//2
+f 1//1 6//6 2//2
+f 1//1 4//4 5//5
+f 1//1 5//5 6//6
+f 2//2 3//3 4//4
+f 2//2 7//7 3//3
+f 2//2 5//5 6//6
+f 2//2 6//6 7//7
+f 3//3 7//7 4//4
+f 3//3 8//8 4//4
+f 3//3 7//7 8//8
+f 4//4 8//8 5//5
+f 4//4 7//7 8//8
+f 5//5 8//8 6//6
+f 6//6 8//8 7//7

data/dinnerware/plate-collision06.obj

@@ -0,0 +1,34 @@
+mtllib dinnerware.mtl
+usemtl pan_tefal
+v 0.098012 -0.000853 0.060000
+v 0.103880 -0.000000 0.060000
+v 0.058300 -0.000000 0.000000
+v 0.055120 -0.000000 0.000000
+v 0.050228 -0.085308 0.060600
+v 0.052459 -0.090862 0.060600
+v 0.029442 -0.050994 0.000000
+v 0.027836 -0.048213 0.000000
+vn 0.692885 0.707963 -0.136739
+vn 0.408978 0.907585 0.095006
+vn -0.212286 -0.468709 0.857465
+vn 0.297056 -0.618852 0.727172
+vn 0.708170 0.492350 -0.506050
+vn 0.745762 -0.289343 -0.600100
+vn 0.808951 0.000863 -0.587876
+vn 0.926527 0.350079 -0.137811
+f 1//1 2//2 4//4
+f 1//1 5//5 2//2
+f 1//1 6//6 2//2
+f 1//1 4//4 5//5
+f 1//1 5//5 6//6
+f 2//2 3//3 4//4
+f 2//2 7//7 3//3
+f 2//2 5//5 6//6
+f 2//2 6//6 7//7
+f 3//3 7//7 4//4
+f 3//3 8//8 4//4
+f 3//3 7//7 8//8
+f 4//4 8//8 5//5
+f 4//4 7//7 8//8
+f 5//5 8//8 6//6
+f 6//6 8//8 7//7

data/dinnerware/plate.urdf

@@ -0,0 +1,69 @@
+<?xml version="1.0"?>
+<robot name="cube">
+  <!-- Frying pan model, Copyright (c) 2016 Oleg Klimov       -->
+  <!-- LICENSE: CC-SA                                         -->
+  <link name="cube">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="plate.obj"/>
+      </geometry>
+    </visual>
+
+    <collision>  <!-- bottom -->
+      <geometry>
+        <cylinder radius="0.05" length="0.005"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0.0025"/>
+    </collision>
+
+    <collision>
+      <geometry>
+        <mesh filename="plate-collision01.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+
+    <collision>
+      <geometry>
+        <mesh filename="plate-collision02.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+
+    <collision>
+      <geometry>
+        <mesh filename="plate-collision03.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+
+    <collision>
+      <geometry>
+        <mesh filename="plate-collision04.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+
+    <collision>
+      <geometry>
+        <mesh filename="plate-collision05.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+
+    <collision>
+      <geometry>
+        <mesh filename="plate-collision06.obj"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+
+    <inertial>
+      <mass value="0.5"/>  <!-- mr^2 = 0.5*0.12^2 = 0.0072 for Z axis, lighter along other axis -->
+      <inertia ixx="0.005" iyy="0.005" izz="0.0072"/>
+    </inertial>
+  </link>
+
+</robot>
+

data/gripper/wsg50_one_motor_gripper_new.sdf

@@ -303,6 +303,10 @@
     </joint>
     
     <link name='finger_right'>
+      <contact>
+        <spinning_friction>.3</spinning_friction>
+        <rolling_friction>0.04</rolling_friction>
+      </contact>
       <pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
       <inertial>
         <mass>0.2</mass>
@@ -343,6 +347,10 @@
     </joint>
     
     <link name='finger_left'>
+      <contact>
+        <spinning_friction>.3</spinning_friction>
+        <rolling_friction>0.04</rolling_friction>
+      </contact>
       <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
       <inertial>
         <mass>0.2</mass>

data/sphere_1cm.urdf

@@ -0,0 +1,22 @@
+<?xml version="0.0" ?>
+<robot name="urdf_robot">
+  <link name="base_link">
+    <contact>
+      <rolling_friction value="0.001"/>
+      <spinning_friction value="0.001"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

data/tray/tray_textured.mtl

@@ -0,0 +1,13 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0.000000
+Ka 0.000000 0.000000 0.000000
+Kd 0.800000 0.800000 0.800000
+Ks 0.800000 0.800000 0.800000
+Ke 0.000000 0.000000 0.000000
+Ni 1.000000
+d 1.000000
+illum 2
+map_Kd tray.jpg

data/tray/tray_textured.obj

@@ -0,0 +1,213 @@
+# Blender v2.78 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib tray_textured.mtl
+o edge_4_Cube.001
+v -0.573309 0.580000 0.261247
+v -0.426691 0.419400 -0.002214
+v -0.590083 0.580000 0.250354
+v -0.573309 -0.580000 0.261247
+v -0.409917 0.419400 0.008679
+v -0.590083 -0.580000 0.250354
+v -0.409917 -0.419400 0.009162
+v -0.426691 -0.419400 -0.001731
+vt 0.9046 0.2397
+vt 0.7929 0.2434
+vt 0.9174 0.2393
+vt 0.9537 0.7559
+vt 0.7801 0.2438
+vt 0.9664 0.7554
+vt 0.8291 0.7599
+vt 0.8419 0.7595
+vn 0.2565 0.8821 -0.3950
+vn 0.8392 0.0002 0.5438
+vn 0.8395 0.0000 0.5433
+vn 0.8396 -0.0000 0.5432
+vn 0.2568 -0.8819 -0.3954
+vn -0.8396 -0.0002 -0.5433
+vn -0.8392 -0.0000 -0.5438
+vn -0.8391 0.0000 -0.5439
+vn 0.5446 -0.0005 -0.8387
+vn -0.5446 -0.0000 0.8387
+vn 0.8391 0.0003 0.5440
+vn -0.8397 -0.0003 -0.5430
+usemtl None
+s 1
+f 1/1/1 2/2/1 3/3/1
+f 4/4/2 5/5/3 1/1/4
+f 6/6/5 7/7/5 4/4/5
+f 3/3/6 8/8/7 6/6/8
+f 5/5/9 8/8/9 2/2/9
+f 4/4/10 3/3/10 6/6/10
+f 1/1/1 5/5/1 2/2/1
+f 4/4/2 7/7/11 5/5/3
+f 6/6/5 8/8/5 7/7/5
+f 3/3/6 2/2/12 8/8/7
+f 5/5/9 7/7/9 8/8/9
+f 4/4/10 1/1/10 3/3/10
+o edge_1_Cube.003
+v 0.580000 0.590083 0.250354
+v -0.419960 0.426691 -0.001860
+v -0.580000 0.590083 0.250354
+v 0.580000 0.573309 0.261247
+v 0.420014 0.426691 -0.001059
+v -0.580000 0.573309 0.261247
+v 0.420014 0.409917 0.009834
+v -0.419960 0.409917 0.009033
+vt 0.8346 0.9187
+vt 0.2203 0.8574
+vt 0.1480 0.9187
+vt 0.8346 0.9129
+vt 0.7623 0.8574
+vt 0.1480 0.9129
+vt 0.7623 0.8511
+vt 0.2203 0.8511
+vn 0.0004 0.8386 -0.5448
+vn 0.0001 0.8391 -0.5439
+vn 0.0000 0.8393 -0.5437
+vn 0.8823 -0.2564 -0.3948
+vn -0.0004 -0.8392 0.5439
+vn -0.0001 -0.8386 0.5447
+vn 0.0000 -0.8385 0.5449
+vn -0.8826 -0.2560 -0.3942
+vn 0.0008 -0.5446 -0.8387
+vn 0.0000 0.5446 0.8387
+vn 0.0005 0.8383 -0.5452
+vn -0.0005 -0.8394 0.5435
+usemtl None
+s 1
+f 9/9/13 10/10/14 11/11/15
+f 12/12/16 13/13/16 9/9/16
+f 14/14/17 15/15/18 12/12/19
+f 11/11/20 16/16/20 14/14/20
+f 13/13/21 16/16/21 10/10/21
+f 12/12/22 11/11/22 14/14/22
+f 9/9/13 13/13/23 10/10/14
+f 12/12/16 15/15/16 13/13/16
+f 14/14/17 16/16/24 15/15/18
+f 11/11/20 10/10/20 16/16/20
+f 13/13/21 15/15/21 16/16/21
+f 12/12/22 9/9/22 11/11/22
+o edge_2_Cube
+v 0.590083 0.580000 0.250354
+v 0.409917 0.420060 0.009390
+v 0.573309 0.580000 0.261247
+v 0.590083 -0.580000 0.250354
+v 0.426691 0.420060 -0.001503
+v 0.573309 -0.580000 0.261247
+v 0.426691 -0.419158 -0.002053
+v 0.409917 -0.419158 0.008840
+vt 0.9410 0.8520
+vt 0.7523 0.8566
+vt 0.9234 0.8524
+vt 0.8896 0.1426
+vt 0.7698 0.8562
+vt 0.8721 0.1430
+vt 0.7185 0.1468
+vt 0.7009 0.1472
+vn -0.2561 0.8826 -0.3943
+vn 0.8394 0.0003 -0.5435
+vn 0.8390 0.0001 -0.5441
+vn 0.8389 0.0000 -0.5443
+vn -0.2569 -0.8818 -0.3956
+vn -0.8390 -0.0003 0.5441
+vn -0.8394 -0.0001 0.5436
+vn -0.8395 -0.0000 0.5434
+vn -0.5446 0.0005 -0.8387
+vn 0.5446 -0.0000 0.8387
+vn 0.8396 0.0004 -0.5433
+vn -0.8388 -0.0004 0.5444
+usemtl None
+s 1
+f 17/17/25 18/18/25 19/19/25
+f 20/20/26 21/21/27 17/17/28
+f 22/22/29 23/23/29 20/20/29
+f 19/19/30 24/24/31 22/22/32
+f 21/21/33 24/24/33 18/18/33
+f 20/20/34 19/19/34 22/22/34
+f 17/17/25 21/21/25 18/18/25
+f 20/20/26 23/23/35 21/21/27
+f 22/22/29 24/24/29 23/23/29
+f 19/19/30 18/18/36 24/24/31
+f 21/21/33 23/23/33 24/24/33
+f 20/20/34 17/17/34 19/19/34
+o base_Cube.004
+v 0.420000 0.420000 0.010000
+v -0.420000 0.420000 -0.010000
+v -0.420000 0.420000 0.010000
+v 0.420000 -0.420000 0.010000
+v 0.420000 0.420000 -0.010000
+v -0.420000 -0.420000 0.010000
+v 0.420000 -0.420000 -0.010000
+v -0.420000 -0.420000 -0.010000
+vt 0.7524 0.8072
+vt -0.3038 0.8371
+vt -0.3038 0.8371
+vt 0.7012 0.1905
+vt 0.7524 0.8072
+vt -0.3550 0.2204
+vt 0.7012 0.1905
+vt -0.3550 0.2204
+vn 0.0000 1.0000 0.0000
+vn 1.0000 -0.0000 0.0000
+vn 0.0000 -1.0000 0.0000
+vn -1.0000 -0.0000 0.0000
+vn 0.0000 0.0000 -1.0000
+vn 0.0000 -0.0000 1.0000
+usemtl None
+s 1
+f 25/25/37 26/26/37 27/27/37
+f 28/28/38 29/29/38 25/25/38
+f 30/30/39 31/31/39 28/28/39
+f 27/27/40 32/32/40 30/30/40
+f 29/29/41 32/32/41 26/26/41
+f 28/28/42 27/27/42 30/30/42
+f 25/25/37 29/29/37 26/26/37
+f 28/28/38 31/31/38 29/29/38
+f 30/30/39 32/32/39 31/31/39
+f 27/27/40 26/26/40 32/32/40
+f 29/29/41 31/31/41 32/32/41
+f 28/28/42 25/25/42 27/27/42
+o edge_3_Cube.002
+v 0.580000 -0.573309 0.261247
+v -0.419400 -0.409917 0.008678
+v -0.580000 -0.573309 0.261247
+v 0.580000 -0.590083 0.250354
+v 0.419883 -0.409917 0.009162
+v -0.580000 -0.590083 0.250354
+v 0.419883 -0.426691 -0.001731
+v -0.419400 -0.426691 -0.002215
+vt 0.8690 0.1040
+vt 0.1365 0.1739
+vt 0.0188 0.1040
+vt 0.8690 0.0968
+vt 0.7517 0.1739
+vt 0.0188 0.0968
+vt 0.7517 0.1668
+vt 0.1365 0.1668
+vn -0.0002 0.8392 0.5438
+vn -0.0000 0.8395 0.5433
+vn 0.0000 0.8396 0.5432
+vn 0.8825 0.2562 -0.3945
+vn 0.0002 -0.8396 -0.5433
+vn 0.0000 -0.8392 -0.5438
+vn 0.0000 -0.8391 -0.5439
+vn -0.8821 0.2565 -0.3950
+vn -0.8822 0.2565 -0.3950
+vn 0.0005 0.5446 -0.8387
+vn 0.0000 -0.5446 0.8387
+vn -0.0003 0.8391 0.5440
+vn 0.0003 -0.8397 -0.5430
+usemtl None
+s 1
+f 33/33/43 34/34/44 35/35/45
+f 36/36/46 37/37/46 33/33/46
+f 38/38/47 39/39/48 36/36/49
+f 35/35/50 40/40/51 38/38/51
+f 37/37/52 40/40/52 34/34/52
+f 36/36/53 35/35/53 38/38/53
+f 33/33/43 37/37/54 34/34/44
+f 36/36/46 39/39/46 37/37/46
+f 38/38/47 40/40/55 39/39/48
+f 35/35/50 34/34/51 40/40/51
+f 37/37/52 39/39/52 40/40/52
+f 36/36/53 33/33/53 35/35/53

data/tray/traybox.urdf

@@ -0,0 +1,49 @@
+<robot name="tabletop">
+  <link name="base_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="tray_textured.obj" scale="0.5 0.5 0.5"/>
+      </geometry>
+      <material name="tray_material">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+     <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.005"/>
+      <geometry>
+	 			<box size=".6 .6 .02"/>
+      </geometry>
+    </collision>
+     <collision>
+      <origin rpy="0 0.575469961 0" xyz="0.25 0 0.059"/>
+      <geometry>
+	 			<box size=".02 .6 .15"/>
+      </geometry>
+    </collision>
+		<collision>
+      <origin rpy="0 -0.575469961 0" xyz="-0.25 0 0.059"/>
+      <geometry>
+	 			<box size=".02 .6 .15"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0.575469961 0 0" xyz="0 -0.25 0.059"/>
+      <geometry>
+	 			<box size=".6 .02 .15"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="-0.575469961 0 0" xyz="0 0.25 0.059"/>
+      <geometry>
+	 			<box size=".6 .02 .15"/>
+      </geometry>
+    </collision>
+
+  </link>
+</robot>

examples/CommonInterfaces/CommonCameraInterface.h

@@ -36,6 +36,9 @@ struct CommonCameraInterface
 
 	virtual void	setAspectRatio(float ratio) = 0;
 	virtual float	getAspectRatio() const = 0;
+    
+    virtual float getCameraFrustumFar() const = 0;
+    virtual float getCameraFrustumNear() const = 0;
 };
 
 #endif //COMMON_CAMERA_INTERFACE_H

examples/CommonInterfaces/CommonExampleInterface.h

@@ -37,6 +37,7 @@ public:
 	
 	virtual void    initPhysics()=0;
 	virtual void    exitPhysics()=0;
+	virtual void	updateGraphics(){}
 	virtual void	stepSimulation(float deltaTime)=0;
 	virtual void	renderScene()=0;
 	virtual void	physicsDebugDraw(int debugFlags)=0;//for now we reuse the flags in Bullet/src/LinearMath/btIDebugDraw.h

examples/ExampleBrowser/InProcessExampleBrowser.cpp

@@ -266,6 +266,7 @@ void	ExampleBrowserThreadFunc(void* userPtr,void* lsMemory)
 				{
 					B3_PROFILE("clock.usleep");
 					clock.usleep(gMinUpdateTimeMicroSecs/10.);
+					exampleBrowser->updateGraphics();
 				} else
 				{
 					B3_PROFILE("exampleBrowser->update");

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -309,8 +309,11 @@ static void MyMouseMoveCallback( float x, float y)
   	bool handled = false;
 	if (sCurrentDemo)
 		handled = sCurrentDemo->mouseMoveCallback(x,y);
-	if (!handled && gui2)
-		handled = gui2->mouseMoveCallback(x,y);
+	if (renderGui)
+	{
+		if (!handled && gui2)
+			handled = gui2->mouseMoveCallback(x,y);
+	}
 	if (!handled)
 	{
 		if (prevMouseMoveCallback)
@@ -327,9 +330,11 @@ static void MyMouseButtonCallback(int button, int state, float x, float y)
 	if (sCurrentDemo)
 		handled = sCurrentDemo->mouseButtonCallback(button,state,x,y);
 
-	if (!handled && gui2)
-		handled = gui2->mouseButtonCallback(button,state,x,y);
-
+	if (renderGui)
+	{
+		if (!handled && gui2)
+			handled = gui2->mouseButtonCallback(button,state,x,y);
+	}
 	if (!handled)
 	{
 		if (prevMouseButtonCallback )
@@ -1125,6 +1130,18 @@ bool OpenGLExampleBrowser::requestedExit()
 	return s_window->requestedExit();
 }
 
+void OpenGLExampleBrowser::updateGraphics()
+{
+	if (sCurrentDemo)
+	{
+			if (!pauseSimulation || singleStepSimulation)
+			{
+				B3_PROFILE("sCurrentDemo->updateGraphics");
+				sCurrentDemo->updateGraphics();
+			}
+	}
+}
+
 void OpenGLExampleBrowser::update(float deltaTime)
 {
 	b3ChromeUtilsEnableProfiling();

examples/ExampleBrowser/OpenGLExampleBrowser.h

@@ -19,6 +19,8 @@ public:
 
 	virtual void update(float deltaTime);
 
+	virtual void updateGraphics();
+
 	virtual bool requestedExit();
 
 	virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -437,10 +437,15 @@ void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const floa
                         int bytesPerPixel = 4; //RGBA
                         
                         int sourcePixelIndex = (xIndex+yIndex*sourceWidth)*bytesPerPixel;
+                        int sourceDepthIndex = xIndex+yIndex*sourceWidth;
+                        
                         m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+0] = sourceRgbaPixelBuffer[sourcePixelIndex+0];
                         m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+1] = sourceRgbaPixelBuffer[sourcePixelIndex+1];
                         m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+2] = sourceRgbaPixelBuffer[sourcePixelIndex+2];
                         m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+3] = 255;
+                        
+                        m_data->m_depthBuffer1[i+j*destinationWidth] = sourceDepthBuffer[sourceDepthIndex];
+                        
                     }
                 }
             }
@@ -456,7 +461,7 @@ void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const floa
         {
             for (int i=0;i<numRequestedPixels;i++)
             {
-                depthBuffer[i] = m_data->m_depthBuffer1[i];
+                depthBuffer[i] = m_data->m_depthBuffer1[i+startPixelIndex];
             }
         }
 		if (numPixelsCopied)

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -71,7 +71,7 @@ struct URDF2BulletCachedData
     }
 
 
-    void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
+    void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
     {
         m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
     }
@@ -81,7 +81,7 @@ struct URDF2BulletCachedData
         return m_urdfLink2rigidBodies[urdfLinkIndex];
     }
 
-    void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
+    void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
     {
         btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
 
@@ -250,7 +250,12 @@ void ConvertURDF2BulletInternal(
     
     
 
-    btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
+    btCompoundShape* tmpShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
+	btCollisionShape* compoundShape = tmpShape;
+	if (tmpShape->getNumChildShapes() == 1 && tmpShape->getChildTransform(0)==btTransform::getIdentity())
+	{
+		compoundShape = tmpShape->getChildShape(0);
+	}
 	
 	int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
 	

examples/OpenGLWindow/SimpleCamera.cpp

@@ -383,3 +383,13 @@ float	SimpleCamera::getAspectRatio() const
 {
 	return m_data->m_aspect;
 }
+
+float SimpleCamera::getCameraFrustumFar() const
+{
+    return m_data->m_frustumZFar;
+}
+
+float SimpleCamera::getCameraFrustumNear() const
+{
+    return m_data->m_frustumZNear;
+}

examples/OpenGLWindow/SimpleCamera.h

@@ -47,6 +47,9 @@ struct SimpleCamera : public CommonCameraInterface
 
 	virtual void	setAspectRatio(float ratio);
 	virtual float	getAspectRatio() const;
+    
+    virtual float getCameraFrustumFar() const;
+    virtual float getCameraFrustumNear() const;
 };
 
-#endif //SIMPLE_CAMERA_H
+#endif //SIMPLE_CAMERA_H

examples/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -787,7 +787,7 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
 
 #ifdef _WIN32
 	sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
-		"-threads 0 -y -b 50000k  -t 20 -c:v libx264 -preset slow -crf 22 -an   -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
+		"-threads 0 -y -b:v 50000k  -t 20 -c:v libx264 -preset slow -crf 22 -an   -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
 
     //sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba   -s %dx%d -i - "
     //		"-y -crf 0  -b:v 1500000 -an -vcodec h264 -vf vflip  %s", width, height, mp4FileName);

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -16,8 +16,8 @@
 #include "b3RobotSimAPI.h"
 #include "../Utils/b3Clock.h"
 
-static btScalar sGripperVerticalVelocity = -0.2f;
-static btScalar sGripperClosingTargetVelocity = 0.5f;
+static btScalar sGripperVerticalVelocity = 0.f;
+static btScalar sGripperClosingTargetVelocity = -0.7f;
 
 class GripperGraspExample : public CommonExampleInterface
 {
@@ -226,9 +226,9 @@ public:
             {
                 b3RobotSimLoadFileArgs args("");
                 b3RobotSimLoadFileResults results;
-                args.m_fileName = "sphere_small.urdf";
-                args.m_startPosition.setValue(0, 0, .107);
-                args.m_startOrientation.setEulerZYX(0, 0, 0);
+                args.m_fileName = "dinnerware/pan_tefal.urdf";
+                args.m_startPosition.setValue(0, -0.2, .47);
+                args.m_startOrientation.setEulerZYX(SIMD_HALF_PI, 0, 0);
                 args.m_useMultiBody = true;
                 m_robotSim.loadFile(args, results);
             }
@@ -492,7 +492,7 @@ public:
             int fingerJointIndices[2]={0,1};
             double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
             };
-            double maxTorqueValues[2]={50.0,50.0};
+            double maxTorqueValues[2]={800.0,800.0};
             for (int i=0;i<2;i++)
             {
                 b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
@@ -550,8 +550,8 @@ public:
 	virtual void resetCamera()
 	{
 		float dist = 1.5;
-        float pitch = 12;
-        float yaw = -10;
+        float pitch = 18;
+        float yaw = 10;
 		float targetPos[3]={-0.2,0.8,0.3};
 		if (m_app->m_renderer  && m_app->m_renderer->getActiveCamera())
 		{

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -338,6 +338,16 @@ int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandl
 	return 0;
 }
 
+int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+
+	command->m_physSimParamArgs.m_contactBreakingThreshold = contactBreakingThreshold;
+	command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD;
+	return 0;
+}
+
 int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -191,7 +191,7 @@ int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHand
 	
 int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
 int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);
-
+int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold);
 
 //b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes
 //Use at own risk: magic things may or my not happen when calling this API

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -12,6 +12,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
 #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 #include "LinearMath/btHashMap.h"
 #include "BulletInverseDynamics/MultiBodyTree.hpp"
 #include "IKTrajectoryHelper.h"
@@ -798,7 +799,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
     m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
     
 	m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
-	m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
+	m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 150;
 	m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
 //	m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2;
 	//todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp)
@@ -2724,7 +2725,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					{
 						m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations;
 					}
-
+					if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD)
+					{
+						gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold;
+					}
 					if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
 					{
 						m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode;

examples/SharedMemory/PhysicsServerExample.cpp

@@ -299,7 +299,6 @@ void	MotionThreadFunc(void* userPtr,void* lsMemory)
 
 
 		double deltaTimeInSeconds = 0;
-		double sleepCounter = 0;
 		do
 		{
 			BT_PROFILE("loop");
@@ -310,27 +309,7 @@ void	MotionThreadFunc(void* userPtr,void* lsMemory)
 			}
 			double dt = double(clock.getTimeMicroseconds())/1000000.;
 			clock.reset();
-
-			sleepCounter+=dt;
-
-			if (sleepCounter > sleepTimeThreshold)
-			{
-				BT_PROFILE("usleep(100)");
-				sleepCounter = 0;
-				b3Clock::usleep(100);
-
-			}
-
-			{
-				if (gEnableRealTimeSimVR)
-				{
-					BT_PROFILE("usleep(1000)");
-					b3Clock::usleep(1000);
-				}
-			}
 			deltaTimeInSeconds+= dt;
-		
-
 			
 			{
 				
@@ -574,7 +553,7 @@ public:
 		
 		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
 		{
-			b3Clock::usleep(100);
+			b3Clock::usleep(0);
 		}
 	}
 
@@ -961,6 +940,8 @@ public:
 
 	virtual void	stepSimulation(float deltaTime);
 
+	virtual void updateGraphics();
+
     void enableCommandLogging()
 	{
 		m_physicsServer.enableCommandLogging(true,"BulletPhysicsCommandLog.bin");
@@ -1363,39 +1344,8 @@ bool PhysicsServerExample::wantsTermination()
     return m_wantsShutdown;
 }
 
-
-
-void	PhysicsServerExample::stepSimulation(float deltaTime)
+void	PhysicsServerExample::updateGraphics()
 {
-	BT_PROFILE("PhysicsServerExample::stepSimulation");
-
-	//this->m_physicsServer.processClientCommands();
-
-	for (int i = m_multiThreadedHelper->m_userDebugLines.size()-1;i>=0;i--)
-	{
-		if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime)
-		{
-			m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime -= deltaTime;
-			if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime<=0)
-			{
-				m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1);
-				m_multiThreadedHelper->m_userDebugLines.pop_back();
-			}
-		}
-	}
-
-	for (int i = m_multiThreadedHelper->m_userDebugText.size()-1;i>=0;i--)
-	{
-		if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime)
-		{
-			m_multiThreadedHelper->m_userDebugText[i].m_lifeTime -= deltaTime;
-			if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime<=0)
-			{
-				m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1);
-				m_multiThreadedHelper->m_userDebugText.pop_back();
-			}
-		}
-	}
 	//check if any graphics related tasks are requested
 	
 	switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
@@ -1564,6 +1514,40 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 	}
 	
 
+}
+
+void	PhysicsServerExample::stepSimulation(float deltaTime)
+{
+	BT_PROFILE("PhysicsServerExample::stepSimulation");
+
+	//this->m_physicsServer.processClientCommands();
+
+	for (int i = m_multiThreadedHelper->m_userDebugLines.size()-1;i>=0;i--)
+	{
+		if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime)
+		{
+			m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime -= deltaTime;
+			if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime<=0)
+			{
+				m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1);
+				m_multiThreadedHelper->m_userDebugLines.pop_back();
+			}
+		}
+	}
+
+	for (int i = m_multiThreadedHelper->m_userDebugText.size()-1;i>=0;i--)
+	{
+		if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime)
+		{
+			m_multiThreadedHelper->m_userDebugText[i].m_lifeTime -= deltaTime;
+			if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime<=0)
+			{
+				m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1);
+				m_multiThreadedHelper->m_userDebugText.pop_back();
+			}
+		}
+	}
+	updateGraphics();
 
 
 	

examples/SharedMemory/SharedMemoryCommands.h

@@ -313,7 +313,8 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64,
 	SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE=128,
 	SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 256,
-	SIM_PARAM_UPDATE_COLLISION_FILTER_MODE=512
+	SIM_PARAM_UPDATE_COLLISION_FILTER_MODE=512,
+	SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1024,
 };
 
 enum EnumLoadBunnyUpdateFlags
@@ -340,6 +341,7 @@ struct SendPhysicsSimulationParameters
 	bool m_allowRealTimeSimulation;
 	int m_useSplitImpulse;
 	double m_splitImpulsePenetrationThreshold;
+	double m_contactBreakingThreshold;
 	int m_internalSimFlags;
 	double m_defaultContactERP;
 	int m_collisionFilterMode;

examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp

@@ -44,16 +44,13 @@ public:
             PhysicsClientSharedMemory::disconnectSharedMemory();
         }
     	unsigned long int ms = m_clock.getTimeMilliseconds();
-	if (ms>20)
-	{ 
-		m_clock.reset(); 
-        	btUpdateInProcessExampleBrowserMainThread(m_data);
-        } else
-	{
-		//b3Clock::usleep(100);
-	}
-	return PhysicsClientSharedMemory::processServerStatus();
-        
+		if (ms>20)
+		{ 
+			m_clock.reset(); 
+        		btUpdateInProcessExampleBrowserMainThread(m_data);
+		}
+		b3Clock::usleep(0);
+		return PhysicsClientSharedMemory::processServerStatus();
         
     }
     

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -958,7 +958,16 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels
         {
 			if (depthBuffer)
 			{
-				depthBuffer[i] = m_data->m_depthBuffer[i+startPixelIndex];
+                float distance = -m_data->m_depthBuffer[i+startPixelIndex];
+                float farPlane = m_data->m_camera.getCameraFrustumFar();
+                float nearPlane = m_data->m_camera.getCameraFrustumNear();
+                
+                btClamp(distance,nearPlane,farPlane);
+                
+                // the depth buffer value is between 0 and 1
+                float a = farPlane / (farPlane - nearPlane);
+                float b = farPlane * nearPlane / (nearPlane - farPlane);
+                depthBuffer[i] = a + b / distance;
 			}
 			if (segmentationMaskBuffer)
             {

examples/StandaloneMain/hellovr_opengl_main.cpp

@@ -135,6 +135,8 @@ public:
 	void SetupRenderModelForTrackedDevice( vr::TrackedDeviceIndex_t unTrackedDeviceIndex );
 	CGLRenderModel *FindOrLoadRenderModel( const char *pchRenderModelName );
 
+	SimpleOpenGL3App* getApp() { return m_app;}
+
 private: 
 	bool m_bDebugOpenGL;
 	bool m_bVerbose;
@@ -2260,7 +2262,7 @@ int main(int argc, char *argv[])
 		b3ChromeUtilsEnableProfiling();
 	}
 
-
+	
 #ifdef BT_USE_CUSTOM_PROFILER
 	b3SetCustomEnterProfileZoneFunc(dcEnter);
 	b3SetCustomLeaveProfileZoneFunc(dcLeave);
@@ -2287,6 +2289,11 @@ int main(int argc, char *argv[])
 
 	}
 
+	char* gVideoFileName = 0;
+    args.GetCmdLineArgument("mp4",gVideoFileName);
+    if (gVideoFileName)
+        pMainApplication->getApp()->dumpFramesToVideo(gVideoFileName);
+ 
 	//request disable VSYNC
 	typedef bool (APIENTRY *PFNWGLSWAPINTERVALFARPROC)(int);
 	PFNWGLSWAPINTERVALFARPROC wglSwapIntervalEXT = 0;

examples/Utils/RobotLoggingUtil.cpp

@@ -0,0 +1,253 @@
+#include "RobotLoggingUtil.h"
+#include <stdio.h>
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "../Importers/ImportURDFDemo/urdfStringSplit.h"
+
+
+static bool readLine(FILE* file, btAlignedObjectArray<char>& line)
+{
+	int c = 0;
+	for (c=fgetc(file);(c != EOF && c != '\n');c=fgetc(file))
+	{
+		line.push_back(c);
+	}
+	line.push_back(0);
+	return (c == EOF);
+}
+
+
+int readMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes, btAlignedObjectArray<MinitaurLogRecord>& logRecords, bool verbose)
+{
+
+	int retVal = 0;
+
+	FILE* f = fopen(fileName,"rb");
+	if (f)
+	{
+		if (verbose)
+		{
+			printf("Opened file %s\n", fileName);
+		}
+		btAlignedObjectArray<char> line0Buf;
+		bool eof = readLine(f,line0Buf);
+		btAlignedObjectArray<char> line1Buf;
+		eof |= readLine(f,line1Buf);
+		std::string line0 = &line0Buf[0];
+		structTypes = &line1Buf[0];
+		
+		btAlignedObjectArray<std::string> separators;
+		separators.push_back(",");
+		
+		urdfStringSplit(structNames,line0,separators);
+		if (verbose)
+		{
+			printf("Num Fields = %d\n",structNames.size());
+		}
+		btAssert(structTypes.size() == structNames.size());
+		if (structTypes.size() != structNames.size())
+		{
+			retVal = eCorruptHeader;
+		}
+		int numStructsRead  = 0;
+
+		if (structTypes.size() == structNames.size())
+		{
+			while (!eof)
+			{
+				unsigned char blaat[1024];
+				size_t s = fread(blaat,2,1,f);
+				if (s!=1)
+				{
+					eof=true;
+					retVal = eInvalidAABBAlignCheck;
+					break;
+				}
+				if ((blaat[0] != 0xaa) || (blaat[1] != 0xbb))
+				{
+					if (verbose)
+					{
+						printf("Expected 0xaa0xbb, terminating\n");
+					}
+				}
+
+				if (verbose)
+				{
+					printf("Reading structure %d\n",numStructsRead);
+				}
+				MinitaurLogRecord record;
+
+				for (int i=0;i<structNames.size();i++)
+				{
+					
+
+					switch (structTypes[i])
+					{
+					case 'I':
+					{
+						size_t s = fread(blaat,sizeof(int),1,f);
+						if (s != 1)
+						{
+							eof = true;
+							retVal = eCorruptValue;
+							break;
+
+						}
+						int v = *(int*)blaat;
+						if (s==1)
+						{
+							if (verbose)
+							{
+								printf("%s = %d\n",structNames[i].c_str(),v);
+							}
+							record.m_values.push_back(v);
+						}
+						break;
+					}
+					case 'f':
+					{
+						float v;
+						size_t s = fread(&v,sizeof(float),1,f);
+						if (s != 1)
+						{
+							eof = true;
+							break;
+						}
+
+						if (s==1)
+						{
+							if (verbose)
+							{
+								printf("%s = %f\n",structNames[i].c_str(),v);
+							}
+							record.m_values.push_back(v);
+						}
+						break;
+					}
+					case 'B':
+					{
+						char v;
+						size_t s = fread(&v,sizeof(char),1,f);
+						if (s != 1)
+						{
+							eof = true;
+							break;
+						}
+						if (s==1)
+						{
+							if (verbose)
+							{
+								printf("%s = %d\n",structNames[i].c_str(),v);
+							}
+							record.m_values.push_back(v);
+						}
+						break;
+					}
+					default:
+					{
+						if (verbose)
+						{
+							printf("Unknown type\n");
+						}
+						retVal = eUnknownType;
+						btAssert(0);
+					}
+					}
+					
+
+				}
+				logRecords.push_back(record);
+				numStructsRead++;
+			}
+			if (verbose)
+			{
+				printf("numStructsRead = %d\n",numStructsRead);
+			}
+			if (retVal==0)
+			{
+				retVal = numStructsRead;
+			}
+		}
+		
+		//read header and 
+	} else
+	{
+		if (verbose)
+		{
+			printf("Could not open file %s", fileName);
+		}
+		retVal = eMinitaurFileNotFound;
+	}
+	return retVal;
+}
+
+
+FILE* createMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes)
+{
+	FILE* f = fopen(fileName,"wb");
+	if (f)
+	{
+		for (int i=0;i<structNames.size();i++)
+		{
+			int len = strlen(structNames[i].c_str());
+			fwrite(structNames[i].c_str(),len,1,f);
+			if (i<structNames.size()-1)
+			{
+				fwrite(",",1,1,f);
+			}
+		}
+		int sz = sizeof("\n");
+		fwrite("\n",sz-1,1,f);
+		fwrite(structTypes.c_str(),strlen(structTypes.c_str()),1,f);
+		fwrite("\n",sz-1,1,f);
+
+	}
+	
+
+	return f;
+}
+
+void appendMinitaurLogData(FILE* f, std::string& structTypes, const MinitaurLogRecord& logData)
+{
+	if (f)
+	{
+		unsigned char buf[2] = {0xaa,0xbb};
+		fwrite(buf,2,1,f);
+		if (structTypes.length() == logData.m_values.size())
+		{
+			for (int i=0;i<logData.m_values.size();i++)
+			{
+				switch(structTypes[i])
+				{
+				case 'I':
+				{
+					fwrite(&logData.m_values[i].m_intVal,sizeof(int),1,f);
+					break;
+				}
+				case 'f':
+				{
+					fwrite(&logData.m_values[i].m_floatVal,sizeof(float),1,f);
+					break;
+				}
+				case 'B':
+				{
+					fwrite(&logData.m_values[i].m_charVal,sizeof(char),1,f);
+					break;
+				}
+				default:
+				{
+
+				}
+				}
+			}
+		}
+	}
+}
+
+void closeMinitaurLogFile(FILE* f)
+{
+	if (f)
+	{
+		fclose(f);
+	}
+}

examples/Utils/RobotLoggingUtil.h

@@ -0,0 +1,54 @@
+#ifndef ROBOT_LOGGING_UTIL_H
+#define ROBOT_LOGGING_UTIL_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include <string>
+
+struct MinitaurLogValue
+{
+	MinitaurLogValue()
+	:m_intVal(0xcdcdcdcd)
+	{
+	}
+	MinitaurLogValue(int iv)
+	:m_intVal(iv)
+	{
+	}
+	MinitaurLogValue(float fv)
+		:m_floatVal(fv)
+	{
+	}
+	MinitaurLogValue(char fv)
+		:m_charVal(fv)
+	{
+	}
+
+	union
+	{
+		char m_charVal;
+		int m_intVal;
+		float m_floatVal;
+	};
+};
+
+struct MinitaurLogRecord
+{
+	btAlignedObjectArray<MinitaurLogValue> m_values;
+};
+
+enum MINITAUR_LOG_ERROR
+{
+	eMinitaurFileNotFound = -1,
+	eCorruptHeader = -2,
+	eUnknownType = -3,
+	eCorruptValue = -4,
+	eInvalidAABBAlignCheck = -5,
+};
+
+int readMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes, btAlignedObjectArray<MinitaurLogRecord>& logRecords, bool verbose);
+
+FILE* createMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes);
+void appendMinitaurLogData(FILE* f, std::string& structTypes, const MinitaurLogRecord& logData);
+void closeMinitaurLogFile(FILE* f);
+
+#endif //ROBOT_LOGGING_UTIL_H

examples/pybullet/manyspheres.py

@@ -0,0 +1,25 @@
+import pybullet as p
+import time
+
+p.connect(p.GUI)
+p.loadURDF("plane.urdf",useMaximalCoordinates=True)
+p.loadURDF("tray/traybox.urdf",useMaximalCoordinates=True)
+
+gravXid = p.addUserDebugParameter("gravityX",-10,10,0)
+gravYid = p.addUserDebugParameter("gravityY",-10,10,0)
+gravZid = p.addUserDebugParameter("gravityZ",-10,10,-10)
+p.setPhysicsEngineParameter(numSolverIterations=10)
+p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
+for i in range (10):
+    for j in range (10):
+        for k in range (5):
+            ob = p.loadURDF("sphere_1cm.urdf",[0.02*i,0.02*j,0.2+0.02*k],useMaximalCoordinates=True)
+p.setGravity(0,0,-10)
+p.setRealTimeSimulation(1)
+while True:
+    gravX = p.readUserDebugParameter(gravXid)
+    gravY = p.readUserDebugParameter(gravYid)
+    gravZ = p.readUserDebugParameter(gravZid)
+    p.setGravity(gravX,gravY,gravZ)
+    time.sleep(0.01)
+

examples/pybullet/pybullet.c

@@ -583,14 +583,15 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 	double splitImpulsePenetrationThreshold = -1;
 	int numSubSteps = -1;
 	int collisionFilterMode = -1;
+	double contactBreakingThreshold = -1;
 	
 	b3PhysicsClientHandle sm = 0;
 
 	int physicsClientId = 0;
-	static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","collisionFilterMode", "physicsClientId", NULL };
+	static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","collisionFilterMode", "contactBreakingThreshold", "physicsClientId", NULL };
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiii", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps,
-									 &collisionFilterMode, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidi", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps,
+									 &collisionFilterMode, &contactBreakingThreshold, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -610,6 +611,7 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 		{
 			b3PhysicsParamSetNumSolverIterations(command, numSolverIterations);
 		}
+
 		if (collisionFilterMode>=0)
 		{
 			b3PhysicsParamSetCollisionFilterMode(command, collisionFilterMode);
@@ -630,6 +632,11 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 		{
 			b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, splitImpulsePenetrationThreshold);
 		}
+		if (contactBreakingThreshold>=0)
+		{
+			b3PhysicsParamSetContactBreakingThreshold(command,contactBreakingThreshold);
+		}
+
 
 		//ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
 

examples/pybullet/rollPitchYaw.py

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