Merge branch 'master' of https://github.com/erwincoumans/bullet3

Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.cpp

@@ -876,11 +876,10 @@ bool btBulletXmlWorldImporter::loadFile(const char* fileName)
 {
 	XMLDocument doc;
 
-	bool loadOkay = doc.LoadFile(fileName);
-	//dump_to_stdout(&doc,0);
 	
+	XMLError loadOkay = doc.LoadFile(fileName);
 	
-	if (loadOkay)
+	if (loadOkay==XML_SUCCESS)
 	{
 		if (get_int_attribute_by_name(doc.FirstChildElement()->ToElement(),"version", &m_fileVersion))
 		{

data/toys/concave_box.mtl

@@ -0,0 +1,11 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2
+map_Kd ../checker_grid.jpg

data/toys/concave_box.obj

@@ -0,0 +1,949 @@
+# Blender v2.78 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib concave_box.mtl
+o Cube_Cube.003
+v 0.252358 -0.051546 0.073254
+v 0.196823 -0.040500 0.026785
+v 0.133463 -0.027897 -0.007745
+v 0.064714 -0.014222 -0.029009
+v 0.237323 -0.101112 0.073254
+v 0.185009 -0.079443 0.026785
+v 0.125326 -0.054721 -0.007745
+v 0.060565 -0.027897 -0.029009
+v 0.212906 -0.146792 0.073254
+v 0.165826 -0.115334 0.026785
+v 0.112112 -0.079443 -0.007745
+v 0.053829 -0.040500 -0.029009
+v 0.180047 -0.186831 0.073254
+v 0.140008 -0.146792 0.026785
+v 0.094329 -0.101112 -0.007745
+v 0.044763 -0.051546 -0.029009
+v 0.140008 -0.219690 0.073254
+v 0.108550 -0.172609 0.026785
+v 0.072660 -0.118895 -0.007745
+v 0.033716 -0.060612 -0.029009
+v -0.006783 -0.000000 -0.036189
+v 0.094329 -0.244106 0.073254
+v 0.072660 -0.191793 0.026785
+v 0.047938 -0.132109 -0.007745
+v 0.021113 -0.067349 -0.029009
+v 0.044763 -0.259142 0.073254
+v 0.033716 -0.203606 0.026785
+v 0.021113 -0.140246 -0.007745
+v 0.007438 -0.071497 -0.029009
+v -0.006783 -0.264218 0.073254
+v -0.006783 -0.207595 0.026785
+v -0.006783 -0.142994 -0.007745
+v -0.006783 -0.072898 -0.029009
+v -0.058330 -0.259142 0.073254
+v -0.047283 -0.203606 0.026785
+v -0.034680 -0.140246 -0.007745
+v -0.021005 -0.071497 -0.029009
+v -0.107895 -0.244106 0.073254
+v -0.086227 -0.191793 0.026785
+v -0.061505 -0.132109 -0.007745
+v -0.034680 -0.067349 -0.029009
+v -0.153575 -0.219690 0.073254
+v -0.122117 -0.172609 0.026785
+v -0.086227 -0.118895 -0.007745
+v -0.047283 -0.060612 -0.029009
+v -0.193614 -0.186831 0.073254
+v -0.153575 -0.146792 0.026785
+v -0.107895 -0.101112 -0.007745
+v -0.058330 -0.051546 -0.029009
+v -0.226473 -0.146792 0.073254
+v -0.179392 -0.115334 0.026785
+v -0.125679 -0.079443 -0.007745
+v -0.067396 -0.040500 -0.029009
+v -0.250889 -0.101112 0.073254
+v -0.198576 -0.079443 0.026785
+v -0.138893 -0.054721 -0.007745
+v -0.074132 -0.027897 -0.029009
+v -0.265925 -0.051546 0.073254
+v -0.210390 -0.040500 0.026785
+v -0.147030 -0.027897 -0.007745
+v -0.078280 -0.014222 -0.029009
+v -0.271002 0.000000 0.073254
+v -0.214378 0.000000 0.026785
+v -0.149777 -0.000000 -0.007745
+v -0.079681 -0.000000 -0.029009
+v -0.265925 0.051546 0.073254
+v -0.210390 0.040500 0.026785
+v -0.147030 0.027897 -0.007745
+v -0.078280 0.014222 -0.029009
+v -0.250889 0.101112 0.073254
+v -0.198576 0.079443 0.026785
+v -0.138893 0.054721 -0.007745
+v -0.074132 0.027897 -0.029009
+v -0.226473 0.146792 0.073254
+v -0.179392 0.115334 0.026785
+v -0.125679 0.079443 -0.007745
+v -0.067396 0.040500 -0.029009
+v -0.193614 0.186831 0.073254
+v -0.153575 0.146792 0.026785
+v -0.107895 0.101112 -0.007745
+v -0.058330 0.051546 -0.029009
+v -0.153575 0.219690 0.073254
+v -0.122117 0.172609 0.026785
+v -0.086227 0.118895 -0.007745
+v -0.047283 0.060612 -0.029009
+v -0.107895 0.244106 0.073254
+v -0.086227 0.191793 0.026785
+v -0.061505 0.132109 -0.007745
+v -0.034680 0.067349 -0.029009
+v -0.058330 0.259141 0.073254
+v -0.047283 0.203606 0.026785
+v -0.034680 0.140246 -0.007745
+v -0.021005 0.071497 -0.029009
+v -0.006783 0.264218 0.073254
+v -0.006783 0.207595 0.026785
+v -0.006783 0.142994 -0.007745
+v -0.006783 0.072898 -0.029009
+v 0.044763 0.259141 0.073254
+v 0.033716 0.203606 0.026785
+v 0.021113 0.140246 -0.007745
+v 0.007438 0.071497 -0.029009
+v 0.094329 0.244106 0.073254
+v 0.072660 0.191793 0.026785
+v 0.047938 0.132109 -0.007745
+v 0.021113 0.067349 -0.029009
+v 0.140008 0.219689 0.073254
+v 0.108550 0.172609 0.026785
+v 0.072660 0.118895 -0.007745
+v 0.033716 0.060612 -0.029009
+v 0.180047 0.186831 0.073254
+v 0.140008 0.146792 0.026785
+v 0.094328 0.101112 -0.007745
+v 0.044763 0.051546 -0.029009
+v 0.212906 0.146792 0.073254
+v 0.165825 0.115334 0.026785
+v 0.112112 0.079443 -0.007745
+v 0.053829 0.040500 -0.029009
+v 0.237322 0.101112 0.073254
+v 0.185009 0.079443 0.026785
+v 0.125326 0.054721 -0.007745
+v 0.060565 0.027897 -0.029009
+v 0.252358 0.051546 0.073254
+v 0.196823 0.040500 0.026785
+v 0.133463 0.027897 -0.007745
+v 0.064713 0.014222 -0.029009
+v 0.257435 -0.000000 0.073254
+v 0.200811 -0.000000 0.026785
+v 0.136210 -0.000000 -0.007745
+v 0.066114 -0.000000 -0.029009
+v -1.000000 1.000000 -0.100000
+v -1.000000 1.000000 0.100000
+v 1.000000 1.000000 -0.100000
+v 1.000000 1.000000 0.100000
+v -1.000000 -1.000000 -0.100000
+v -1.000000 -1.000000 0.100000
+v 1.000000 -1.000000 -0.100000
+v 1.000000 -1.000000 0.100000
+v -0.006783 -0.286168 0.100000
+v -0.062612 -0.280669 0.100000
+v 0.195568 -0.202351 0.100000
+v 0.231157 -0.158987 0.100000
+v 0.257601 -0.109512 0.100000
+v 0.195568 0.202351 0.100000
+v -0.209135 0.202351 0.100000
+v 0.273886 -0.055829 0.100000
+v 0.273886 0.055829 0.100000
+v 0.257601 0.109512 0.100000
+v 0.152203 0.237940 0.100000
+v -0.209135 -0.202351 0.100000
+v -0.165770 -0.237940 0.100000
+v -0.244724 -0.158986 0.100000
+v -0.116295 -0.264385 0.100000
+v -0.244723 0.158986 0.100000
+v -0.165770 0.237940 0.100000
+v -0.116295 0.264385 0.100000
+v 0.231156 0.158986 0.100000
+v -0.271168 -0.109512 0.100000
+v -0.287453 0.055829 0.100000
+v -0.292951 0.000000 0.100000
+v 0.279384 -0.000000 0.100000
+v 0.102728 -0.264385 0.100000
+v 0.049045 0.280669 0.100000
+v 0.049045 -0.280669 0.100000
+v -0.287453 -0.055829 0.100000
+v -0.062612 0.280669 0.100000
+v -0.006783 0.286168 0.100000
+v 0.152203 -0.237940 0.100000
+v 0.102728 0.264385 0.100000
+v -0.271168 0.109512 0.100000
+vt 0.5258 0.8663
+vt 0.5000 1.0000
+vt 0.5000 0.8663
+vt 0.5202 0.6339
+vt 0.5000 0.6339
+vt 0.5139 0.4613
+vt 0.5000 0.4613
+vt 0.5071 0.3550
+vt 0.5000 0.3550
+vt 0.5000 0.4966
+vt 0.5071 0.5324
+vt 0.5000 0.5331
+vt 0.5139 0.5303
+vt 0.5506 0.8663
+vt 0.5279 1.0000
+vt 0.5274 0.4613
+vt 0.5139 0.3550
+vt 0.5734 0.8663
+vt 0.5548 1.0000
+vt 0.5577 0.6339
+vt 0.5397 0.6339
+vt 0.5397 0.4613
+vt 0.5202 0.3550
+vt 0.5202 0.5269
+vt 0.5258 0.5224
+vt 0.6012 1.0000
+vt 0.5795 1.0000
+vt 0.5734 0.6339
+vt 0.5506 0.4613
+vt 0.5258 0.3550
+vt 0.5700 0.6339
+vt 0.5700 0.8663
+vt 0.5900 0.8663
+vt 0.5363 0.4613
+vt 0.5472 0.4613
+vt 0.5169 0.3550
+vt 0.5224 0.3550
+vt 0.5303 0.5169
+vt 0.5978 1.0000
+vt 0.5514 1.0000
+vt 0.5761 1.0000
+vt 0.5543 0.6339
+vt 0.5472 0.8663
+vt 0.5363 0.6339
+vt 0.5240 0.4613
+vt 0.5337 0.5106
+vt 0.5224 0.8663
+vt 0.5106 0.4613
+vt 0.5106 0.3550
+vt 0.5357 0.5037
+vt 0.4966 1.0000
+vt 0.5245 1.0000
+vt 0.5169 0.6339
+vt 0.4966 0.8663
+vt 0.4966 0.6339
+vt 0.5037 0.3550
+vt 0.4966 0.4613
+vt 0.5364 0.4966
+vt 0.4827 0.4613
+vt 0.4895 0.3550
+vt 0.4966 0.3550
+vt 0.5357 0.4895
+vt 0.4708 0.8663
+vt 0.4461 0.8663
+vt 0.4687 1.0000
+vt 0.4764 0.6339
+vt 0.4569 0.6339
+vt 0.4692 0.4613
+vt 0.5337 0.4827
+vt 0.4827 0.3550
+vt 0.4569 0.4613
+vt 0.5303 0.4764
+vt 0.4232 0.8663
+vt 0.4419 1.0000
+vt 0.4389 0.6339
+vt 0.4032 0.8663
+vt 0.4171 1.0000
+vt 0.4232 0.6339
+vt 0.4708 0.3550
+vt 0.4764 0.3550
+vt 0.5258 0.4708
+vt 0.5202 0.3550
+vt 0.5506 0.4613
+vt 0.5258 0.3550
+vt 0.5202 0.4663
+vt 0.5734 0.8663
+vt 0.6012 1.0000
+vt 0.5934 0.8663
+vt 0.5734 0.6339
+vt 0.5577 0.6339
+vt 0.5506 0.8663
+vt 0.5795 1.0000
+vt 0.5274 0.4613
+vt 0.5397 0.4613
+vt 0.5139 0.4629
+vt 0.5071 0.4609
+vt 0.5279 1.0000
+vt 0.5548 1.0000
+vt 0.5202 0.6339
+vt 0.5397 0.6339
+vt 0.5139 0.4613
+vt 0.5071 0.3550
+vt 0.5139 0.3550
+vt 0.5000 0.8663
+vt 0.5258 0.8663
+vt 0.5000 0.6339
+vt 0.5000 0.4613
+vt 0.5000 0.4602
+vt 0.4929 0.4609
+vt 0.4742 0.8663
+vt 0.5000 1.0000
+vt 0.4798 0.6339
+vt 0.4861 0.4613
+vt 0.4929 0.3550
+vt 0.5000 0.3550
+vt 0.4603 0.6339
+vt 0.4726 0.4613
+vt 0.4861 0.4629
+vt 0.4494 0.8663
+vt 0.4721 1.0000
+vt 0.4266 0.8663
+vt 0.4452 1.0000
+vt 0.4603 0.4613
+vt 0.4861 0.3550
+vt 0.4798 0.4663
+vt 0.4266 0.6339
+vt 0.4423 0.6339
+vt 0.4742 0.3550
+vt 0.4798 0.3550
+vt 0.4742 0.4708
+vt 0.4066 0.8663
+vt 0.4205 1.0000
+vt 0.4232 0.8663
+vt 0.3954 1.0000
+vt 0.4032 0.8663
+vt 0.4389 0.6339
+vt 0.4232 0.6339
+vt 0.4569 0.4613
+vt 0.4461 0.4613
+vt 0.4708 0.3550
+vt 0.4697 0.4764
+vt 0.4569 0.6339
+vt 0.4827 0.3550
+vt 0.4764 0.3550
+vt 0.4663 0.4827
+vt 0.4461 0.8663
+vt 0.4171 1.0000
+vt 0.4708 0.8663
+vt 0.4419 1.0000
+vt 0.4764 0.6339
+vt 0.4827 0.4613
+vt 0.4692 0.4613
+vt 0.4895 0.3550
+vt 0.4643 0.4895
+vt 0.4966 0.3550
+vt 0.4636 0.4966
+vt 0.4966 0.8663
+vt 0.4687 1.0000
+vt 0.4966 0.6339
+vt 0.5224 0.8663
+vt 0.4966 1.0000
+vt 0.5169 0.6339
+vt 0.4966 0.4613
+vt 0.5037 0.3550
+vt 0.4643 0.5037
+vt 0.5106 0.3550
+vt 0.5106 0.4613
+vt 0.4663 0.5106
+vt 0.5472 0.8663
+vt 0.5245 1.0000
+vt 0.5363 0.6339
+vt 0.5240 0.4613
+vt 0.5700 0.8663
+vt 0.5514 1.0000
+vt 0.5363 0.4613
+vt 0.5169 0.3550
+vt 0.4697 0.5169
+vt 0.4742 0.5224
+vt 0.5900 0.8663
+vt 0.5761 1.0000
+vt 0.5700 0.6339
+vt 0.5543 0.6339
+vt 0.5224 0.3550
+vt 0.4423 0.6339
+vt 0.4066 0.8663
+vt 0.4266 0.6339
+vt 0.4603 0.4613
+vt 0.4494 0.4613
+vt 0.4798 0.3550
+vt 0.4742 0.3550
+vt 0.4798 0.5269
+vt 0.4266 0.8663
+vt 0.3988 1.0000
+vt 0.4494 0.8663
+vt 0.4205 1.0000
+vt 0.4603 0.6339
+vt 0.4861 0.3550
+vt 0.4861 0.5303
+vt 0.4798 0.6339
+vt 0.4861 0.4613
+vt 0.4726 0.4613
+vt 0.4929 0.3550
+vt 0.4929 0.5324
+vt 0.4742 0.8663
+vt 0.4452 1.0000
+vt 0.4721 1.0000
+vt 0.0000 1.0000
+vt 1.0000 0.0000
+vt 0.0000 0.0000
+vt 0.0000 1.0000
+vt 1.0000 0.0000
+vt 0.0000 0.0000
+vt 0.0000 1.0000
+vt -1.0000 0.0000
+vt 0.0000 0.0000
+vt 0.0000 1.0000
+vt -1.0000 0.0000
+vt 0.0000 0.0000
+vt 0.0000 -1.0000
+vt -1.0000 0.0000
+vt -0.5279 -0.3631
+vt -0.5000 -0.3603
+vt -0.5000 0.3535
+vt -0.5279 0.3563
+vt -1.0000 0.0000
+vt 0.5934 0.8663
+vt 0.3954 1.0000
+vt 0.4461 0.4613
+vt 0.4494 0.4613
+vt 0.3988 1.0000
+vt 0.5978 1.0000
+vt 0.5472 0.4613
+vt 1.0000 1.0000
+vt 1.0000 1.0000
+vt -1.0000 1.0000
+vt -1.0000 1.0000
+vt 1.0000 -1.0000
+vt -1.0000 -1.0000
+vt -0.6431 -0.5034
+vt -0.6403 -0.4755
+vt -0.3569 -0.5034
+vt 0.0000 -1.0000
+vt 0.0000 0.0000
+vt -0.3597 -0.4755
+vt -0.6322 -0.4486
+vt -0.6190 -0.4239
+vt -0.3678 -0.4486
+vt -0.3810 -0.4239
+vt -0.6012 -0.4022
+vt -0.5795 -0.3844
+vt -0.3988 -0.4022
+vt -0.4205 -0.3844
+vt -0.5548 -0.3712
+vt -0.4452 -0.3712
+vt -0.4721 -0.3631
+vt 0.0000 0.0000
+vt -0.3569 0.4966
+vt -0.3597 0.4687
+vt -0.3678 0.4419
+vt -0.6403 0.4687
+vt -0.6431 0.4966
+vt -0.6322 0.4419
+vt -0.3810 0.4171
+vt -0.3988 0.3954
+vt -0.6190 0.4171
+vt -0.6012 0.3954
+vt -0.4205 0.3776
+vt -0.4452 0.3644
+vt -0.5795 0.3776
+vt -0.5548 0.3644
+vt -0.4721 0.3563
+vn -0.7708 0.0759 0.6326
+vn -0.6332 0.0624 0.7715
+vn -0.4709 0.0464 0.8810
+vn -0.2902 0.0286 0.9565
+vn -0.0980 0.0097 0.9951
+vn -0.0942 0.0286 0.9951
+vn -0.7412 0.2248 0.6326
+vn -0.6088 0.1847 0.7715
+vn -0.4528 0.1374 0.8810
+vn -0.2790 0.0846 0.9565
+vn -0.6831 0.3651 0.6326
+vn -0.5611 0.2999 0.7715
+vn -0.4173 0.2230 0.8810
+vn -0.2571 0.1374 0.9565
+vn -0.0869 0.0464 0.9951
+vn -0.0761 0.0625 0.9951
+vn -0.5987 0.4913 0.6326
+vn -0.4918 0.4036 0.7715
+vn -0.3658 0.3002 0.8810
+vn -0.2254 0.1850 0.9565
+vn -0.4036 0.4918 0.7715
+vn -0.3002 0.3658 0.8810
+vn -0.1850 0.2254 0.9565
+vn -0.0625 0.0761 0.9951
+vn -0.4913 0.5987 0.6326
+vn -0.3651 0.6831 0.6326
+vn -0.2999 0.5611 0.7715
+vn -0.2230 0.4173 0.8810
+vn -0.1374 0.2571 0.9565
+vn -0.0464 0.0869 0.9951
+vn -0.1847 0.6088 0.7715
+vn -0.1374 0.4528 0.8810
+vn -0.0846 0.2790 0.9565
+vn -0.0286 0.0942 0.9951
+vn -0.2248 0.7412 0.6326
+vn -0.0759 0.7708 0.6326
+vn -0.0624 0.6332 0.7715
+vn -0.0464 0.4709 0.8810
+vn -0.0286 0.2902 0.9565
+vn -0.0097 0.0980 0.9951
+vn 0.0464 0.4709 0.8810
+vn 0.0286 0.2902 0.9565
+vn 0.0097 0.0980 0.9951
+vn 0.0759 0.7708 0.6326
+vn 0.0624 0.6332 0.7715
+vn 0.2248 0.7412 0.6326
+vn 0.1847 0.6088 0.7715
+vn 0.1374 0.4528 0.8810
+vn 0.0846 0.2790 0.9565
+vn 0.0286 0.0942 0.9951
+vn 0.1374 0.2571 0.9565
+vn 0.0464 0.0869 0.9951
+vn 0.3651 0.6831 0.6326
+vn 0.2999 0.5611 0.7715
+vn 0.2230 0.4173 0.8810
+vn 0.4913 0.5987 0.6326
+vn 0.4036 0.4918 0.7715
+vn 0.3002 0.3658 0.8810
+vn 0.1850 0.2254 0.9565
+vn 0.0625 0.0761 0.9951
+vn 0.2254 0.1850 0.9565
+vn 0.0761 0.0625 0.9951
+vn 0.5987 0.4913 0.6326
+vn 0.4918 0.4036 0.7715
+vn 0.3658 0.3002 0.8810
+vn 0.6831 0.3651 0.6326
+vn 0.5611 0.2999 0.7715
+vn 0.4173 0.2230 0.8810
+vn 0.2571 0.1374 0.9565
+vn 0.0869 0.0464 0.9951
+vn 0.0942 0.0286 0.9951
+vn 0.7412 0.2248 0.6326
+vn 0.6088 0.1847 0.7715
+vn 0.4528 0.1374 0.8810
+vn 0.2790 0.0846 0.9565
+vn 0.7708 0.0759 0.6326
+vn 0.6332 0.0624 0.7715
+vn 0.4709 0.0464 0.8810
+vn 0.2902 0.0286 0.9565
+vn 0.0980 0.0097 0.9951
+vn 0.0980 -0.0097 0.9951
+vn 0.7708 -0.0759 0.6326
+vn 0.6332 -0.0624 0.7715
+vn 0.4709 -0.0464 0.8810
+vn 0.2902 -0.0286 0.9565
+vn 0.6088 -0.1847 0.7715
+vn 0.4528 -0.1374 0.8810
+vn 0.2790 -0.0846 0.9565
+vn 0.0942 -0.0286 0.9951
+vn 0.7412 -0.2248 0.6326
+vn 0.6831 -0.3651 0.6326
+vn 0.5611 -0.2999 0.7715
+vn 0.4173 -0.2230 0.8810
+vn 0.2571 -0.1374 0.9565
+vn 0.0869 -0.0464 0.9951
+vn 0.3658 -0.3002 0.8810
+vn 0.2254 -0.1850 0.9565
+vn 0.0761 -0.0625 0.9951
+vn 0.5987 -0.4913 0.6326
+vn 0.4918 -0.4036 0.7715
+vn 0.4913 -0.5987 0.6326
+vn 0.4036 -0.4918 0.7715
+vn 0.3002 -0.3658 0.8810
+vn 0.1850 -0.2254 0.9565
+vn 0.0625 -0.0761 0.9951
+vn 0.2230 -0.4173 0.8810
+vn 0.1374 -0.2571 0.9565
+vn 0.0464 -0.0869 0.9951
+vn 0.3651 -0.6831 0.6326
+vn 0.2999 -0.5611 0.7715
+vn 0.2248 -0.7412 0.6326
+vn 0.1847 -0.6088 0.7715
+vn 0.1374 -0.4528 0.8810
+vn 0.0846 -0.2790 0.9565
+vn 0.0286 -0.0942 0.9951
+vn 0.0286 -0.2902 0.9565
+vn 0.0097 -0.0980 0.9951
+vn 0.0759 -0.7708 0.6326
+vn 0.0624 -0.6332 0.7715
+vn 0.0464 -0.4709 0.8810
+vn -0.0759 -0.7708 0.6326
+vn -0.0624 -0.6332 0.7715
+vn -0.0464 -0.4709 0.8810
+vn -0.0286 -0.2902 0.9565
+vn -0.0097 -0.0980 0.9951
+vn -0.0846 -0.2790 0.9565
+vn -0.0286 -0.0942 0.9951
+vn -0.2248 -0.7412 0.6326
+vn -0.1847 -0.6088 0.7715
+vn -0.1374 -0.4528 0.8810
+vn -0.3651 -0.6831 0.6326
+vn -0.2999 -0.5611 0.7715
+vn -0.2231 -0.4173 0.8810
+vn -0.1374 -0.2571 0.9565
+vn -0.0464 -0.0869 0.9951
+vn -0.0625 -0.0761 0.9951
+vn -0.4913 -0.5987 0.6326
+vn -0.4036 -0.4918 0.7715
+vn -0.3002 -0.3658 0.8810
+vn -0.1850 -0.2254 0.9565
+vn -0.4918 -0.4036 0.7715
+vn -0.3658 -0.3002 0.8810
+vn -0.2254 -0.1850 0.9565
+vn -0.0761 -0.0625 0.9951
+vn -0.5987 -0.4913 0.6326
+vn -0.6831 -0.3651 0.6326
+vn -0.5611 -0.2999 0.7715
+vn -0.4173 -0.2231 0.8810
+vn -0.2571 -0.1374 0.9565
+vn -0.0869 -0.0464 0.9951
+vn -0.6088 -0.1847 0.7715
+vn -0.4528 -0.1374 0.8810
+vn -0.2790 -0.0846 0.9565
+vn -0.0942 -0.0286 0.9951
+vn -0.7412 -0.2248 0.6326
+vn -0.7708 -0.0759 0.6326
+vn -0.6332 -0.0624 0.7715
+vn -0.4709 -0.0464 0.8810
+vn -0.2902 -0.0286 0.9565
+vn -0.0980 -0.0097 0.9951
+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
+vn 0.2231 0.4173 0.8810
+vn 0.4173 -0.2231 0.8810
+vn -0.4173 -0.2230 0.8810
+usemtl None
+s off
+f 1/1/1 160/2/1 126/3/1
+f 2/4/2 126/3/2 127/5/2
+f 3/6/3 127/5/3 128/7/3
+f 4/8/4 128/7/4 129/9/4
+f 21/10/5 4/11/5 129/12/5
+f 21/10/6 8/13/6 4/11/6
+f 5/14/7 145/15/7 1/1/7
+f 2/4/8 5/14/8 1/1/8
+f 7/16/9 2/4/9 3/6/9
+f 8/17/10 3/6/10 4/8/10
+f 9/18/11 142/19/11 5/14/11
+f 10/20/12 5/14/12 6/21/12
+f 11/22/13 6/21/13 7/16/13
+f 12/23/14 7/16/14 8/17/14
+f 21/10/15 12/24/15 8/13/15
+f 21/10/16 16/25/16 12/24/16
+f 9/18/17 140/26/17 141/27/17
+f 14/28/18 9/18/18 10/20/18
+f 15/29/19 10/20/19 11/22/19
+f 16/30/20 11/22/20 12/23/20
+f 14/31/21 17/32/21 13/33/21
+f 19/34/22 14/31/22 15/35/22
+f 20/36/23 15/35/23 16/37/23
+f 21/10/24 20/38/24 16/25/24
+f 17/32/25 140/39/25 13/33/25
+f 17/32/26 161/40/26 167/41/26
+f 18/42/27 22/43/27 17/32/27
+f 19/34/28 23/44/28 18/42/28
+f 20/36/29 24/45/29 19/34/29
+f 21/10/30 25/46/30 20/38/30
+f 23/44/31 26/47/31 22/43/31
+f 28/48/32 23/44/32 24/45/32
+f 25/49/33 28/48/33 24/45/33
+f 21/10/34 29/50/34 25/46/34
+f 26/47/35 161/40/35 22/43/35
+f 26/47/36 138/51/36 163/52/36
+f 27/53/37 30/54/37 26/47/37
+f 28/48/38 31/55/38 27/53/38
+f 29/56/39 32/57/39 28/48/39
+f 21/10/40 33/58/40 29/50/40
+f 36/59/41 31/55/41 32/57/41
+f 37/60/42 32/57/42 33/61/42
+f 21/10/43 37/62/43 33/58/43
+f 34/63/44 138/51/44 30/54/44
+f 31/55/45 34/63/45 30/54/45
+f 38/64/46 139/65/46 34/63/46
+f 35/66/47 38/64/47 34/63/47
+f 36/59/48 39/67/48 35/66/48
+f 37/60/49 40/68/49 36/59/49
+f 21/10/50 41/69/50 37/62/50
+f 41/70/51 44/71/51 40/68/51
+f 21/10/52 45/72/52 41/69/52
+f 42/73/53 152/74/53 38/64/53
+f 43/75/54 38/64/54 39/67/54
+f 44/71/55 39/67/55 40/68/55
+f 46/76/56 150/77/56 42/73/56
+f 43/75/57 46/76/57 42/73/57
+f 44/71/58 47/78/58 43/75/58
+f 49/79/59 44/71/59 45/80/59
+f 21/10/60 49/81/60 45/72/60
+f 53/82/61 48/83/61 49/84/61
+f 21/10/62 53/85/62 49/81/62
+f 50/86/63 149/87/63 46/88/63
+f 47/89/64 50/86/64 46/88/64
+f 48/83/65 51/90/65 47/89/65
+f 54/91/66 151/92/66 50/86/66
+f 51/90/67 54/91/67 50/86/67
+f 56/93/68 51/90/68 52/94/68
+f 53/82/69 56/93/69 52/94/69
+f 21/10/70 57/95/70 53/85/70
+f 21/10/71 61/96/71 57/95/71
+f 54/91/72 164/97/72 157/98/72
+f 59/99/73 54/91/73 55/100/73
+f 60/101/74 55/100/74 56/93/74
+f 61/102/75 56/93/75 57/103/75
+f 62/104/76 164/97/76 58/105/76
+f 63/106/77 58/105/77 59/99/77
+f 64/107/78 59/99/78 60/101/78
+f 61/102/79 64/107/79 60/101/79
+f 21/10/80 65/108/80 61/96/80
+f 21/10/81 69/109/81 65/108/81
+f 66/110/82 159/111/82 62/104/82
+f 67/112/83 62/104/83 63/106/83
+f 68/113/84 63/106/84 64/107/84
+f 69/114/85 64/107/85 65/115/85
+f 71/116/86 66/110/86 67/112/86
+f 72/117/87 67/112/87 68/113/87
+f 69/114/88 72/117/88 68/113/88
+f 21/10/89 73/118/89 69/109/89
+f 70/119/90 158/120/90 66/110/90
+f 74/121/91 169/122/91 70/119/91
+f 71/116/92 74/121/92 70/119/92
+f 76/123/93 71/116/93 72/117/93
+f 73/124/94 76/123/94 72/117/94
+f 21/10/95 77/125/95 73/118/95
+f 76/123/96 79/126/96 75/127/96
+f 81/128/97 76/123/97 77/129/97
+f 21/10/98 81/130/98 77/125/98
+f 78/131/99 153/132/99 74/121/99
+f 79/126/100 74/121/100 75/127/100
+f 82/133/101 144/134/101 78/135/101
+f 83/136/102 78/135/102 79/137/102
+f 84/138/103 79/137/103 80/139/103
+f 81/140/104 84/138/104 80/139/104
+f 21/10/105 85/141/105 81/130/105
+f 84/138/106 87/142/106 83/136/106
+f 89/143/107 84/138/107 85/144/107
+f 21/10/108 89/145/108 85/141/108
+f 86/146/109 154/147/109 82/133/109
+f 87/142/110 82/133/110 83/136/110
+f 90/148/111 155/149/111 86/146/111
+f 91/150/112 86/146/112 87/142/112
+f 92/151/113 87/142/113 88/152/113
+f 93/153/114 88/152/114 89/143/114
+f 21/10/115 93/154/115 89/145/115
+f 97/155/116 92/151/116 93/153/116
+f 21/10/117 97/156/117 93/154/117
+f 94/157/118 165/158/118 90/148/118
+f 95/159/119 90/148/119 91/150/119
+f 92/151/120 95/159/120 91/150/120
+f 98/160/121 166/161/121 94/157/121
+f 99/162/122 94/157/122 95/159/122
+f 96/163/123 99/162/123 95/159/123
+f 101/164/124 96/163/124 97/155/124
+f 21/10/125 101/165/125 97/156/125
+f 105/166/126 100/167/126 101/164/126
+f 21/10/127 105/168/127 101/165/127
+f 102/169/128 162/170/128 98/160/128
+f 103/171/129 98/160/129 99/162/129
+f 104/172/130 99/162/130 100/167/130
+f 106/173/131 168/174/131 102/169/131
+f 103/171/132 106/173/132 102/169/132
+f 108/175/133 103/171/133 104/172/133
+f 109/176/134 104/172/134 105/166/134
+f 21/10/135 109/177/135 105/168/135
+f 21/10/136 113/178/136 109/177/136
+f 110/179/137 148/180/137 106/173/137
+f 111/181/138 106/173/138 107/182/138
+f 108/175/139 111/181/139 107/182/139
+f 113/183/140 108/175/140 109/176/140
+f 115/184/141 110/185/141 111/186/141
+f 116/187/142 111/186/142 112/188/142
+f 117/189/143 112/188/143 113/190/143
+f 21/10/144 117/191/144 113/178/144
+f 114/192/145 143/193/145 110/185/145
+f 118/194/146 156/195/146 114/192/146
+f 119/196/147 114/192/147 115/184/147
+f 116/187/148 119/196/148 115/184/148
+f 121/197/149 116/187/149 117/189/149
+f 21/10/150 121/198/150 117/191/150
+f 123/199/151 118/194/151 119/196/151
+f 124/200/152 119/196/152 120/201/152
+f 125/202/153 120/201/153 121/197/153
+f 21/10/154 125/203/154 121/198/154
+f 122/204/155 147/205/155 118/194/155
+f 126/3/156 146/206/156 122/204/156
+f 127/5/157 122/204/157 123/199/157
+f 128/7/158 123/199/158 124/200/158
+f 129/9/159 124/200/159 125/202/159
+f 21/10/160 129/12/160 125/203/160
+f 131/207/161 132/208/161 130/209/161
+f 133/210/162 136/211/162 132/212/162
+f 137/213/163 134/214/163 136/215/163
+f 135/216/164 130/217/164 134/218/164
+f 136/211/165 130/219/165 132/212/165
+f 133/220/166 146/221/166 160/222/166
+f 159/223/166 158/224/166 131/225/166
+f 1/1/1 145/15/1 160/2/1
+f 2/4/2 1/1/2 126/3/2
+f 3/6/3 2/4/3 127/5/3
+f 4/8/4 3/6/4 128/7/4
+f 5/14/7 142/19/7 145/15/7
+f 2/4/8 6/21/8 5/14/8
+f 7/16/9 6/21/9 2/4/9
+f 8/17/10 7/16/10 3/6/10
+f 9/18/11 141/27/11 142/19/11
+f 10/20/12 9/18/12 5/14/12
+f 11/22/13 10/20/13 6/21/13
+f 12/23/14 11/22/14 7/16/14
+f 9/18/17 13/226/17 140/26/17
+f 14/28/18 13/226/18 9/18/18
+f 15/29/19 14/28/19 10/20/19
+f 16/30/20 15/29/20 11/22/20
+f 14/31/21 18/42/21 17/32/21
+f 19/34/22 18/42/22 14/31/22
+f 20/36/23 19/34/23 15/35/23
+f 17/32/25 167/41/25 140/39/25
+f 17/32/26 22/43/26 161/40/26
+f 18/42/27 23/44/27 22/43/27
+f 19/34/28 24/45/28 23/44/28
+f 20/36/29 25/49/29 24/45/29
+f 23/44/31 27/53/31 26/47/31
+f 28/48/32 27/53/32 23/44/32
+f 25/49/33 29/56/33 28/48/33
+f 26/47/35 163/52/35 161/40/35
+f 26/47/36 30/54/36 138/51/36
+f 27/53/37 31/55/37 30/54/37
+f 28/48/38 32/57/38 31/55/38
+f 29/56/39 33/61/39 32/57/39
+f 36/59/41 35/66/41 31/55/41
+f 37/60/42 36/59/42 32/57/42
+f 34/63/44 139/65/44 138/51/44
+f 31/55/45 35/66/45 34/63/45
+f 38/64/46 152/74/46 139/65/46
+f 35/66/47 39/67/47 38/64/47
+f 36/59/48 40/68/48 39/67/48
+f 37/60/49 41/70/49 40/68/49
+f 41/70/51 45/80/51 44/71/51
+f 42/73/53 150/77/53 152/74/53
+f 43/75/54 42/73/54 38/64/54
+f 44/71/167 43/75/167 39/67/167
+f 46/76/56 149/227/56 150/77/56
+f 43/75/57 47/78/57 46/76/57
+f 44/71/58 48/228/58 47/78/58
+f 49/79/59 48/228/59 44/71/59
+f 53/82/61 52/94/61 48/83/61
+f 50/86/63 151/92/63 149/87/63
+f 47/89/64 51/90/64 50/86/64
+f 48/83/65 52/94/65 51/90/65
+f 54/91/66 157/98/66 151/92/66
+f 51/90/67 55/100/67 54/91/67
+f 56/93/68 55/100/68 51/90/68
+f 53/82/69 57/103/69 56/93/69
+f 54/91/72 58/105/72 164/97/72
+f 59/99/73 58/105/73 54/91/73
+f 60/101/74 59/99/74 55/100/74
+f 61/102/75 60/101/75 56/93/75
+f 62/104/76 159/111/76 164/97/76
+f 63/106/77 62/104/77 58/105/77
+f 64/107/78 63/106/78 59/99/78
+f 61/102/79 65/115/79 64/107/79
+f 66/110/82 158/120/82 159/111/82
+f 67/112/83 66/110/83 62/104/83
+f 68/113/84 67/112/84 63/106/84
+f 69/114/85 68/113/85 64/107/85
+f 71/116/86 70/119/86 66/110/86
+f 72/117/87 71/116/87 67/112/87
+f 69/114/88 73/124/88 72/117/88
+f 70/119/90 169/122/90 158/120/90
+f 74/121/91 153/132/91 169/122/91
+f 71/116/92 75/127/92 74/121/92
+f 76/123/168 75/127/168 71/116/168
+f 73/124/94 77/129/94 76/123/94
+f 76/123/96 80/229/96 79/126/96
+f 81/128/97 80/229/97 76/123/97
+f 78/131/99 144/230/99 153/132/99
+f 79/126/100 78/131/100 74/121/100
+f 82/133/101 154/147/101 144/134/101
+f 83/136/102 82/133/102 78/135/102
+f 84/138/103 83/136/103 79/137/103
+f 81/140/104 85/144/104 84/138/104
+f 84/138/106 88/152/106 87/142/106
+f 89/143/107 88/152/107 84/138/107
+f 86/146/109 155/149/109 154/147/109
+f 87/142/110 86/146/110 82/133/110
+f 90/148/111 165/158/111 155/149/111
+f 91/150/112 90/148/112 86/146/112
+f 92/151/113 91/150/113 87/142/113
+f 93/153/114 92/151/114 88/152/114
+f 97/155/116 96/163/116 92/151/116
+f 94/157/118 166/161/118 165/158/118
+f 95/159/119 94/157/119 90/148/119
+f 92/151/120 96/163/120 95/159/120
+f 98/160/121 162/170/121 166/161/121
+f 99/162/122 98/160/122 94/157/122
+f 96/163/123 100/167/123 99/162/123
+f 101/164/124 100/167/124 96/163/124
+f 105/166/126 104/172/126 100/167/126
+f 102/169/128 168/174/128 162/170/128
+f 103/171/129 102/169/129 98/160/129
+f 104/172/130 103/171/130 99/162/130
+f 106/173/131 148/180/131 168/174/131
+f 103/171/132 107/182/132 106/173/132
+f 108/175/133 107/182/133 103/171/133
+f 109/176/134 108/175/134 104/172/134
+f 110/179/137 143/231/137 148/180/137
+f 111/181/138 110/179/138 106/173/138
+f 108/175/139 112/232/139 111/181/139
+f 113/183/140 112/232/140 108/175/140
+f 115/184/141 114/192/141 110/185/141
+f 116/187/142 115/184/142 111/186/142
+f 117/189/143 116/187/143 112/188/143
+f 114/192/145 156/195/145 143/193/145
+f 118/194/146 147/205/146 156/195/146
+f 119/196/147 118/194/147 114/192/147
+f 116/187/169 120/201/169 119/196/169
+f 121/197/149 120/201/149 116/187/149
+f 123/199/151 122/204/151 118/194/151
+f 124/200/152 123/199/152 119/196/152
+f 125/202/153 124/200/153 120/201/153
+f 122/204/155 146/206/155 147/205/155
+f 126/3/156 160/2/156 146/206/156
+f 127/5/157 126/3/157 122/204/157
+f 128/7/158 127/5/158 123/199/158
+f 129/9/159 128/7/159 124/200/159
+f 131/207/161 133/233/161 132/208/161
+f 133/210/162 137/234/162 136/211/162
+f 137/213/163 135/235/163 134/214/163
+f 135/216/164 131/236/164 130/217/164
+f 136/211/165 134/237/165 130/219/165
+f 133/220/166 131/238/166 166/239/166
+f 133/220/166 166/239/166 162/240/166
+f 138/241/166 135/242/166 137/243/166
+f 163/244/166 138/241/166 137/243/166
+f 133/220/166 162/240/166 168/245/166
+f 133/220/166 168/245/166 148/246/166
+f 161/247/166 163/244/166 137/243/166
+f 167/248/166 161/247/166 137/243/166
+f 133/220/166 148/246/166 143/249/166
+f 133/220/166 143/249/166 156/250/166
+f 140/251/166 167/248/166 137/243/166
+f 141/252/166 140/251/166 137/243/166
+f 137/243/166 133/220/166 160/222/166
+f 133/220/166 156/250/166 147/253/166
+f 142/254/166 141/252/166 137/243/166
+f 145/255/166 142/254/166 137/243/166
+f 133/220/166 147/253/166 146/221/166
+f 160/222/166 145/255/166 137/243/166
+f 135/256/166 138/257/166 139/258/166
+f 135/256/166 139/258/166 152/259/166
+f 165/260/166 166/261/166 131/225/166
+f 155/262/166 165/260/166 131/225/166
+f 135/256/166 152/259/166 150/263/166
+f 135/256/166 150/263/166 149/264/166
+f 154/265/166 155/262/166 131/225/166
+f 144/266/166 154/265/166 131/225/166
+f 135/256/166 149/264/166 151/267/166
+f 135/256/166 151/267/166 157/268/166
+f 153/269/166 144/266/166 131/225/166
+f 169/270/166 153/269/166 131/225/166
+f 131/225/166 135/256/166 159/223/166
+f 135/256/166 157/268/166 164/271/166
+f 158/224/166 169/270/166 131/225/166
+f 135/256/166 164/271/166 159/223/166

data/toys/concave_box.urdf

@@ -0,0 +1,30 @@
+<?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="0"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="concave_box.obj" scale="1 1 1"/>      
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<cdf filename="concave_box.cdf" scale="1 1 1"/>      
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

examples/Constraints/ConstraintDemo.cpp

@@ -356,7 +356,7 @@ void	AllConstraintDemo::initPhysics()
 
 		spSlider6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true;
 		spSlider6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f;
-		spSlider6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;
+		spSlider6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f;
 
 
 		m_dynamicsWorld->addConstraint(spSlider6Dof);
@@ -431,7 +431,7 @@ void	AllConstraintDemo::initPhysics()
 
 //		pGen6DOF->getTranslationalLimitMotor()->m_enableMotor[0] = true;
 //		pGen6DOF->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f;
-//		pGen6DOF->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;
+//		pGen6DOF->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f;
 
 
 //		pGen6DOF->setAngularLowerLimit(btVector3(0., SIMD_HALF_PI*0.9, 0.));
@@ -662,7 +662,7 @@ void	AllConstraintDemo::initPhysics()
 
 		pGen6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true;
 		pGen6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f;
-		pGen6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;
+		pGen6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f;
 	}
 #endif
 

examples/Constraints/Dof6Spring2Setup.cpp

@@ -300,11 +300,11 @@ void Dof6Spring2Setup::initPhysics()
 		#ifdef USE_6DOF2
 			constraint->enableMotor(5,true);
 			constraint->setTargetVelocity(5,3.f);
-			constraint->setMaxMotorForce(5,10.f);
+			constraint->setMaxMotorForce(5,600.f);
 		#else
 			constraint->getRotationalLimitMotor(2)->m_enableMotor = true;
 			constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f;
-			constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10;
+			constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 600.f;
 		#endif
 			constraint->setDbgDrawSize(btScalar(2.f));
 			m_dynamicsWorld->addConstraint(constraint, true);
@@ -335,13 +335,13 @@ void Dof6Spring2Setup::initPhysics()
 		#ifdef USE_6DOF2
 			constraint->enableMotor(5,true);
 			constraint->setTargetVelocity(5,3.f);
-			constraint->setMaxMotorForce(5,10.f);
+			constraint->setMaxMotorForce(5,600.f);
 			constraint->setServo(5,true);
 			constraint->setServoTarget(5, M_PI_2);
 		#else
 			constraint->getRotationalLimitMotor(2)->m_enableMotor = true;
 			constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f;
-			constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10;
+			constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 600.f;
 			//servo motor is not implemented in 6dofspring constraint
 		#endif
 			constraint->setDbgDrawSize(btScalar(2.f));

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -410,6 +410,14 @@ void OpenGLExampleBrowserVisualizerFlagCallback(int flag, bool enable)
     if (flag == COV_ENABLE_WIREFRAME)
     {
         visualWireframe = enable;
+		if (visualWireframe)
+		{
+			gDebugDrawFlags |= btIDebugDraw::DBG_DrawWireframe;
+		}
+		else
+		{
+			gDebugDrawFlags &= ~btIDebugDraw::DBG_DrawWireframe;
+		}
     }
 }
 

examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp

@@ -575,7 +575,7 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLIn
 	}
 	 
 	XMLDocument doc;
