diff --git a/Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.cpp b/Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.cpp index 800ec7014..51a6f5c23 100644 --- a/Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.cpp +++ b/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); - - if (loadOkay) + XMLError loadOkay = doc.LoadFile(fileName); + + if (loadOkay==XML_SUCCESS) { if (get_int_attribute_by_name(doc.FirstChildElement()->ToElement(),"version", &m_fileVersion)) { diff --git a/data/toys/concave_box.cdf b/data/toys/concave_box.cdf new file mode 100644 index 000000000..1183762d8 Binary files /dev/null and b/data/toys/concave_box.cdf differ diff --git a/data/toys/concave_box.mtl b/data/toys/concave_box.mtl new file mode 100644 index 000000000..35c3a209b --- /dev/null +++ b/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 diff --git a/data/toys/concave_box.obj b/data/toys/concave_box.obj new file mode 100644 index 000000000..bc58715ec --- /dev/null +++ b/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 diff --git a/data/toys/concave_box.urdf b/data/toys/concave_box.urdf new file mode 100644 index 000000000..08ff9c02d --- /dev/null +++ b/data/toys/concave_box.urdf @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/Constraints/ConstraintDemo.cpp b/examples/Constraints/ConstraintDemo.cpp index 72ad1a8a6..cf7dc063d 100644 --- a/examples/Constraints/ConstraintDemo.cpp +++ b/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 diff --git a/examples/Constraints/Dof6Spring2Setup.cpp b/examples/Constraints/Dof6Spring2Setup.cpp index 89129f7a6..a3fd5e8f4 100644 --- a/examples/Constraints/Dof6Spring2Setup.cpp +++ b/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)); diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 6d70cc946..99c953c09 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/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; + } } } diff --git a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp index 3a36d7af0..f5c5cc563 100644 --- a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp +++ b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp @@ -575,7 +575,7 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray @@ -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 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; diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index c6a30edf7..1be8285c1 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -436,9 +436,16 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, XMLElement* g, ErrorLogger* l geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast(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; diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index b3809bd86..fed2cc141 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/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); diff --git a/examples/OpenGLWindow/Shaders/projectiveTextureInstancingPS.glsl b/examples/OpenGLWindow/Shaders/projectiveTextureInstancingPS.glsl index 4e11e3762..e2da4b4aa 100644 --- a/examples/OpenGLWindow/Shaders/projectiveTextureInstancingPS.glsl +++ b/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) diff --git a/examples/OpenGLWindow/Shaders/projectiveTextureInstancingPS.h b/examples/OpenGLWindow/Shaders/projectiveTextureInstancingPS.h index 2bdb30b64..0c5f72438 100644 --- a/examples/OpenGLWindow/Shaders/projectiveTextureInstancingPS.h +++ b/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" diff --git a/examples/OpenGLWindow/Shaders/projectiveTextureInstancingVS.glsl b/examples/OpenGLWindow/Shaders/projectiveTextureInstancingVS.glsl index c3417ed55..8a080395c 100644 --- a/examples/OpenGLWindow/Shaders/projectiveTextureInstancingVS.glsl +++ b/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; } diff --git a/examples/OpenGLWindow/Shaders/projectiveTextureInstancingVS.h b/examples/OpenGLWindow/Shaders/projectiveTextureInstancingVS.h index b1de7880d..449c410be 100644 --- a/examples/OpenGLWindow/Shaders/projectiveTextureInstancingVS.h +++ b/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" ; diff --git a/examples/SharedMemory/CMakeLists.txt b/examples/SharedMemory/CMakeLists.txt index 68b776737..0d0ea65e7 100644 --- a/examples/SharedMemory/CMakeLists.txt +++ b/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) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f40fb2fa5..6851729a9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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 - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(3+limitAxis,torqueImpulse); + con->setMaxMotorForce(3+limitAxis, torque); } 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 - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(3+limitAxis,torqueImpulse); + con->setMaxMotorForce(3+limitAxis, torque); 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 - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(limitAxis,torqueImpulse); + con->setMaxMotorForce(limitAxis, torque); 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 - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(limitAxis,torqueImpulse); + con->setMaxMotorForce(limitAxis, torque); con->enableMotor(limitAxis,true); break; } diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index 0b444fe73..c3edbcdf8 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -59,6 +59,9 @@ static vr::VRControllerState_t sPrevStates[vr::k_unMaxTrackedDeviceCount] = { 0 #ifdef _WIN32 #include #endif +#ifdef __linux__ +#define APIENTRY +#endif void ThreadSleep( unsigned long nMilliseconds ) { @@ -1804,13 +1807,11 @@ 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; } } diff --git a/examples/ThirdPartyLibs/openvr/samples/shared/strtools.cpp b/examples/ThirdPartyLibs/openvr/samples/shared/strtools.cpp index 6166db483..d47519224 100644 --- a/examples/ThirdPartyLibs/openvr/samples/shared/strtools.cpp +++ b/examples/ThirdPartyLibs/openvr/samples/shared/strtools.cpp @@ -4,6 +4,12 @@ #include #include +#ifdef __linux__ +#define stricmp strcasecmp +#define strnicmp strncasecmp +#endif + + //----------------------------------------------------------------------------- // Purpose: //----------------------------------------------------------------------------- diff --git a/examples/Tutorial/Dof6ConstraintTutorial.cpp b/examples/Tutorial/Dof6ConstraintTutorial.cpp index 125fd4e61..889182e11 100644 --- a/examples/Tutorial/Dof6ConstraintTutorial.cpp +++ b/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)); diff --git a/examples/pybullet/examples/projective_texture.py b/examples/pybullet/examples/projective_texture.py index 00ae2320d..c4b21b9d0 100644 --- a/examples/pybullet/examples/projective_texture.py +++ b/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) diff --git a/examples/pybullet/examples/signedDistanceField.py b/examples/pybullet/examples/signedDistanceField.py new file mode 100644 index 000000000..6e4153ef8 --- /dev/null +++ b/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) + \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/ARS/ars.py b/examples/pybullet/gym/pybullet_envs/ARS/ars.py new file mode 100644 index 000000000..cf59ede85 --- /dev/null +++ b/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 diff --git a/examples/pybullet/gym/pybullet_envs/ARS/ars_server.py b/examples/pybullet/gym/pybullet_envs/ARS/ars_server.py new file mode 100644 index 000000000..f680dd632 --- /dev/null +++ b/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) diff --git a/examples/pybullet/gym/pybullet_envs/ARS/config_ars.py b/examples/pybullet/gym/pybullet_envs/ARS/config_ars.py new file mode 100644 index 000000000..e333f88ab --- /dev/null +++ b/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) diff --git a/examples/pybullet/gym/pybullet_envs/ARS/eval_ars.py b/examples/pybullet/gym/pybullet_envs/ARS/eval_ars.py new file mode 100644 index 000000000..d0c144bfa --- /dev/null +++ b/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) diff --git a/examples/pybullet/gym/pybullet_envs/ARS/filter.py b/examples/pybullet/gym/pybullet_envs/ARS/filter.py new file mode 100644 index 000000000..ac68a2341 --- /dev/null +++ b/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() diff --git a/examples/pybullet/gym/pybullet_envs/ARS/log/config.yaml b/examples/pybullet/gym/pybullet_envs/ARS/log/config.yaml new file mode 100644 index 000000000..36690d51d --- /dev/null +++ b/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 + diff --git a/examples/pybullet/gym/pybullet_envs/ARS/log/lin_policy_plus_990_bla.npz b/examples/pybullet/gym/pybullet_envs/ARS/log/lin_policy_plus_990_bla.npz new file mode 100644 index 000000000..54fd39ba7 Binary files /dev/null and b/examples/pybullet/gym/pybullet_envs/ARS/log/lin_policy_plus_990_bla.npz differ diff --git a/examples/pybullet/gym/pybullet_envs/ARS/logz.py b/examples/pybullet/gym/pybullet_envs/ARS/logz.py new file mode 100644 index 000000000..790d83c21 --- /dev/null +++ b/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 diff --git a/examples/pybullet/gym/pybullet_envs/ARS/optimizers.py b/examples/pybullet/gym/pybullet_envs/ARS/optimizers.py new file mode 100644 index 000000000..063be9b87 --- /dev/null +++ b/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 + diff --git a/examples/pybullet/gym/pybullet_envs/ARS/policies.py b/examples/pybullet/gym/pybullet_envs/ARS/policies.py new file mode 100644 index 000000000..0adc27451 --- /dev/null +++ b/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 . + """ + + 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 diff --git a/examples/pybullet/gym/pybullet_envs/ARS/shared_noise.py b/examples/pybullet/gym/pybullet_envs/ARS/shared_noise.py new file mode 100644 index 000000000..1f4074920 --- /dev/null +++ b/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) + diff --git a/examples/pybullet/gym/pybullet_envs/ARS/start_ars_servers.py b/examples/pybullet/gym/pybullet_envs/ARS/start_ars_servers.py new file mode 100644 index 000000000..94aa6835f --- /dev/null +++ b/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) diff --git a/examples/pybullet/gym/pybullet_envs/ARS/train_ars.borg b/examples/pybullet/gym/pybullet_envs/ARS/train_ars.borg new file mode 100644 index 000000000..c09775b50 --- /dev/null +++ b/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 + } + } +} diff --git a/examples/pybullet/gym/pybullet_envs/ARS/train_ars.py b/examples/pybullet/gym/pybullet_envs/ARS/train_ars.py new file mode 100644 index 000000000..147dbc6eb --- /dev/null +++ b/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) diff --git a/examples/pybullet/gym/pybullet_envs/ARS/train_ars_test.py b/examples/pybullet/gym/pybullet_envs/ARS/train_ars_test.py new file mode 100644 index 000000000..13f149fc5 --- /dev/null +++ b/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() diff --git a/examples/pybullet/gym/pybullet_envs/ARS/utility.py b/examples/pybullet/gym/pybullet_envs/ARS/utility.py new file mode 100644 index 000000000..591418be5 --- /dev/null +++ b/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 diff --git a/examples/pybullet/gym/pybullet_envs/ARS/utils.py b/examples/pybullet/gym/pybullet_envs/ARS/utils.py new file mode 100644 index 000000000..3b7e03afa --- /dev/null +++ b/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 diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env.py index 6b2c5f884..dc3a02897 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env.py +++ b/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[2 * i + 1] = ( - 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[int(2 * i + 1)] = ( + leg_pose[NUM_LEGS + i] + (-1)**int(i / 2) * leg_pose[i]) return motor_pose def _signal(self, t): diff --git a/examples/pybullet/gym/pybullet_utils/urdfEditor.py b/examples/pybullet/gym/pybullet_utils/urdfEditor.py index 9cfd950ed..ee1a2278a 100644 --- a/examples/pybullet/gym/pybullet_utils/urdfEditor.py +++ b/examples/pybullet/gym/pybullet_utils/urdfEditor.py @@ -375,8 +375,8 @@ class UrdfEditor(object): physicsClientId=physicsClientId) - urdfVisuals = base.urdf_visual_shapes - baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals], + urdfVisuals = base.urdf_visual_shapes + 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 - 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], - radii=[v.geom_radius for v in urdfVisuals], - lengths=[v.geom_length[0] for v in urdfVisuals], - fileNames=[v.geom_meshfilename for v in urdfVisuals], - meshScales=[v.geom_meshscale for v in urdfVisuals], - rgbaColors=[v.material_rgba for v in urdfVisuals], - visualFramePositions=[v.origin_xyz for v in urdfVisuals], - visualFrameOrientations=[v.origin_rpy for v in urdfVisuals], - 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) + urdfVisuals = link.urdf_visual_shapes + 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], + radii=[v.geom_radius for v in urdfVisuals], + lengths=[v.geom_length[0] for v in urdfVisuals], + fileNames=[v.geom_meshfilename for v in urdfVisuals], + meshScales=[v.geom_meshscale for v in urdfVisuals], + rgbaColors=[v.material_rgba for v in urdfVisuals], + visualFramePositions=[v.origin_xyz for v in urdfVisuals], + visualFrameOrientations=[v.origin_rpy for v in urdfVisuals], + physicsClientId=physicsClientId) linkMasses.append(linkMass) linkCollisionShapeIndices.append(linkCollisionShapeIndex) diff --git a/examples/pybullet/unittests/unittests.py b/examples/pybullet/unittests/unittests.py index 3975c44af..469234998 100644 --- a/examples/pybullet/unittests/unittests.py +++ b/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): @@ -40,7 +42,75 @@ class TestPybulletMethods(unittest.TestCase): self.assertLess(vel[1][1],1e-10) 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() diff --git a/examples/pybullet/unittests/utils.py b/examples/pybullet/unittests/utils.py new file mode 100644 index 000000000..b3e5f68fe --- /dev/null +++ b/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)]) diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp index 168a773d5..3ae2922e5 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp +++ 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_upperLimit[srow] = limot->m_maxMotorForce; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; } } if(limit) diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h index 084d36055..169b1b94a 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h +++ b/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; diff --git a/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/src/BulletCollision/BroadphaseCollision/btDispatcher.h index d7a97e8e0..a0e4c1892 100644 --- a/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/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) { } diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index d2331f5df..d8cbe9614 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -153,7 +153,7 @@ partId, int triangleIndex) { m_resultOut->setBody1Wrap(tmpWrap); } - + colAlgo->~btCollisionAlgorithm(); @@ -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,14 +178,14 @@ void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform(); const btCollisionShape* convexShape = static_cast(m_convexBodyWrap->getCollisionShape()); //CollisionShape* triangleShape = static_cast(triBody->m_collisionShape); - convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); - btScalar extraMargin = collisionMarginTriangle+ resultOut->m_closestPointDistanceThreshold; - - btVector3 extra(extraMargin,extraMargin,extraMargin); + convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax); + btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold; + + btVector3 extra(extraMargin, extraMargin, extraMargin); m_aabbMax += extra; m_aabbMin -= extra; - + } void btConvexConcaveCollisionAlgorithm::clearCache() @@ -194,16 +194,16 @@ 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"); - + const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap; const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap; 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,26 +214,30 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec if (convex->isPolyhedral()) { - btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*) convex; - for (int v=0;vgetNumVertices();v++) + btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; + 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()) { resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); //m_btConvexTriangleCallback.m_manifoldPtr->clearManifold(); - btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*) convex; - for (int v=0;vgetWorldTransform()*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); } } diff --git a/src/BulletCollision/CollisionShapes/btMiniSDF.cpp b/src/BulletCollision/CollisionShapes/btMiniSDF.cpp index c57673381..afe45e1d2 100644 --- a/src/BulletCollision/CollisionShapes/btMiniSDF.cpp +++ b/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 #include //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;ifps * limot->m_stopERP); info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; - info->m_lowerLimit[srow] = -limot->m_maxMotorForce; - info->m_upperLimit[srow] = limot->m_maxMotorForce; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; } } if(limit) diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index bea8629c3..b2ad45f74 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/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; diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp index c6865bfd6..611b30342 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp +++ b/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_upperLimit[srow] = limot->m_maxMotorForce; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + 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_upperLimit[srow] = limot->m_maxMotorForce; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; info->cfm[srow] = limot->m_motorCFM; srow += info->rowskip; ++count; diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h index 536e5af1b..fc2ed4f7a 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h +++ b/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; diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp index 6f765884e..3f875989e 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp +++ b/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); } } } diff --git a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index 47a0417d0..f299aa34e 100644 --- a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -121,12 +121,19 @@ void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedT btQuaternion rotatingOrn(right,-wheel.m_rotation); btMatrix3x3 rotatingMat(rotatingOrn); - btMatrix3x3 basis2( - right[0],fwd[0],up[0], - right[1],fwd[1],up[1], - right[2],fwd[2],up[2] - ); - + btMatrix3x3 basis2; + basis2[0][m_indexRightAxis] = -right[0]; + basis2[1][m_indexRightAxis] = -right[1]; + 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( wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength @@ -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]);