-	if (!doc.LoadFile(filename))
+	if (doc.LoadFile(filename) != XML_SUCCESS)
 		return;
 
 	//We need units to be in meter, so apply a scaling using the asset/units meter 

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -21,6 +21,7 @@ subject to the following restrictions:
 #include "../ImportSTLDemo/LoadMeshFromSTL.h"
 #include "../ImportColladaDemo/LoadMeshFromCollada.h"
 #include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
+#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
 #include "../../CommonInterfaces/CommonGUIHelperInterface.h"
 #include "Bullet3Common/b3FileUtils.h"
 #include <string>
@@ -509,6 +510,10 @@ bool findExistingMeshFile(
 	{
 		*out_type = UrdfGeometry::FILE_OBJ;
 	}
+	else if (ext == ".cdf")
+	{
+		*out_type = UrdfGeometry::FILE_CDF;
+	}
 	else
 	{
 		b3Warning("%s: invalid mesh filename extension '%s'\n", error_message_prefix.c_str(), ext.c_str());
@@ -662,7 +667,53 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
 			shape ->setMargin(gUrdfDefaultCollisionMargin);
             break;
 	}
+	case URDF_GEOM_CDF:
+		{
 			
+			char relativeFileName[1024];
+			char pathPrefix[1024];
+			pathPrefix[0] = 0;
+			if (b3ResourcePath::findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
+			{
+				b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
+				
+
+				btAlignedObjectArray<char> sdfData;
+				{
+					std::streampos fsize = 0;
+					std::ifstream file(relativeFileName, std::ios::binary);
+					if (file.good())
+					{
+						fsize = file.tellg();
+						file.seekg(0, std::ios::end);
+						fsize = file.tellg() - fsize;
+						file.seekg(0, std::ios::beg);
+						sdfData.resize(fsize);
+						int bytesRead = file.rdbuf()->sgetn(&sdfData[0], fsize);
+						btAssert(bytesRead == fsize);
+						file.close();
+					}
+				}
+
+				if (sdfData.size())
+				{
+					btSdfCollisionShape* sdfShape = new btSdfCollisionShape();
+					bool valid = sdfShape->initializeSDF(&sdfData[0], sdfData.size());
+					btAssert(valid);
+
+					if (valid)
+					{
+						shape = sdfShape;
+					}
+					else
+					{
+						delete sdfShape;
+					}
+
+				}
+			}
+			break;
+		}
 	case URDF_GEOM_MESH:
 	{
 		GLInstanceGraphicsShape* glmesh = 0;

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -436,9 +436,16 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, XMLElement* g, ErrorLogger* l
 			geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
 		}
 	}
-	else if (type_name == "mesh")
+	else if ((type_name == "mesh") || (type_name == "cdf"))
 	{
-		geom.m_type = URDF_GEOM_MESH;
+		if ((type_name == "cdf"))
+		{
+			geom.m_type = URDF_GEOM_CDF;
+		}
+		else
+		{
+			geom.m_type = URDF_GEOM_MESH;
+		}
 		geom.m_meshScale.setValue(1,1,1);
 		std::string fn;
 

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -54,7 +54,8 @@ enum UrdfGeomTypes
 	URDF_GEOM_CYLINDER,
 	URDF_GEOM_MESH,
 	URDF_GEOM_PLANE,
-	URDF_GEOM_CAPSULE, //non-standard URDF?
+	URDF_GEOM_CAPSULE, //non-standard URDF
+	URDF_GEOM_CDF,//signed-distance-field, non-standard URDF
 	URDF_GEOM_UNKNOWN, 
 };
 
@@ -79,6 +80,8 @@ struct UrdfGeometry
 		FILE_STL     =1,
 		FILE_COLLADA =2,
 		FILE_OBJ     =3,
+		FILE_CDF = 4,
+
 	};
 	int         m_meshFileType;
 	std::string m_meshFileName;
@@ -266,7 +269,7 @@ protected:
 	bool parseMaterial(UrdfMaterial& material, tinyxml2::XMLElement *config, ErrorLogger* logger);
 	bool parseJointLimits(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
     bool parseJointDynamics(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
-	bool parseJoint(UrdfJoint& link, tinyxml2::XMLElement *config, ErrorLogger* logger);
+	bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement *config, ErrorLogger* logger);
 	bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement *config, ErrorLogger* logger);
 	
     

examples/OpenGLWindow/Shaders/projectiveTextureInstancingPS.glsl

@@ -6,13 +6,9 @@ in Fragment
      vec4 color;
 } fragment;
 
-in Vert
-{
-	vec2 texcoord;
-} vert;
-
 uniform sampler2D Diffuse;
 uniform mat4 ViewMatrixInverse;
+uniform mat4 TextureMVP;
 
 in vec3 lightPos,cameraPosition, normal,ambient;
 in vec4 vertexPos;
@@ -23,10 +19,11 @@ in vec3 materialSpecularColor;
 out vec4 color;
 
 
-
 void main(void)
 {
-	vec4 texel = fragment.color*texture(Diffuse,vert.texcoord.xy);
+    vec4 projcoords = TextureMVP * vertexPos;
+    vec2 texturecoords = projcoords.xy/max(projcoords.z,0.1);
+	vec4 texel = fragment.color*texture(Diffuse,texturecoords);
 	vec3 ct,cf;
 	float intensity,at,af;
 	if (fragment.color.w==0)

examples/OpenGLWindow/Shaders/projectiveTextureInstancingPS.h

@@ -6,12 +6,9 @@ static const char* projectiveTextureInstancingFragmentShader= \
 "{\n"
 "     vec4 color;\n"
 "} fragment;\n"
-"in Vert\n"
-"{\n"
-"	vec2 texcoord;\n"
-"} vert;\n"
 "uniform sampler2D Diffuse;\n"
 "uniform mat4 ViewMatrixInverse;\n"
+"uniform mat4 TextureMVP;\n"
 "in vec3 lightPos,cameraPosition, normal,ambient;\n"
 "in vec4 vertexPos;\n"
 "in float materialShininess;\n"
@@ -20,7 +17,9 @@ static const char* projectiveTextureInstancingFragmentShader= \
 "out vec4 color;\n"
 "void main(void)\n"
 "{\n"
-"	vec4 texel = fragment.color*texture(Diffuse,vert.texcoord.xy);\n"
+"    vec4 projcoords = TextureMVP * vertexPos;\n"
+"    vec2 texturecoords = projcoords.xy/max(projcoords.z,0.1);\n"
+"	vec4 texel = fragment.color*texture(Diffuse,texturecoords);\n"
 "	vec3 ct,cf;\n"
 "	float intensity,at,af;\n"
 "	if (fragment.color.w==0)\n"

examples/OpenGLWindow/Shaders/projectiveTextureInstancingVS.glsl

@@ -93,7 +93,5 @@ void main(void)
 	gl_Position = vertexLoc;
 
 	fragment.color = instance_color;
-	vec4 projcoords = TextureMVP * vec4((instance_position+localcoord).xyz,1);
-	vert.texcoord = projcoords.xy/projcoords.z;
 }
 

examples/OpenGLWindow/Shaders/projectiveTextureInstancingVS.h

@@ -80,7 +80,5 @@ static const char* projectiveTextureInstancingVertexShader= \
 "	vec4 vertexLoc = MVP* vec4((instance_position+localcoord).xyz,1);\n"
 "	gl_Position = vertexLoc;\n"
 "	fragment.color = instance_color;\n"
-"	vec4 projcoords = TextureMVP * vec4((instance_position+localcoord).xyz,1);\n"
-"	vert.texcoord = projcoords.xy/projcoords.z;\n"
 "}\n"
 ;

examples/SharedMemory/CMakeLists.txt

@@ -211,7 +211,7 @@ IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
 ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
 
 
-#VR/OpenVR only on Windows and Mac OSX for now
+#VR/OpenVR on Windows and Mac OSX
 IF (WIN32 OR APPLE)
 
 INCLUDE_DIRECTORIES(
@@ -325,4 +325,80 @@ IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
                         SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_VR PROPERTIES  RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
 ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
 
+#VR/OpenVR on Linux
+ELSE(WIN32 OR APPLE)
+       IF(CMAKE_SIZEOF_VOID_P MATCHES 8)
+                LINK_DIRECTORIES(${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/bin/linux64)
+        ELSE()
+                LINK_DIRECTORIES(${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/bin/linux32)
+        ENDIF()
+
+        ADD_EXECUTABLE(App_PhysicsServer_SharedMemory_VR
+                ${SharedMemory_SRCS}
+               ../StandaloneMain/hellovr_opengl_main.cpp
+               ../ExampleBrowser/OpenGLGuiHelper.cpp
+               ../ExampleBrowser/GL_ShapeDrawer.cpp
+               ../ExampleBrowser/CollisionShape2TriangleMesh.cpp
+               ../RenderingExamples/TinyVRGui.cpp
+               ../RenderingExamples/TinyVRGui.h
+               ../RenderingExamples/TimeSeriesCanvas.cpp
+               ../RenderingExamples/TimeSeriesFontData.cpp
+               ../MultiThreading/b3PosixThreadSupport.cpp
+               ../MultiThreading/b3PosixThreadSupport.h
+               ../ThirdPartyLibs/openvr/samples/shared/lodepng.cpp
+               ../ThirdPartyLibs/openvr/samples/shared/lodepng.h
+               ../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp
+               ../ThirdPartyLibs/openvr/samples/shared/Matrices.h
+               ../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp
+               ../ThirdPartyLibs/openvr/samples/shared/pathtools.h
+               ../ThirdPartyLibs/openvr/samples/shared/strtools.cpp
+               ../ThirdPartyLibs/openvr/samples/shared/strtools.h
+               ../ThirdPartyLibs/openvr/samples/shared/Vectors.h
+        )
+
+       target_include_directories(App_PhysicsServer_SharedMemory_VR PRIVATE
+                ${BULLET_PHYSICS_SOURCE_DIR}/src
+                ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
+                ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew
+                ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/headers
+                ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/samples
+                ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/samples/shared
+       )
+
+       target_compile_definitions(App_PhysicsServer_SharedMemory_VR PRIVATE
+               POSIX
+               LINUX
+               BT_ENABLE_VR
+               GLEW_STATIC
+               GLEW_INIT_OPENGL11_FUNCTIONS=1
+               GLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1
+       )
+
+       target_compile_options(App_PhysicsServer_SharedMemory_VR PRIVATE
+               -std=c++11
+       )
+
+       target_link_libraries(App_PhysicsServer_SharedMemory_VR PRIVATE
+               openvr_api
+               pthread
+               ${DL}
+               ${OPENGL_gl_LIBRARY}
+               ${OPENGL_glu_LIBRARY}
+               Bullet3Common
+               BulletWorldImporter
+               BulletInverseDynamicsUtils
+               BulletInverseDynamics
+               BulletDynamics
+               BulletCollision
+               LinearMath
+               BussIK
+               OpenGLWindow
+       )
+
+       IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
+               SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_VR PROPERTIES  DEBUG_POSTFIX "_Debug")
+               SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_VR PROPERTIES  MINSIZEREL_POSTFIX "_MinsizeRel")
+               SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_VR PROPERTIES  RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
+       ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
+
 ENDIF(WIN32 OR APPLE)

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -597,7 +597,7 @@ struct CommandLogPlayback
 #ifdef BACKWARD_COMPAT	
 					 cmd->m_physSimParamArgs = unused.m_physSimParamArgs;
 					 #else
-					s= s = fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
+					s = fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
 					s = fread(&cmd->m_physSimParamArgs ,sizeof(b3PhysicsSimulationParameters),1,m_file);
 
 					 #endif
@@ -5010,9 +5010,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
 													{
 														con->enableMotor(3+limitAxis,true);
 														con->setTargetVelocity(3+limitAxis, qdotTarget);
-														//this is max motor force impulse
+														con->setMaxMotorForce(3+limitAxis, torque);
-														btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
-														con->setMaxMotorForce(3+limitAxis,torqueImpulse);
 													}
 													break;
 												}
@@ -5025,9 +5023,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
 														//next one is the maximum velocity to reach target position.
 														//the maximum velocity is limited by maxMotorForce
 														con->setTargetVelocity(3+limitAxis, 100);
-														//this is max motor force impulse
+														con->setMaxMotorForce(3+limitAxis, torque);
-														btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
-														con->setMaxMotorForce(3+limitAxis,torqueImpulse);
 														con->enableMotor(3+limitAxis,true);
 													}
 													break;
@@ -5063,9 +5059,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
 												{
 													con->enableMotor(limitAxis,true);
 													con->setTargetVelocity(limitAxis, -qdotTarget);
-													//this is max motor force impulse
+													con->setMaxMotorForce(limitAxis, torque);
-													btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
-													con->setMaxMotorForce(limitAxis,torqueImpulse);
 													break;
 												}
 											case CONTROL_MODE_POSITION_VELOCITY_PD:
@@ -5075,9 +5069,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
 													//next one is the maximum velocity to reach target position.
 													//the maximum velocity is limited by maxMotorForce
 													con->setTargetVelocity(limitAxis, 100);
-													//this is max motor force impulse
+													con->setMaxMotorForce(limitAxis, torque);
-													btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
-													con->setMaxMotorForce(limitAxis,torqueImpulse);
 													con->enableMotor(limitAxis,true);
 													break;
 												}

examples/StandaloneMain/hellovr_opengl_main.cpp

@@ -59,6 +59,9 @@ static vr::VRControllerState_t sPrevStates[vr::k_unMaxTrackedDeviceCount] = { 0
 #ifdef _WIN32
 #include <Windows.h>
 #endif
+#ifdef __linux__
+#define APIENTRY
+#endif
 
 void ThreadSleep( unsigned long nMilliseconds )
 {
@@ -1804,12 +1807,10 @@ void CMainApplication::RenderStereoTargets()
 	}
 
 	glBindFramebuffer( GL_FRAMEBUFFER, leftEyeDesc.m_nRenderFramebufferId );
- 	glViewport(0, 0, m_nRenderWidth, m_nRenderHeight );
- 	
-	
  	
 
 	m_app->m_window->startRendering();
+        glViewport(0, 0, m_nRenderWidth, m_nRenderHeight );
 
 
 	RenderScene( vr::Eye_Left );
@@ -1863,9 +1864,9 @@ void CMainApplication::RenderStereoTargets()
 	}
 	
 	glBindFramebuffer( GL_FRAMEBUFFER, rightEyeDesc.m_nRenderFramebufferId );
- 	glViewport(0, 0, m_nRenderWidth, m_nRenderHeight );
  	
 	m_app->m_window->startRendering();
+        glViewport(0, 0, m_nRenderWidth, m_nRenderHeight );
 	
 	RenderScene( vr::Eye_Right );
 	
@@ -2081,6 +2082,7 @@ void CMainApplication::UpdateHMDMatrixPose()
 					case vr::TrackedDeviceClass_HMD:               m_rDevClassChar[nDevice] = 'H'; break;
 					case vr::TrackedDeviceClass_Invalid:           m_rDevClassChar[nDevice] = 'I'; break;
 					case vr::TrackedDeviceClass_TrackingReference: m_rDevClassChar[nDevice] = 'T'; break;
+                                        case vr::TrackedDeviceClass_GenericTracker:    m_rDevClassChar[nDevice] = 'G'; break;
 					default:                                       m_rDevClassChar[nDevice] = '?'; break;
 					}
 				}

examples/ThirdPartyLibs/openvr/samples/shared/strtools.cpp

@@ -4,6 +4,12 @@
 #include <stdio.h>
 #include <stdlib.h>
 
+#ifdef __linux__
+#define stricmp strcasecmp
+#define strnicmp strncasecmp
+#endif
+
+
 //-----------------------------------------------------------------------------
 // Purpose:
 //-----------------------------------------------------------------------------

examples/Tutorial/Dof6ConstraintTutorial.cpp

@@ -313,11 +313,11 @@ void Dof6ConstraintTutorial::initPhysics()
 		#ifdef USE_6DOF2
 			constraint->enableMotor(5,true);
 			constraint->setTargetVelocity(5,3.f);
-			constraint->setMaxMotorForce(5,10.f);
+			constraint->setMaxMotorForce(5,600.f);
 		#else
 			constraint->getRotationalLimitMotor(2)->m_enableMotor = true;
 			constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f;
-			constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10;
+			constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 600.f;
 		#endif
 			constraint->setDbgDrawSize(btScalar(2.f));
 			m_dynamicsWorld->addConstraint(constraint, true);
@@ -348,13 +348,13 @@ void Dof6ConstraintTutorial::initPhysics()
 		#ifdef USE_6DOF2
 			constraint->enableMotor(5,true);
 			constraint->setTargetVelocity(5,3.f);
-			constraint->setMaxMotorForce(5,10.f);
+			constraint->setMaxMotorForce(5,600.f);
 			constraint->setServo(5,true);
 			constraint->setServoTarget(5, M_PI_2);
 		#else
 			constraint->getRotationalLimitMotor(2)->m_enableMotor = true;
 			constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f;
-			constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10;
+			constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 600.f;
 			//servo motor is not implemented in 6dofspring constraint
 		#endif
 			constraint->setDbgDrawSize(btScalar(2.f));

examples/pybullet/examples/projective_texture.py

@@ -1,6 +1,5 @@
 import pybullet as p
 from time import sleep
-from PIL import Image
 import matplotlib.pyplot as plt
 import numpy as np
 
@@ -9,7 +8,7 @@ physicsClient = p.connect(p.GUI)
 p.setGravity(0,0,0)
 bearStartPos1 = [-3.3,0,0]
 bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
-bearId1 = p.loadURDF("teddy_large.urdf", bearStartPos1, bearStartOrientation1)
+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)

examples/pybullet/examples/signedDistanceField.py

@@ -0,0 +1,15 @@
+import pybullet as p
+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.loadURDF("duck_vhacd.urdf")
+timeStep = 1./240.
+p.setTimeStep(timeStep)
+while (1):
+	p.stepSimulation()
+	time.sleep(timeStep)
+	

examples/pybullet/gym/pybullet_envs/ARS/ars.py

@@ -0,0 +1,397 @@
+"""Internal implementation of the Augmented Random Search method."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import os, inspect
+currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
+os.sys.path.insert(0,currentdir)
+
+from concurrent import futures
+import copy
+import os
+import time
+import gym
+import numpy as np
+import logz
+import utils
+import optimizers
+#from google3.pyglib import gfile
+import policies
+import shared_noise
+import utility
+
+class Worker(object):
+  """Object class for parallel rollout generation."""
+
+  def __init__(self,
+               env_seed,
+               env_callback,
+               policy_params=None,
+               deltas=None,
+               rollout_length=1000,
+               delta_std=0.02):
+
+    # initialize OpenAI environment for each worker
+    self.env = env_callback()
+    self.env.seed(env_seed)
+
+    # each worker gets access to the shared noise table
+    # with independent random streams for sampling
+    # from the shared noise table.
+    self.deltas = shared_noise.SharedNoiseTable(deltas, env_seed + 7)
+    self.policy_params = policy_params
+    if policy_params['type'] == 'linear':
+      self.policy = policies.LinearPolicy(policy_params)
+    else:
+      raise NotImplementedError
+
+    self.delta_std = delta_std
+    self.rollout_length = rollout_length
+
+  def get_weights_plus_stats(self):
+    """
+        Get current policy weights and current statistics of past states.
+        """
+    assert self.policy_params['type'] == 'linear'
+    return self.policy.get_weights_plus_stats()
+
+  def rollout(self, shift=0., rollout_length=None):
+    """Performs one rollout of maximum length rollout_length.
+
+    At each time-step it substracts shift from the reward.
+    """
+
+    if rollout_length is None:
+      rollout_length = self.rollout_length
+
+    total_reward = 0.
+    steps = 0
+
+    ob = self.env.reset()
+    for i in range(rollout_length):
+      action = self.policy.act(ob)
+      ob, reward, done, _ = self.env.step(action)
+      steps += 1
+      total_reward += (reward - shift)
+      if done:
+        break
+
+    return total_reward, steps
+
+  def do_rollouts(self, w_policy, num_rollouts=1, shift=1, evaluate=False):
+    """
+        Generate multiple rollouts with a policy parametrized by w_policy.
+        """
+    print('Doing {} rollouts'.format(num_rollouts))
+    rollout_rewards, deltas_idx = [], []
+    steps = 0
+
+    for i in range(num_rollouts):
+
+      if evaluate:
+        self.policy.update_weights(w_policy)
+        deltas_idx.append(-1)
+
+        # set to false so that evaluation rollouts are not used for updating state statistics
+        self.policy.update_filter = False
+
+        # for evaluation we do not shift the rewards (shift = 0) and we use the
+        # default rollout length (1000 for the MuJoCo locomotion tasks)
+        reward, r_steps = self.rollout(
+            shift=0., rollout_length=self.rollout_length)
+        rollout_rewards.append(reward)
+
+      else:
+        idx, delta = self.deltas.get_delta(w_policy.size)
+
+        delta = (self.delta_std * delta).reshape(w_policy.shape)
+        deltas_idx.append(idx)
+
+        # set to true so that state statistics are updated
+        self.policy.update_filter = True
+
+        # compute reward and number of timesteps used for positive perturbation rollout
+        self.policy.update_weights(w_policy + delta)
+        pos_reward, pos_steps = self.rollout(shift=shift)
+
+        # compute reward and number of timesteps used for negative pertubation rollout
+        self.policy.update_weights(w_policy - delta)
+        neg_reward, neg_steps = self.rollout(shift=shift)
+        steps += pos_steps + neg_steps
+
+        rollout_rewards.append([pos_reward, neg_reward])
+
+    return {
+        'deltas_idx': deltas_idx,
+        'rollout_rewards': rollout_rewards,
+        'steps': steps
+    }
+
+  def stats_increment(self):
+    self.policy.observation_filter.stats_increment()
+    return
+
+  def get_weights(self):
+    return self.policy.get_weights()
+
+  def get_filter(self):
+    return self.policy.observation_filter
+
+  def sync_filter(self, other):
+    self.policy.observation_filter.sync(other)
+    return
+
+
+class ARSLearner(object):
+  """
+    Object class implementing the ARS algorithm.
+    """
+
+  def __init__(self,
+               env_callback,
+               policy_params=None,
+               num_workers=32,
+               num_deltas=320,
+               deltas_used=320,
+               delta_std=0.02,
+               logdir=None,
+               rollout_length=1000,
+               step_size=0.01,
+               shift='constant zero',
+               params=None,
+               seed=123):
+
+    logz.configure_output_dir(logdir)
+    # params_to_save = copy.deepcopy(params)
+    # params_to_save['env'] = None
+    # logz.save_params(params_to_save)
+    utility.save_config(params, logdir)
+    env = env_callback()
+
+    self.timesteps = 0
+    self.action_size = env.action_space.shape[0]
+    self.ob_size = env.observation_space.shape[0]
+    self.num_deltas = num_deltas
+    self.deltas_used = deltas_used
+    self.rollout_length = rollout_length
+    self.step_size = step_size
+    self.delta_std = delta_std
+    self.logdir = logdir
+    self.shift = shift
+    self.params = params
+    self.max_past_avg_reward = float('-inf')
+    self.num_episodes_used = float('inf')
+
+    # create shared table for storing noise
+    print('Creating deltas table.')
+    deltas = shared_noise.create_shared_noise()
+    self.deltas = shared_noise.SharedNoiseTable(deltas, seed=seed + 3)
+    print('Created deltas table.')
+
+    # initialize workers with different random seeds
+    print('Initializing workers.')
+    self.num_workers = num_workers
+    self.workers = [
+        Worker(
+            seed + 7 * i,
+            env_callback=env_callback,
+            policy_params=policy_params,
+            deltas=deltas,
+            rollout_length=rollout_length,
+            delta_std=delta_std) for i in range(num_workers)
+    ]
+
+    # initialize policy
+    if policy_params['type'] == 'linear':
+      self.policy = policies.LinearPolicy(policy_params)
+      self.w_policy = self.policy.get_weights()
+    else:
+      raise NotImplementedError
+
+    # initialize optimization algorithm
+    self.optimizer = optimizers.SGD(self.w_policy, self.step_size)
+    print('Initialization of ARS complete.')
+
+  def aggregate_rollouts(self, num_rollouts=None, evaluate=False):
+    """
+        Aggregate update step from rollouts generated in parallel.
+        """
+
+    if num_rollouts is None:
+      num_deltas = self.num_deltas
+    else:
+      num_deltas = num_rollouts
+
+    results_one = []  #rollout_ids_one
+    results_two = []  #rollout_ids_two
+
+    t1 = time.time()
+    num_rollouts = int(num_deltas / self.num_workers)
+#     if num_rollouts > 0:
+#       with futures.ThreadPoolExecutor(
+#           max_workers=self.num_workers) as executor:
+#         workers = [
+#             executor.submit(
+#                 worker.do_rollouts,
+#                 self.w_policy,
+#                 num_rollouts=num_rollouts,
+#                 shift=self.shift,
+#                 evaluate=evaluate) for worker in self.workers
+#         ]
+#         for worker in futures.as_completed(workers):
+#           results_one.append(worker.result())
+#
+#       workers = [
+#           executor.submit(
+#               worker.do_rollouts,
+#               self.w_policy,
+#               num_rollouts=1,
+#               shift=self.shift,
+#               evaluate=evaluate)
+#           for worker in self.workers[:(num_deltas % self.num_workers)]
+#       ]
+#       for worker in futures.as_completed(workers):
+#         results_two.append(worker.result())
+
+    # parallel generation of rollouts
+    rollout_ids_one = [
+        worker.do_rollouts(
+            self.w_policy,
+            num_rollouts=num_rollouts,
+            shift=self.shift,
+            evaluate=evaluate) for worker in self.workers
+    ]
+
+    rollout_ids_two = [
+        worker.do_rollouts(
+            self.w_policy, num_rollouts=1, shift=self.shift, evaluate=evaluate)
+        for worker in self.workers[:(num_deltas % self.num_workers)]
+    ]
+    results_one = rollout_ids_one
+    results_two = rollout_ids_two
+# gather results
+
+    rollout_rewards, deltas_idx = [], []
+
+    for result in results_one:
+      if not evaluate:
+        self.timesteps += result['steps']
+      deltas_idx += result['deltas_idx']
+      rollout_rewards += result['rollout_rewards']
+
+    for result in results_two:
+      if not evaluate:
+        self.timesteps += result['steps']
+      deltas_idx += result['deltas_idx']
+      rollout_rewards += result['rollout_rewards']
+
+    deltas_idx = np.array(deltas_idx)
+    rollout_rewards = np.array(rollout_rewards, dtype=np.float64)
+
+    print('Maximum reward of collected rollouts:', rollout_rewards.max())
+    info_dict = {
+        "max_reward": rollout_rewards.max()
+    }
+    t2 = time.time()
+
+    print('Time to generate rollouts:', t2 - t1)
+
+    if evaluate:
+      return rollout_rewards
+
+    # select top performing directions if deltas_used < num_deltas
+    max_rewards = np.max(rollout_rewards, axis=1)
+    if self.deltas_used > self.num_deltas:
+      self.deltas_used = self.num_deltas
+
+    idx = np.arange(max_rewards.size)[max_rewards >= np.percentile(
+        max_rewards, 100 * (1 - (self.deltas_used / self.num_deltas)))]
+    deltas_idx = deltas_idx[idx]
+    rollout_rewards = rollout_rewards[idx, :]
+
+    # normalize rewards by their standard deviation
+    rollout_rewards /= np.std(rollout_rewards)
+
+    t1 = time.time()
+    # aggregate rollouts to form g_hat, the gradient used to compute SGD step
+    g_hat, count = utils.batched_weighted_sum(
+        rollout_rewards[:, 0] - rollout_rewards[:, 1],
+        (self.deltas.get(idx, self.w_policy.size) for idx in deltas_idx),
+        batch_size=500)
+    g_hat /= deltas_idx.size
+    t2 = time.time()
+    print('time to aggregate rollouts', t2 - t1)
+    return g_hat, info_dict
+
+  def train_step(self):
+    """
+        Perform one update step of the policy weights.
+        """
+
+    g_hat, info_dict = self.aggregate_rollouts()
+    print('Euclidean norm of update step:', np.linalg.norm(g_hat))
+    self.w_policy -= self.optimizer._compute_step(g_hat).reshape(
+        self.w_policy.shape)
+    return info_dict
+
+  def train(self, num_iter):
+
+    start = time.time()
+    for i in range(num_iter):
+
+      t1 = time.time()
+      info_dict = self.train_step()
+      t2 = time.time()
+      print('total time of one step', t2 - t1)
+      print('iter ', i, ' done')
+
+      # record statistics every 10 iterations
+      if ((i) % 10 == 0):
+
+        rewards = self.aggregate_rollouts(num_rollouts=8, evaluate=True)
+        w = self.workers[0].get_weights_plus_stats()
+
+        checkpoint_filename = os.path.join(
+            self.logdir, 'lin_policy_plus_{:03d}.npz'.format(i))
+        print('Save checkpoints to {}...', checkpoint_filename)
+        checkpoint_file = open(checkpoint_filename, 'w')
+        np.savez(checkpoint_file, w)
+        print('End save checkpoints.')
+        print(sorted(self.params.items()))
+        logz.log_tabular('Time', time.time() - start)
+        logz.log_tabular('Iteration', i + 1)
+        logz.log_tabular('AverageReward', np.mean(rewards))
+        logz.log_tabular('StdRewards', np.std(rewards))
+        logz.log_tabular('MaxRewardRollout', np.max(rewards))
+        logz.log_tabular('MinRewardRollout', np.min(rewards))
+        logz.log_tabular('timesteps', self.timesteps)
+        logz.dump_tabular()
+
+      t1 = time.time()
+      # get statistics from all workers
+      for j in range(self.num_workers):
+        self.policy.observation_filter.update(self.workers[j].get_filter())
+      self.policy.observation_filter.stats_increment()
+
+      # make sure master filter buffer is clear
+      self.policy.observation_filter.clear_buffer()
+      # sync all workers
+      #filter_id = ray.put(self.policy.observation_filter)
+      setting_filters_ids = [
+          worker.sync_filter(self.policy.observation_filter)
+          for worker in self.workers
+      ]
+      # waiting for sync of all workers
+      #ray.get(setting_filters_ids)
+
+      increment_filters_ids = [
+          worker.stats_increment() for worker in self.workers
+      ]
+      # waiting for increment of all workers
+      #ray.get(increment_filters_ids)
+      t2 = time.time()
+      print('Time to sync statistics:', t2 - t1)
+
+    return info_dict

examples/pybullet/gym/pybullet_envs/ARS/ars_server.py

@@ -0,0 +1,62 @@
+"""
+blaze build -c opt //experimental/users/jietan/ARS:ars_server
+
+blaze-bin/experimental/users/jietan/ARS/ars_server \
+--config_name=MINITAUR_GYM_CONFIG
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import time
+from absl import app
+from absl import flags
+from concurrent import futures
+import grpc
+from grpc import loas2
+from google3.robotics.reinforcement_learning.minitaur.envs import minitaur_gym_env
+from google3.robotics.reinforcement_learning.minitaur.envs import minitaur_reactive_env
+from google3.robotics.reinforcement_learning.minitaur.envs.env_randomizers import minitaur_env_randomizer
+from google3.robotics.reinforcement_learning.minitaur.envs.env_randomizers import minitaur_env_randomizer_from_config as randomizer_config_lib
+from google3.experimental.users.jietan.ARS import ars_evaluation_service_pb2_grpc
+from google3.experimental.users.jietan.ARS import ars_evaluation_service
+
+FLAGS = flags.FLAGS
+flags.DEFINE_integer("server_id", 0, "number of servers")
+flags.DEFINE_integer("port", 20000, "port number.")
+flags.DEFINE_string("config_name", None, "The name of the config dictionary.")
+flags.DEFINE_bool('run_on_borg', False,
+                  'Whether the servers are running on borg.')
+
+_ONE_DAY_IN_SECONDS = 60 * 60 * 24
+
+
+def main(unused_argv):
+  servers = []
+  server_creds = loas2.loas2_server_credentials()
+  port = FLAGS.port
+  if not FLAGS.run_on_borg:
+    port = 20000 + FLAGS.server_id
+  server = grpc.server(
+      futures.ThreadPoolExecutor(max_workers=10), ports=(port,))
+  servicer = ars_evaluation_service.ParameterEvaluationServicer(
+      FLAGS.config_name, worker_id=FLAGS.server_id)
+  ars_evaluation_service_pb2_grpc.add_EvaluationServicer_to_server(
+      servicer, server)
+  server.add_secure_port("[::]:{}".format(port), server_creds)
+  servers.append(server)
+  server.start()
+  print("Start server {}".format(FLAGS.server_id))
+
+  # prevent the main thread from exiting
+  try:
+    while True:
+      time.sleep(_ONE_DAY_IN_SECONDS)
+  except KeyboardInterrupt:
+    for server in servers:
+      server.stop(0)
+
+
+if __name__ == "__main__":
+  app.run(main)

examples/pybullet/gym/pybullet_envs/ARS/config_ars.py

@@ -0,0 +1,83 @@
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import functools
+from pybullet_envs.minitaur.envs import minitaur_gym_env
+from pybullet_envs.minitaur.envs import minitaur_reactive_env
+from pybullet_envs.minitaur.envs.env_randomizers import minitaur_env_randomizer
+from pybullet_envs.minitaur.envs.env_randomizers import minitaur_env_randomizer_from_config as randomizer_config_lib
+
+MAX_LENGTH = 1000
+
+
+def merge_two_dicts(x, y):
+  """Given two dicts, merge them into a new dict as a shallow copy."""
+  z = dict(x)
+  z.update(y)
+  return z
+
+
+# The default configurations.
+DEFAULT_CONFIG = dict(
+    num_workers=8,
+    num_directions=8,
+    num_iterations=1000,
+    deltas_used=8,
+    step_size=0.02,
+    delta_std=0.03,
+    rollout_length=MAX_LENGTH,
+    shift=0,
+    seed=237,
+    policy_type="linear",
+    filter="MeanStdFilter",
+)
+
+# Configuration specific to minitaur_gym_env.MinitaurGymEnv class.
+MINITAUR_GYM_CONFIG_ADDITIONS = dict(
+    env=functools.partial(
+        minitaur_gym_env.MinitaurGymEnv,
+        urdf_version=minitaur_gym_env.DERPY_V0_URDF_VERSION,
+        accurate_motor_model_enabled=True,
+        motor_overheat_protection=True,
+        pd_control_enabled=True,
+        env_randomizer=None,#minitaur_env_randomizer.MinitaurEnvRandomizer(),
+        render=False,
+        num_steps_to_log=MAX_LENGTH))
+MINITAUR_GYM_CONFIG = merge_two_dicts(DEFAULT_CONFIG,
+                                      MINITAUR_GYM_CONFIG_ADDITIONS)
+
+# Configuration specific to MinitaurReactiveEnv class.
+MINITAUR_REACTIVE_CONFIG_ADDITIONS = dict(
+    env=functools.partial(
+        minitaur_reactive_env.MinitaurReactiveEnv,
+        urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
+        energy_weight=0.005,
+        accurate_motor_model_enabled=True,
+        pd_latency=0.003,
+        control_latency=0.02,
+        motor_kd=0.015,
+        remove_default_joint_damping=True,
+        env_randomizer=None,
+        render=False,
+        num_steps_to_log=MAX_LENGTH))
+MINITAUR_REACTIVE_CONFIG = merge_two_dicts(DEFAULT_CONFIG,
+                                           MINITAUR_REACTIVE_CONFIG_ADDITIONS)
+
+# Configuration specific to MinitaurReactiveEnv class with randomizer.
+MINITAUR_REACTIVE_RANDOMIZER_CONFIG_ADDITIONS = dict(
+    env=functools.partial(
+        minitaur_reactive_env.MinitaurReactiveEnv,
+        urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
+        energy_weight=0.005,
+        accurate_motor_model_enabled=True,
+        pd_latency=0.003,
+        control_latency=0.02,
+        motor_kd=0.015,
+        remove_default_joint_damping=True,
+        env_randomizer=randomizer_config_lib.MinitaurEnvRandomizerFromConfig(),
+        render=False,
+        num_steps_to_log=MAX_LENGTH))
+MINITAUR_REACTIVE_RANDOMIZER_CONFIG = merge_two_dicts(
+    DEFAULT_CONFIG, MINITAUR_REACTIVE_RANDOMIZER_CONFIG_ADDITIONS)

examples/pybullet/gym/pybullet_envs/ARS/eval_ars.py

@@ -0,0 +1,99 @@
+"""
+blaze run -c opt //experimental/users/jietan/ARS:eval_ars -- \
+--logdir=/cns/ij-d/home/jietan/experiment/ARS/ars_react_nr01.191950338.191950550/ \
+--checkpoint=lin_policy_plus_990.npz \
+--num_rollouts=10
+"""
+
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import os, inspect
+import time
+
+currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
+os.sys.path.insert(0,currentdir)
+
+from absl import app
+from absl import flags
+
+import pdb
+import os
+import numpy as np
+import gym
+import config_ars
+import utility
+import policies
+
+FLAGS = flags.FLAGS
+
+flags.DEFINE_string('logdir', None, 'The path of the checkpoint.')
+flags.DEFINE_string('checkpoint', None, 'The file name of the checkpoint.')
+flags.DEFINE_integer('num_rollouts', 1, 'The number of rollouts.')
+
+
+def main(argv):
+  del argv  # Unused.
+
+  print('loading and building expert policy')
+  checkpoint_file = os.path.join(FLAGS.logdir, FLAGS.checkpoint)
+  lin_policy = np.load(checkpoint_file, encoding='bytes')
+  lin_policy = lin_policy.items()[0][1]
+
+  M = lin_policy[0]
+  # mean and std of state vectors estimated online by ARS.
+  mean = lin_policy[1]
+  std = lin_policy[2]
+
+  config = utility.load_config(FLAGS.logdir)
+  print("config=",config)
+  env = config['env'](hard_reset=True, render=True)
+  ob_dim = env.observation_space.shape[0]
+  ac_dim = env.action_space.shape[0]
+
+  # set policy parameters. Possible filters: 'MeanStdFilter' for v2, 'NoFilter' for v1.
+  policy_params = {
+      'type': 'linear',
+      'ob_filter': config['filter'],
+      'ob_dim': ob_dim,
+      'ac_dim': ac_dim,
+      "weights": M,
+      "mean": mean,
+      "std": std,
+  }
+  policy = policies.LinearPolicy(policy_params, update_filter=False)
+  returns = []
+  observations = []
+  actions = []
+  for i in range(FLAGS.num_rollouts):
+    print('iter', i)
+    obs = env.reset()
+    done = False
+    totalr = 0.
+    steps = 0
+    while not done:
+      action = policy.act(obs)
+      observations.append(obs)
+      actions.append(action)
+
+      obs, r, done, _ = env.step(action)
+      time.sleep(1./100.)
+      totalr += r
+      steps += 1
+      if steps % 100 == 0:
+        print('%i/%i' % (steps, config['rollout_length']))
+      if steps >= config['rollout_length']:
+        break
+    returns.append(totalr)
+
+  print('returns', returns)
+  print('mean return', np.mean(returns))
+  print('std of return', np.std(returns))
+
+
+if __name__ == '__main__':
+  flags.mark_flag_as_required('logdir')
+  flags.mark_flag_as_required('checkpoint')
+  app.run(main)

examples/pybullet/gym/pybullet_envs/ARS/filter.py

@@ -0,0 +1,280 @@
+# Code in this file is copied and adapted from
+# https://github.com/ray-project/ray/blob/master/python/ray/rllib/utils/filter.py
+
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import numpy as np
+
+
+class Filter(object):
+    """Processes input, possibly statefully."""
+
+    def update(self, other, *args, **kwargs):
+        """Updates self with "new state" from other filter."""
+        raise NotImplementedError
+
+    def copy(self):
+        """Creates a new object with same state as self.
+
+        Returns:
+            copy (Filter): Copy of self"""
+        raise NotImplementedError
+
+    def sync(self, other):
+        """Copies all state from other filter to self."""
+        raise NotImplementedError
+
+
+class NoFilter(Filter):
+    def __init__(self, *args):
+        pass
+
+    def __call__(self, x, update=True):
+        return np.asarray(x, dtype = np.float64)
+
+    def update(self, other, *args, **kwargs):
+        pass
+
+    def copy(self):
+        return self
+
+    def sync(self, other):
+        pass
+
+    def stats_increment(self):
+        pass
+
+    def clear_buffer(self):
+        pass
+
+    def get_stats(self):
+        return 0, 1
+
+    @property
+    def mean(self):
+        return 0
+
+    @property
+    def var(self):
+        return 1
+
+    @property
+    def std(self):
+        return 1
+
+
+
+# http://www.johndcook.com/blog/standard_deviation/
+class RunningStat(object):
+
+    def __init__(self, shape=None):
+        self._n = 0
+        self._M = np.zeros(shape, dtype = np.float64)
+        self._S = np.zeros(shape,  dtype = np.float64)
+        self._M2 = np.zeros(shape,  dtype = np.float64)
+
+    def copy(self):
+        other = RunningStat()
+        other._n = self._n
+        other._M = np.copy(self._M)
+        other._S = np.copy(self._S)
+        return other
+
+    def push(self, x):
+        x = np.asarray(x)
+        # Unvectorized update of the running statistics.
+        assert x.shape == self._M.shape, ("x.shape = {}, self.shape = {}"
+                                          .format(x.shape, self._M.shape))
+        n1 = self._n
+        self._n += 1
+        if self._n == 1:
+            self._M[...] = x
+        else:
+            delta = x - self._M
+            deltaM2 = np.square(x) - self._M2
+            self._M[...] += delta / self._n
+            self._S[...] += delta * delta * n1 / self._n
+            
+
+    def update(self, other):
+        n1 = self._n
+        n2 = other._n
+        n = n1 + n2
+        delta = self._M - other._M
+        delta2 = delta * delta
+        M = (n1 * self._M + n2 * other._M) / n
+        S = self._S + other._S + delta2 * n1 * n2 / n
+        self._n = n
+        self._M = M
+        self._S = S
+
+    def __repr__(self):
+        return '(n={}, mean_mean={}, mean_std={})'.format(
+            self.n, np.mean(self.mean), np.mean(self.std))
+
+    @property
+    def n(self):
+        return self._n
+
+    @property
+    def mean(self):
+        return self._M
+
+    @property
+    def var(self):
+        return self._S / (self._n - 1) if self._n > 1 else np.square(self._M)
+
+    @property
+    def std(self):
+        return np.sqrt(self.var)
+
+    @property
+    def shape(self):
+        return self._M.shape
+
+
+class MeanStdFilter(Filter):
+    """Keeps track of a running mean for seen states"""
+
+    def __init__(self, shape, demean=True, destd=True):
+        self.shape = shape
+        self.demean = demean
+        self.destd = destd
+        self.rs = RunningStat(shape)
+        # In distributed rollouts, each worker sees different states.
+        # The buffer is used to keep track of deltas amongst all the
+        # observation filters.
+
+        self.buffer = RunningStat(shape)
+
+        self.mean = np.zeros(shape, dtype = np.float64)
+        self.std = np.ones(shape, dtype = np.float64)
+
+    def clear_buffer(self):
+        self.buffer = RunningStat(self.shape)
+        return
+
+    def update(self, other, copy_buffer=False):
+        """Takes another filter and only applies the information from the
+        buffer.
+
+        Using notation `F(state, buffer)`
+        Given `Filter1(x1, y1)` and `Filter2(x2, yt)`,
+        `update` modifies `Filter1` to `Filter1(x1 + yt, y1)`
+        If `copy_buffer`, then `Filter1` is modified to
+        `Filter1(x1 + yt, yt)`.
+        """
+        self.rs.update(other.buffer)
+        if copy_buffer:
+            self.buffer = other.buffer.copy()
+        return 
+
+    def copy(self):
+        """Returns a copy of Filter."""
+        other = MeanStdFilter(self.shape)
+        other.demean = self.demean
+        other.destd = self.destd
+        other.rs = self.rs.copy()
+        other.buffer = self.buffer.copy()
+        return other
+
+    def sync(self, other):
+        """Syncs all fields together from other filter.
+
+        Using notation `F(state, buffer)`
+        Given `Filter1(x1, y1)` and `Filter2(x2, yt)`,
+        `sync` modifies `Filter1` to `Filter1(x2, yt)`
+        """
+        assert other.shape == self.shape, "Shapes don't match!"
+        self.demean = other.demean
+        self.destd = other.destd
+        self.rs = other.rs.copy()
+        self.buffer = other.buffer.copy()
+        return
+
+    def __call__(self, x, update=True):
+        x = np.asarray(x, dtype = np.float64)
+        if update:
+            if len(x.shape) == len(self.rs.shape) + 1:
+                # The vectorized case.
+                for i in range(x.shape[0]):
+                    self.rs.push(x[i])
+                    self.buffer.push(x[i])
+            else:
+                # The unvectorized case.
+                self.rs.push(x)
+                self.buffer.push(x)
+        if self.demean:
+            x = x - self.mean
+        if self.destd:
+            x = x / (self.std + 1e-8)
+        return x
+
+    def stats_increment(self):
+        self.mean = self.rs.mean
+        self.std = self.rs.std
+
+        # Set values for std less than 1e-7 to +inf to avoid 
+        # dividing by zero. State elements with zero variance
+        # are set to zero as a result. 
+        self.std[self.std < 1e-7] = float("inf") 
+        return
+
+    def get_stats(self):
+        return self.rs.mean, (self.rs.std + 1e-8)
+
+    def __repr__(self):
+        return 'MeanStdFilter({}, {}, {}, {}, {}, {})'.format(
+            self.shape, self.demean,
+            self.rs, self.buffer)
+
+    
+def get_filter(filter_config, shape = None):
+    if filter_config == "MeanStdFilter":
+        return MeanStdFilter(shape)
+    elif filter_config == "NoFilter":
+        return NoFilter()
+    else:
+        raise Exception("Unknown observation_filter: " +
+                        str(filter_config))
+
+
+def test_running_stat():
+    for shp in ((), (3,), (3, 4)):
+        li = []
+        rs = RunningStat(shp)
+        for _ in range(5):
+            val = np.random.randn(*shp)
+            rs.push(val)
+            li.append(val)
+            m = np.mean(li, axis=0)
+            assert np.allclose(rs.mean, m)
+            v = np.square(m) if (len(li) == 1) else np.var(li, ddof=1, axis=0)
+            assert np.allclose(rs.var, v)
+
+
+def test_combining_stat():
+    for shape in [(), (3,), (3, 4)]:
+        li = []
+        rs1 = RunningStat(shape)
+        rs2 = RunningStat(shape)
+        rs = RunningStat(shape)
+        for _ in range(5):
+            val = np.random.randn(*shape)
+            rs1.push(val)
+            rs.push(val)
+            li.append(val)
+        for _ in range(9):
+            rs2.push(val)
+            rs.push(val)
+            li.append(val)
+        rs1.update(rs2)
+        assert np.allclose(rs.mean, rs1.mean)
+        assert np.allclose(rs.std, rs1.std)
+
+
+test_running_stat()
+test_combining_stat()

examples/pybullet/gym/pybullet_envs/ARS/log/config.yaml

@@ -0,0 +1,29 @@
+delta_std: 0.03
+deltas_used: 8
+env: !!python/object/apply:functools.partial
+  args:
+  - &id001 !!python/name:pybullet_envs.minitaur.envs.minitaur_reactive_env.MinitaurReactiveEnv ''
+  state: !!python/tuple
+  - *id001
+  - !!python/tuple []
+  - accurate_motor_model_enabled: true
+    control_latency: 0.02
+    energy_weight: 0.005
+    env_randomizer: null
+    motor_kd: 0.015
+    num_steps_to_log: 1000
+    pd_latency: 0.003
+    remove_default_joint_damping: true
+    render: false
+    urdf_version: rainbow_dash_v0
+  - null
+filter: MeanStdFilter
+num_directions: 8
+num_iterations: 1000
+num_workers: 8
+policy_type: linear
+rollout_length: 1000
+seed: 237
+shift: 0
+step_size: 0.02
+

examples/pybullet/gym/pybullet_envs/ARS/logz.py

@@ -0,0 +1,104 @@
+# Code in this file is copied and adapted from 
+# https://github.com/berkeleydeeprlcourse
+
+import json
+
+"""
+
+Some simple logging functionality, inspired by rllab's logging.
+Assumes that each diagnostic gets logged each iteration
+
+Call logz.configure_output_dir() to start logging to a 
+tab-separated-values file (some_folder_name/log.txt)
+
+"""
+
+import os.path as osp, shutil, time, atexit, os, subprocess
+
+color2num = dict(
+    gray=30,
+    red=31,
+    green=32,
+    yellow=33,
+    blue=34,
+    magenta=35,
+    cyan=36,
+    white=37,
+    crimson=38
+)
+
+def colorize(string, color, bold=False, highlight=False):
+    attr = []
+    num = color2num[color]
+    if highlight: num += 10
+    attr.append(str(num))
+    if bold: attr.append('1')
+    return '\x1b[%sm%s\x1b[0m' % (';'.join(attr), string)
+
+class G(object):
+    output_dir = None
+    output_file = None
+    first_row = True
+    log_headers = []
+    log_current_row = {}
+
+def configure_output_dir(d=None):
+    """
+    Set output directory to d, or to /tmp/somerandomnumber if d is None
+    """
+    G.first_row = True
+    G.log_headers = []
+    G.log_current_row = {}
+    
+    G.output_dir = d or "/tmp/experiments/%i"%int(time.time())
+    if not osp.exists(G.output_dir):
+        os.makedirs(G.output_dir)
+    G.output_file = open(osp.join(G.output_dir, "log.txt"), 'w')
+    atexit.register(G.output_file.close)
+    print(colorize("Logging data to %s"%G.output_file.name, 'green', bold=True))
+
+def log_tabular(key, val):
+    """
+    Log a value of some diagnostic
+    Call this once for each diagnostic quantity, each iteration
+    """
+    if G.first_row:
+        G.log_headers.append(key)
+    else:
+        assert key in G.log_headers, "Trying to introduce a new key %s that you didn't include in the first iteration"%key
+    assert key not in G.log_current_row, "You already set %s this iteration. Maybe you forgot to call dump_tabular()"%key
+    G.log_current_row[key] = val
+
+    
+def save_params(params):
+    with open(osp.join(G.output_dir, "params.json"), 'w') as out:
+        out.write(json.dumps(params, separators=(',\n','\t:\t'), sort_keys=True))
+
+
+def dump_tabular():
+    """
+    Write all of the diagnostics from the current iteration
+    """
+    vals = []
+    key_lens = [len(key) for key in G.log_headers]
+    max_key_len = max(15,max(key_lens))
+    keystr = '%'+'%d'%max_key_len
+    fmt = "| " + keystr + "s | %15s |"
+    n_slashes = 22 + max_key_len
+    print("-"*n_slashes)
+    for key in G.log_headers:
+        val = G.log_current_row.get(key, "")
+        if hasattr(val, "__float__"): valstr = "%8.3g"%val
+        else: valstr = val
+        print(fmt%(key, valstr))
+        vals.append(val)
+    print("-"*n_slashes)
+    if G.output_file is not None:
+        if G.first_row:
+            G.output_file.write("\t".join(G.log_headers))
+            G.output_file.write("\n")
+        G.output_file.write("\t".join(map(str,vals)))
+        G.output_file.write("\n")
+        G.output_file.flush()
+    G.log_current_row.clear()
+    G.first_row=False

examples/pybullet/gym/pybullet_envs/ARS/optimizers.py

@@ -0,0 +1,35 @@
+# Code in this file is copied and adapted from
+# https://github.com/openai/evolution-strategies-starter.
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import numpy as np
+
+# OPTIMIZERS FOR MINIMIZING OBJECTIVES
+class Optimizer(object):
+    def __init__(self, w_policy):
+        self.w_policy = w_policy.flatten()
+        self.dim = w_policy.size
+        self.t = 0
+
+    def update(self, globalg):
+        self.t += 1
+        step = self._compute_step(globalg)
+        ratio = np.linalg.norm(step) / (np.linalg.norm(self.w_policy) + 1e-5)
+        return self.w_policy + step, ratio
+
+    def _compute_step(self, globalg):
+        raise NotImplementedError
+
+
+class SGD(Optimizer):
+    def __init__(self, pi, stepsize):
+        Optimizer.__init__(self, pi)
+        self.stepsize = stepsize
+
+    def _compute_step(self, globalg):
+        step = -self.stepsize * globalg
+        return step
+

examples/pybullet/gym/pybullet_envs/ARS/policies.py

@@ -0,0 +1,72 @@
+"""
+Policy class for computing action from weights and observation vector.
+Horia Mania --- hmania@berkeley.edu
+Aurelia Guy
+Benjamin Recht
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import numpy as np
+import filter
+
+
+class Policy(object):
+
+  def __init__(self, policy_params):
+
+    self.ob_dim = policy_params['ob_dim']
+    self.ac_dim = policy_params['ac_dim']
+    self.weights = np.empty(0)
+
+    # a filter for updating statistics of the observations and normalizing
+    # inputs to the policies
+    self.observation_filter = filter.get_filter(
+        policy_params['ob_filter'], shape=(self.ob_dim,))
+    self.update_filter = True
+
+  def update_weights(self, new_weights):
+    self.weights[:] = new_weights[:]
+    return
+
+  def get_weights(self):
+    return self.weights
+
+  def get_observation_filter(self):
+    return self.observation_filter
+
+  def act(self, ob):
+    raise NotImplementedError
+
+  def copy(self):
+    raise NotImplementedError
+
+
+class LinearPolicy(Policy):
+  """
+    Linear policy class that computes action as <w, ob>.
+    """
+
+  def __init__(self, policy_params, update_filter=True):
+    Policy.__init__(self, policy_params)
+    self.weights = np.zeros(self.ac_dim * self.ob_dim, dtype=np.float64)
+    if "weights" in policy_params:
+      self.weights = policy_params["weights"]
+    if "mean" in policy_params:
+      self.observation_filter.mean = policy_params["mean"]
+    if "std" in policy_params:
+      self.observation_filter.std = policy_params["std"]
+    self.update_filter = update_filter
+
+  def act(self, ob):
+    ob = self.observation_filter(ob, update=self.update_filter)
+    matrix_weights = np.reshape(self.weights, (self.ac_dim, self.ob_dim))
+    return np.clip(np.dot(matrix_weights, ob), -1.0, 1.0)
+
+  def get_weights_plus_stats(self):
+
+    mu, std = self.observation_filter.get_stats()
+    aux = np.asarray([self.weights, mu, std])
+    return aux

examples/pybullet/gym/pybullet_envs/ARS/shared_noise.py

@@ -0,0 +1,41 @@
+"""TODO(jietan): DO NOT SUBMIT without one-line documentation for shared_noise.
+Code in this file is copied and adapted from
+https://github.com/ray-project/ray/tree/master/python/ray/rllib/es
+TODO(jietan): DO NOT SUBMIT without a detailed description of shared_noise.
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+import numpy as np
+
+
+def create_shared_noise():
+    """
+    Create a large array of noise to be shared by all workers. Used
+    for avoiding the communication of the random perturbations delta.
+    """
+
+    seed = 12345
+    count = 250000000
+    noise = np.random.RandomState(seed).randn(count).astype(np.float64)
+    return noise
+
+
+class SharedNoiseTable(object):
+    def __init__(self, noise, seed = 11):
+
+        self.rg = np.random.RandomState(seed)
+        self.noise = noise
+        assert self.noise.dtype == np.float64
+
+    def get(self, i, dim):
+        return self.noise[i:i + dim]
+
+    def sample_index(self, dim):
+        return self.rg.randint(0, len(self.noise) - dim + 1)
+
+    def get_delta(self, dim):
+        idx = self.sample_index(dim)
+        return idx, self.get(idx, dim)
+

examples/pybullet/gym/pybullet_envs/ARS/start_ars_servers.py

@@ -0,0 +1,28 @@
+"""TODO(jietan): DO NOT SUBMIT without one-line documentation for start_ars_servers.
+
+blaze build -c opt //experimental/users/jietan/ARS:start_ars_servers
+blaze-bin/experimental/users/jietan/ARS/start_ars_servers
+
+TODO(jietan): DO NOT SUBMIT without a detailed description of start_ars_servers.
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+import time
+import subprocess
+from absl import app
+from absl import flags
+
+FLAGS = flags.FLAGS
+flags.DEFINE_integer("num_servers", 8, "number of servers")
+
+def main(argv):
+  del argv  # Unused.
+  for server_id in xrange(FLAGS.num_servers):
+    args = ["blaze-bin/experimental/users/jietan/ARS/ars_server", "--config_name=MINITAUR_GYM_CONFIG", "--server_id={}".format(server_id), "--run_on_borg=False"]
+    subprocess.Popen(args)
+
+
+if __name__ == '__main__':
+  app.run(main)

examples/pybullet/gym/pybullet_envs/ARS/train_ars.borg

@@ -0,0 +1,93 @@
+// Example borg file to do a parameter sweep.
+//
+// To run:
+//   echo `srcfs get_readonly`-`g4 p | head -1 | awk '{print $2}'`
+//   blaze build -c opt experimental/users/jietan/ARS:ars_server.par
+//   blaze build -c opt experimental/users/jietan/ARS:ars_client.par
+//   borgcfg --skip_confirmation --vars 'base_cl=191950338,my_cl=191950550,label=ars_react_nr01,config=MINITAUR_REACTIVE_CONFIG' experimental/users/jietan/ARS/train_ars.borg reload
+//   borgcfg --skip_confirmation --vars 'base_cl=191950338,my_cl=191950550,label=ars_react_rd01,config=MINITAUR_REACTIVE_RANDOMIZER_CONFIG' experimental/users/jietan/ARS/train_ars.borg reload
+
+
+import '//production/borg/templates/lambda/buildtool_support.borg' as build
+import '//production/borg/templates/lambda/dnsname.borg' as dns
+
+vars = {
+  cell = 'atlanta'
+  charged_user = 'robotics'
+  base_cl = 0
+  my_cl = 0
+  label = external
+  user = real_username()
+  workers = 8
+  config = external
+  cns_home = "/cns/ij-d/home/%user%"
+  logdir = "%cns_home%/experiment/ARS/%label%.%base_cl%.%my_cl%/"
+}
+
+service augmented_random_search {
+  runtime {
+    cell = vars.cell
+  }
+
+  scheduling = {
+    priority = 100
+    batch_quota = {
+      strategy = 'RUN_SOON'
+    }
+    deadline = 3600 * 24
+  }
+  accounting = {
+    charged_user = vars.charged_user
+  }
+  requirements {
+    autopilot = true
+  }
+  params = {
+    mygoogle3 = build.google3dir(myfilename())
+    experiment_dir = 'experimental/users/jietan/ARS/'
+  }
+
+  job ars_server = {
+    runtime {
+      cell = vars.cell
+    }
+    name = real_username() + '_server_' + vars.label
+    replicas = vars.workers
+    binary_path = build.binfile_v2(params.mygoogle3,
+                                   params.experiment_dir + 'ars_server')
+    runfiles = binary_path + '.runfiles/google3/'
+    packages = {
+      package third_party = {
+        directory = runfiles + 'third_party/'
+      }
+    }
+    binary = build.binfile(params.mygoogle3,
+                           params.experiment_dir + 'ars_server.par')
+    args = {
+      server_id = '%task%'
+      config_name = vars.config
+      port = '%port%'
+      run_on_borg = true
+    }
+  }
+  job ars_client = {
+    name = real_username() + '_client_' + vars.label
+    binary_path = build.binfile_v2(params.mygoogle3,
+                                   params.experiment_dir + 'ars_client')
+    runfiles = binary_path + '.runfiles/google3/'
+    packages = {
+      package third_party = {
+        directory = runfiles + 'third_party/'
+      }
+    }
+    binary = build.binfile(params.mygoogle3,
+                           params.experiment_dir + 'ars_client.par')
+    args = {
+      server_address = dns.borg_dns_name(ars_server)
+      num_servers = vars.workers
+      config_name = vars.config
+      logdir = vars.logdir
+      run_on_borg = true
+    }
+  }
+}

examples/pybullet/gym/pybullet_envs/ARS/train_ars.py

@@ -0,0 +1,64 @@
+"""TODO(jietan): DO NOT SUBMIT without one-line documentation for train_ars.
+
+blaze build -c opt //experimental/users/jietan/ARS:train_ars
+blaze-bin/experimental/users/jietan/ARS/train_ars \
+--logdir=/cns/ij-d/home/jietan/experiment/ARS/test1 \
+--config_name=MINITAUR_GYM_CONFIG
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+
+import os
+from absl import app
+from absl import flags
+import ars
+import config_ars
+
+FLAGS = flags.FLAGS
+flags.DEFINE_string('logdir', None, 'The directory to write the log file.')
+flags.DEFINE_string('config_name', None, 'The name of the config dictionary')
+
+
+def run_ars(config, logdir):
+
+  env = config["env"]()
+  ob_dim = env.observation_space.shape[0]
+  ac_dim = env.action_space.shape[0]
+
+  # set policy parameters. Possible filters: 'MeanStdFilter' for v2, 'NoFilter' for v1.
+  policy_params = {
+      'type': 'linear',
+      'ob_filter': config['filter'],
+      'ob_dim': ob_dim,
+      'ac_dim': ac_dim
+  }
+
+  ARS = ars.ARSLearner(
+      env_callback=config['env'],
+      policy_params=policy_params,
+      num_deltas=config['num_directions'],
+      deltas_used=config['deltas_used'],
+      step_size=config['step_size'],
+      delta_std=config['delta_std'],
+      logdir=logdir,
+      rollout_length=config['rollout_length'],
+      shift=config['shift'],
+      params=config,
+      seed=config['seed'])
+
+  return ARS.train(config['num_iterations'])
+
+
+def main(argv):
+  del argv  # Unused.
+  config = getattr(config_ars, FLAGS.config_name)
+  run_ars(config=config, logdir=FLAGS.logdir)
+
+
+if __name__ == '__main__':
+  flags.mark_flag_as_required('logdir')
+  flags.mark_flag_as_required('config_name')
+  app.run(main)

examples/pybullet/gym/pybullet_envs/ARS/train_ars_test.py

@@ -0,0 +1,29 @@
+"""Tests for google3.experimental.users.jietan.ARS.train_ars.
+blaze build -c opt //experimental/users/jietan/ARS:train_ars_test
+blaze-bin/experimental/users/jietan/ARS/train_ars_test
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from absl import flags
+from google3.testing.pybase import googletest
+from google3.experimental.users.jietan.ARS import train_ars
+from google3.experimental.users.jietan.ARS import config_ars
+
+FLAGS = flags.FLAGS
+MAX_RETURN_AFTER_TWO_ITEATIONS = 0.0890905394617
+
+class TrainArsTest(googletest.TestCase):
+
+  def testArsTwoStepResult(self):
+    config = getattr(config_ars, "MINITAUR_REACTIVE_CONFIG")
+    config['num_iterations'] = 2
+    info = train_ars.run_ars(config=config, logdir=FLAGS.test_tmpdir)
+    print (info)
+    self.assertAlmostEqual(info["max_reward"], MAX_RETURN_AFTER_TWO_ITEATIONS)
+
+
+if __name__ == '__main__':
+  googletest.main()

examples/pybullet/gym/pybullet_envs/ARS/utility.py

@@ -0,0 +1,52 @@
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+import os
+import ruamel.yaml as yaml
+
+def save_config(config, logdir):
+  """Save a new configuration by name.
+
+  If a logging directory is specified, is will be created and the configuration
+  will be stored there. Otherwise, a log message will be printed.
+
+  Args:
+    config: Configuration object.
+    logdir: Location for writing summaries and checkpoints if specified.
+
+  Returns:
+    Configuration object.
+  """
+  message = 'Start a new run and write summaries and checkpoints to {}.'
+  print(message.format(logdir))
+  config_path = os.path.join(logdir, 'config.yaml')
+  yaml.dump(config, config_path, default_flow_style=False)
+  return config
+
+
+def load_config(logdir):
+  """Load a configuration from the log directory.
+
+  Args:
+    logdir: The logging directory containing the configuration file.
+
+  Raises:
+    IOError: The logging directory does not contain a configuration file.
+
+  Returns:
+    Configuration object.
+  """
+  config_path = logdir and os.path.join(logdir, 'config.yaml')
+  if not config_path:
+    message = (
+        'Cannot resume an existing run since the logging directory does not '
+        'contain a configuration file.')
+    raise IOError(message)
+  print("config_path=",config_path)
+
+  stream = open(config_path, 'r')
+  config = yaml.load(stream)
+  message = 'Resume run and write summaries and checkpoints to {}.'
+  print(message.format(logdir))
+  return config

examples/pybullet/gym/pybullet_envs/ARS/utils.py

@@ -0,0 +1,28 @@
+# Code in this file is copied and adapted from
+# https://github.com/openai/evolution-strategies-starter.
+
+import numpy as np
+
+def itergroups(items, group_size):
+    assert group_size >= 1
+    group = []
+    for x in items:
+        group.append(x)
+        if len(group) == group_size:
+            yield tuple(group)
+            del group[:]
+    if group:
+        yield tuple(group)
+
+
+
+def batched_weighted_sum(weights, vecs, batch_size):
+    total = 0
+    num_items_summed = 0
+    for batch_weights, batch_vecs in zip(itergroups(weights, batch_size),
+                                         itergroups(vecs, batch_size)):
+        assert len(batch_weights) == len(batch_vecs) <= batch_size
+        total += np.dot(np.asarray(batch_weights, dtype=np.float64),
+                        np.asarray(batch_vecs, dtype=np.float64))
+        num_items_summed += len(batch_weights)
+    return total, num_items_summed

examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env.py

@@ -144,9 +144,9 @@ class MinitaurReactiveEnv(minitaur_gym_env.MinitaurGymEnv):
   def _convert_from_leg_model(self, leg_pose):
     motor_pose = np.zeros(NUM_MOTORS)
     for i in range(NUM_LEGS):
-      motor_pose[2 * i] = leg_pose[NUM_LEGS + i] - (-1)**(i / 2) * leg_pose[i]
+      motor_pose[int(2 * i)] = leg_pose[NUM_LEGS + i] - (-1)**int(i / 2) * leg_pose[i]
-      motor_pose[2 * i + 1] = (
+      motor_pose[int(2 * i + 1)] = (
-          leg_pose[NUM_LEGS + i] + (-1)**(i / 2) * leg_pose[i])
+          leg_pose[NUM_LEGS + i] + (-1)**int(i / 2) * leg_pose[i])
     return motor_pose
 
   def _signal(self, t):

examples/pybullet/gym/pybullet_utils/urdfEditor.py

@@ -375,8 +375,8 @@ class UrdfEditor(object):
 					physicsClientId=physicsClientId)
 
 
-                urdfVisuals = base.urdf_visual_shapes
+		urdfVisuals = base.urdf_visual_shapes
-                baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals],
+		baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals],
                                                            halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals],
                                                            radii=[v.geom_radius for v in urdfVisuals],
                                                            lengths=[v.geom_length[0] for v in urdfVisuals],
@@ -453,28 +453,17 @@ class UrdfEditor(object):
 					collisionFrameOrientations=linkOrientationsArray,
 					physicsClientId=physicsClientId)
 
-                        urdfVisuals = link.urdf_visual_shapes
+				urdfVisuals = link.urdf_visual_shapes
-                        linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals],
+				linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals],
-                                                           halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals],
+							halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals],
-                                                           radii=[v.geom_radius for v in urdfVisuals],
+							radii=[v.geom_radius for v in urdfVisuals],
-                                                           lengths=[v.geom_length[0] for v in urdfVisuals],
+							lengths=[v.geom_length[0] for v in urdfVisuals],
-                                                           fileNames=[v.geom_meshfilename for v in urdfVisuals],
+							fileNames=[v.geom_meshfilename for v in urdfVisuals],
-                                                           meshScales=[v.geom_meshscale for v in urdfVisuals],
+							meshScales=[v.geom_meshscale for v in urdfVisuals],
-                                                           rgbaColors=[v.material_rgba for v in urdfVisuals],
+							rgbaColors=[v.material_rgba for v in urdfVisuals],
-                                                           visualFramePositions=[v.origin_xyz for v in urdfVisuals],
+							visualFramePositions=[v.origin_xyz for v in urdfVisuals],
-                                                           visualFrameOrientations=[v.origin_rpy for v in urdfVisuals],
+							visualFrameOrientations=[v.origin_rpy for v in urdfVisuals],
-                                                           physicsClientId=physicsClientId)
+							physicsClientId=physicsClientId)
-
-#                         linkVisualShapeIndex = p.createVisualShape(shapeType=urdfVisual.geom_type,
-#                                                                    halfExtents=[ext * 0.5 for ext in urdfVisual.geom_extents],
-#                                                                    radius=urdfVisual.geom_radius,
-#                                                                    length=urdfVisual.geom_length[0],
-#                                                                    fileName=urdfVisual.geom_meshfilename,
-#                                                                    meshScale=urdfVisual.geom_meshscale,
-#                                                                    rgbaColor=urdfVisual.material_rgba,
-#                                                                    visualFramePosition=urdfVisual.origin_xyz,
-#                                                                    visualFrameOrientation=urdfVisual.origin_rpy,
-#                                                                    physicsClientId=physicsClientId)
 
 			linkMasses.append(linkMass)
 			linkCollisionShapeIndices.append(linkCollisionShapeIndex)

examples/pybullet/unittests/unittests.py

@@ -2,6 +2,8 @@ import unittest
 import pybullet
 import time
 
+from utils import allclose, dot
+
 class TestPybulletMethods(unittest.TestCase):
 
     def test_import(self):
@@ -41,6 +43,74 @@ class TestPybulletMethods(unittest.TestCase):
         self.assertLess(vel[1][2],1e-10)
         p.disconnect()
 
+class TestPybulletJacobian(unittest.TestCase):
+
+    def getMotorJointStates(self, robot):
+        import pybullet as p
+        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(self, robot, position, kp=1.0, kv=0.3):
+        import pybullet as p
+        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)
+
+    def testJacobian(self):
+        import pybullet as p
+
+        clid = p.connect(p.SHARED_MEMORY)
+        if (clid<0):
+            p.connect(p.DIRECT)
+
+        time_step = 0.001
+        gravity_constant = -9.81
+
+        urdfs = ["TwoJointRobot_w_fixedJoints.urdf",
+                 "TwoJointRobot_w_fixedJoints.urdf",
+                 "kuka_iiwa/model.urdf",
+                 "kuka_lwr/kuka.urdf"]
+        for urdf in urdfs:
+            p.resetSimulation()
+            p.setTimeStep(time_step)
+            p.setGravity(0.0, 0.0, gravity_constant)
+
+            robotId = p.loadURDF(urdf, useFixedBase=True)
+            p.resetBasePositionAndOrientation(robotId,[0,0,0],[0,0,0,1])
+            numJoints = p.getNumJoints(robotId)
+            endEffectorIndex = numJoints - 1
+
+            # Set a joint target for the position control and step the sim.
+            self.setJointPosition(robotId, [0.1 * (i % 3)
+                                            for i in range(numJoints)])
+            p.stepSimulation()
+
+            # Get the joint and link state directly from Bullet.
+            mpos, mvel, mtorq = self.getMotorJointStates(robotId)
+
+            result = p.getLinkState(robotId, endEffectorIndex,
+                    computeLinkVelocity=1, computeForwardKinematics=1)
+            link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
+            # Get the Jacobians for the CoM of the end-effector link.
+            # Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
+            # The localPosition is always defined in terms of the link frame coordinates.
+
+            zero_vec = [0.0] * len(mpos)
+            jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex,
+                    com_trn, mpos, zero_vec, zero_vec)
+
+            assert(allclose(dot(jac_t, mvel), link_vt))
+            assert(allclose(dot(jac_r, mvel), link_vr))
+        p.disconnect()
+
 if __name__ == '__main__':
     unittest.main()
 

examples/pybullet/unittests/utils.py

@@ -0,0 +1,7 @@
+def dot(A, b):
+    """Dot product between a 2D matrix and a 1D vector"""
+    return [sum([aij*bj for aij, bj in zip(ai, b)]) for ai in A]
+
+def allclose(a, b, tol=1e-7):
+    """Are all elements of a vector close to one another"""
+    return all([abs(ai - bi) < tol for ai, bi in zip(a,b)])

src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp

@@ -599,8 +599,8 @@ int b3Generic6DofConstraint::get_limit_motor_info2(
 													tag_vel, 
 													info->fps * limot->m_stopERP);
 				info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
-                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+                info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
-                info->m_upperLimit[srow] = limot->m_maxMotorForce;
+                info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
             }
         }
         if(limit)

src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h

@@ -69,7 +69,7 @@ public:
     {
     	m_accumulatedImpulse = 0.f;
         m_targetVelocity = 0;
-        m_maxMotorForce = 0.1f;
+        m_maxMotorForce = 6.0f;
         m_maxLimitForce = 300.0f;
         m_loLimit = 1.0f;
         m_hiLimit = -1.0f;

src/BulletCollision/BroadphaseCollision/btDispatcher.h

@@ -47,7 +47,7 @@ struct btDispatcherInfo
 		m_allowedCcdPenetration(btScalar(0.04)),
 		m_useConvexConservativeDistanceUtil(false),
 		m_convexConservativeDistanceThreshold(0.0f),
-		m_deterministicOverlappingPairs(true)
+		m_deterministicOverlappingPairs(false)
 	{
 
 	}

src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -164,7 +164,7 @@ partId, int triangleIndex)
 
 
 
-void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
+void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
 {
 	m_convexBodyWrap = convexBodyWrap;
 	m_triBodyWrap = triBodyWrap;
@@ -178,10 +178,10 @@ void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr
 	convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform();
 	const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
 	//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
-	convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
+	convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax);
-	btScalar extraMargin = collisionMarginTriangle+ resultOut->m_closestPointDistanceThreshold;
+	btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold;
 
-	btVector3 extra(extraMargin,extraMargin,extraMargin);
+	btVector3 extra(extraMargin, extraMargin, extraMargin);
 
 	m_aabbMax += extra;
 	m_aabbMin -= extra;
@@ -194,7 +194,7 @@ void btConvexConcaveCollisionAlgorithm::clearCache()
 
 }
 
-void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
 
@@ -203,7 +203,7 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
 
 	if (triBodyWrap->getCollisionShape()->isConcave())
 	{
-		if (triBodyWrap->getCollisionShape()->getShapeType()==SDF_SHAPE_PROXYTYPE)
+		if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE)
 		{
 			btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape();
 			if (convexBodyWrap->getCollisionShape()->isConvex())
@@ -214,17 +214,21 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
 
 				if (convex->isPolyhedral())
 				{
-					btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*) convex;
+					btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex;
-					for (int v=0;v<poly->getNumVertices();v++)
+					for (int v = 0; v < poly->getNumVertices(); v++)
 					{
 						btVector3 vtx;
-						poly->getVertex(v,vtx);
+						poly->getVertex(v, vtx);
 						queryVertices.push_back(vtx);
 					}
 				}
-				if (convex->getShapeType()==SPHERE_SHAPE_PROXYTYPE)
+				btScalar maxDist = SIMD_EPSILON;
+
+				if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
 				{
-					queryVertices.push_back(btVector3(0,0,0));
+					queryVertices.push_back(btVector3(0, 0, 0));
+					btSphereShape* sphere = (btSphereShape*)convex;
+					maxDist = sphere->getRadius() + SIMD_EPSILON;
 
 				}
 				if (queryVertices.size())
@@ -232,8 +236,8 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
 					resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
 					//m_btConvexTriangleCallback.m_manifoldPtr->clearManifold();
 
-					btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*) convex;
+					btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex;
-					for (int v=0;v<queryVertices.size();v++)
+					for (int v = 0; v < queryVertices.size(); v++)
 					{
 						const btVector3& vtx = queryVertices[v];
 						btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform()*vtx;
@@ -241,12 +245,20 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
 
 						btVector3 normalLocal;
 						btScalar dist;
-						if (sdfShape->queryPoint(vtxInSdf,dist, normalLocal))
+						if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal))
 						{
-							if (dist<=SIMD_EPSILON)
+							if (dist <= maxDist)
 							{
 								normalLocal.safeNormalize();
 								btVector3 normal = triBodyWrap->getWorldTransform().getBasis()*normalLocal;
+
+								if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
+								{
+									btSphereShape* sphere = (btSphereShape*)convex;
+									dist -= sphere->getRadius();
+									vtxWorldSpace -= sphere->getRadius()*normal;
+
+								}
 								resultOut->addContactPoint(normal,vtxWorldSpace-normal*dist, dist);
 							}
 						}

src/BulletCollision/CollisionShapes/btMiniSDF.cpp

@@ -1,5 +1,13 @@
 #include "btMiniSDF.h"
 
+//
+//Based on code from DiscreGrid, https://github.com/InteractiveComputerGraphics/Discregrid
+//example:
+//GenerateSDF.exe -r "32 32 32" -d "-1.6 -1.6 -.6 1.6 1.6 .6" concave_box.obj
+//The MIT License (MIT)
+//
+//Copyright (c) 2017 Dan Koschier
+//
 
 #include <limits.h>
 #include <string.h> //memcpy
@@ -89,7 +97,10 @@ bool btMiniSDF::load(const char* data, int size)
 		std::size_t n_nodes0;
 		ds.read(nodes0);
 		n_nodes0 = nodes0;
-
+		if (n_nodes0 > 1024 * 1024 * 1024)
+		{
+			return m_isValid;
+		}
 		m_nodes.resize(n_nodes0);
 		for (unsigned int i=0;i<n_nodes0;i++)
 		{

src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp

@@ -72,7 +72,17 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 
 
 	btScalar dist2 = v.length2();
+
+#ifdef BT_USE_DOUBLE_PRECISION
 	btScalar epsilon = SIMD_EPSILON * 10;
+#else
+//todo: epsilon kept for backward compatibility of unit tests.
+//will need to digg deeper to make the algorithm more robust
+//since, a large epsilon can cause an early termination with false
+//positive results (ray intersections that shouldn't be there)
+	btScalar epsilon = btScalar(0.0001);
+#endif //BT_USE_DOUBLE_PRECISION
+
 
 	btVector3	w,p;
 	btScalar VdotR;

src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

@@ -855,8 +855,8 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
 													tag_vel, 
 													info->fps * limot->m_stopERP);
 				info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
-                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+                info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
-                info->m_upperLimit[srow] = limot->m_maxMotorForce;
+                info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
             }
         }
         if(limit)

src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

@@ -77,7 +77,7 @@ public:
     {
     	m_accumulatedImpulse = 0.f;
         m_targetVelocity = 0;
-        m_maxMotorForce = 0.1f;
+        m_maxMotorForce = 6.0f;
         m_maxLimitForce = 300.0f;
         m_loLimit = 1.0f;
         m_hiLimit = -1.0f;

src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp

@@ -719,8 +719,8 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 			tag_vel,
 			info->fps * limot->m_motorERP);
 		info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
-		info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+		info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
-		info->m_upperLimit[srow] = limot->m_maxMotorForce;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
 		info->cfm[srow] = limot->m_motorCFM;
 		srow += info->rowskip;
 		++count;
@@ -769,8 +769,8 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 			mot_fact = 0;
 		}
 		info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
-		info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+		info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
-		info->m_upperLimit[srow] = limot->m_maxMotorForce;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
 		info->cfm[srow] = limot->m_motorCFM;
 		srow += info->rowskip;
 		++count;

src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h

@@ -107,7 +107,7 @@ public:
 		m_motorCFM               = 0.f;
 		m_enableMotor            = false;
 		m_targetVelocity         = 0;
-		m_maxMotorForce          = 0.1f;
+		m_maxMotorForce          = 6.0f;
 		m_servoMotor             = false;
 		m_servoTarget            = 0;
 		m_enableSpring           = false;

src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp

@@ -131,7 +131,7 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
 			btScalar force = delta * m_springStiffness[i];
 			btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
 			m_linearLimits.m_targetVelocity[i] =  velFactor * force;
-			m_linearLimits.m_maxMotorForce[i] =  btFabs(force) / info->fps;
+			m_linearLimits.m_maxMotorForce[i] =  btFabs(force);
 		}
 	}
 	for(i = 0; i < 3; i++)
@@ -146,7 +146,7 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
 			btScalar force = -delta * m_springStiffness[i+3];
 			btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
 			m_angularLimits[i].m_targetVelocity = velFactor * force;
-			m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
+			m_angularLimits[i].m_maxMotorForce = btFabs(force);
 		}
 	}
 }

src/BulletDynamics/Vehicle/btRaycastVehicle.cpp

@@ -121,11 +121,18 @@ void	btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedT
 	btQuaternion rotatingOrn(right,-wheel.m_rotation);
 	btMatrix3x3 rotatingMat(rotatingOrn);
 
-	btMatrix3x3 basis2(
+    btMatrix3x3 basis2;
-		right[0],fwd[0],up[0],
+    basis2[0][m_indexRightAxis] = -right[0];
-		right[1],fwd[1],up[1],
+    basis2[1][m_indexRightAxis] = -right[1];
-		right[2],fwd[2],up[2]
+    basis2[2][m_indexRightAxis] = -right[2];
-	);
+
+    basis2[0][m_indexUpAxis] = up[0];
+    basis2[1][m_indexUpAxis] = up[1];
+    basis2[2][m_indexUpAxis] = up[2];
+
+    basis2[0][m_indexForwardAxis] = fwd[0];
+    basis2[1][m_indexForwardAxis] = fwd[1];
+    basis2[2][m_indexForwardAxis] = fwd[2];
 
 	wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
 	wheel.m_worldTransform.setOrigin(
@@ -567,7 +574,7 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 					const btTransform& wheelTrans = getWheelTransformWS( i );
 
 					btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
-					m_axle[i] = btVector3(	
+					m_axle[i] = -btVector3(	
 						wheelBasis0[0][m_indexRightAxis],
 						wheelBasis0[1][m_indexRightAxis],
 						wheelBasis0[2][m_indexRightAxis]);