From 9a7efb132ea1f70ff034071367e7f55b94dd9cc0 Mon Sep 17 00:00:00 2001 From: budzianowski Date: Wed, 8 Jan 2025 01:06:45 -0800 Subject: [PATCH] update imu placement and xmls --- sim/resources/zbot2/imu_data.csv | 128 +++++++++ sim/resources/zbot2/robot.urdf | 5 +- sim/resources/zbot2/robot.xml | 412 ++++++++++++++------------- sim/resources/zbot2/robot_fixed.urdf | 2 +- sim/resources/zbot2/robot_fixed.xml | 299 +++++++++---------- sim/scripts/imu_data_comparison.py | 245 ++++++++++++++++ 6 files changed, 741 insertions(+), 350 deletions(-) create mode 100644 sim/resources/zbot2/imu_data.csv create mode 100644 sim/scripts/imu_data_comparison.py diff --git a/sim/resources/zbot2/imu_data.csv b/sim/resources/zbot2/imu_data.csv new file mode 100644 index 00000000..0008fa80 --- /dev/null +++ b/sim/resources/zbot2/imu_data.csv @@ -0,0 +1,128 @@ +timestamp,accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z +2025-01-07T17:45:54.101723,-1.1399999857,9.7899999619,-0.3599999845,-0.2500000000,0.4375000000,1.3750000000,-12.2500000000,-19.5000000000,-19.5000000000 +2025-01-07T17:45:54.180039,-0.8499999642,9.7799997330,-0.4299999774,-0.5000000000,-0.3750000000,2.6875000000,-12.2500000000,-19.1875000000,-19.5000000000 +2025-01-07T17:45:54.261766,-0.7699999809,9.8099994659,-0.3499999940,-0.8750000000,0.0625000000,0.1875000000,-12.2500000000,-20.1875000000,-19.2500000000 +2025-01-07T17:45:54.343540,-0.9799999595,9.7699995041,-0.3499999940,-0.5625000000,-0.2500000000,0.7500000000,-12.0000000000,-20.1875000000,-20.7500000000 +2025-01-07T17:45:54.420801,-0.8899999857,9.8000001907,-0.3299999833,-0.2500000000,0.0625000000,0.0000000000,-12.2500000000,-20.1875000000,-20.7500000000 +2025-01-07T17:45:54.501844,-0.8999999762,9.7899999619,-0.3299999833,-0.0625000000,-0.1250000000,-0.1250000000,-13.0000000000,-19.8750000000,-20.7500000000 +2025-01-07T17:45:54.584752,-0.8399999738,9.7699995041,-0.3100000024,0.0625000000,0.1250000000,0.3750000000,-12.2500000000,-21.0000000000,-21.1875000000 +2025-01-07T17:45:54.663277,-0.8999999762,9.7699995041,-0.3199999928,0.1875000000,0.1875000000,0.0000000000,-13.0000000000,-20.1875000000,-19.5625000000 +2025-01-07T17:45:54.744018,-0.8100000024,9.7899999619,-0.3299999833,0.0000000000,-0.0625000000,0.6250000000,-13.0000000000,-19.8750000000,-20.0625000000 +2025-01-07T17:45:54.825365,-0.8199999928,9.7799997330,-0.2999999821,0.5625000000,-0.3125000000,1.2500000000,-12.6875000000,-20.1875000000,-22.0000000000 +2025-01-07T17:45:54.904344,-0.7599999905,9.8299999237,-0.3999999762,0.6250000000,-0.2500000000,-0.1875000000,-13.0000000000,-19.8750000000,-20.7500000000 +2025-01-07T17:45:54.983908,-1.0000000000,9.8099994659,-0.4699999988,0.4375000000,0.1875000000,-0.8125000000,-13.0000000000,-19.5000000000,-20.3750000000 +2025-01-07T17:45:55.064528,-0.8899999857,9.8199996948,-0.3499999940,-0.4375000000,0.0000000000,0.3750000000,-13.3750000000,-20.5625000000,-19.5625000000 +2025-01-07T17:45:55.152842,-0.9300000072,9.7799997330,-0.3599999845,-0.3125000000,0.1875000000,0.3750000000,-12.2500000000,-20.5625000000,-20.3750000000 +2025-01-07T17:45:55.235584,-0.8899999857,9.7799997330,-0.3999999762,-0.5625000000,0.3750000000,-0.1250000000,-13.0000000000,-20.5625000000,-20.3750000000 +2025-01-07T17:45:55.316714,-0.8100000024,9.8099994659,-0.3299999833,-0.5625000000,0.0000000000,-0.1250000000,-13.3750000000,-19.8750000000,-20.0000000000 +2025-01-07T17:45:55.397102,-0.8399999738,9.7699995041,-0.2899999917,-0.2500000000,-0.1250000000,-0.5000000000,-12.2500000000,-19.8750000000,-19.5625000000 +2025-01-07T17:45:55.484345,-0.8999999762,9.7899999619,-0.2999999821,0.0625000000,-0.4375000000,-0.3125000000,-13.7500000000,-20.1875000000,-19.2500000000 +2025-01-07T17:45:55.567359,-0.8299999833,9.8000001907,-0.3599999845,0.1875000000,-0.3750000000,0.6250000000,-12.6875000000,-20.1875000000,-19.5625000000 +2025-01-07T17:45:55.650892,-0.8299999833,9.8000001907,-0.3799999952,0.1250000000,0.0000000000,-0.1875000000,-12.6875000000,-21.0000000000,-20.7500000000 +2025-01-07T17:45:55.729605,-0.8999999762,9.7899999619,-0.3400000036,-0.1250000000,0.0625000000,-0.6875000000,-12.6875000000,-19.0625000000,-21.5625000000 +2025-01-07T17:45:55.809749,-0.8599999547,9.7899999619,-0.3199999928,-0.1250000000,0.0625000000,-0.3750000000,-12.6875000000,-20.1875000000,-20.0625000000 +2025-01-07T17:45:55.889831,-0.8899999857,9.7899999619,-0.2999999821,-0.1875000000,0.6250000000,-0.3750000000,-13.3750000000,-19.8750000000,-21.5625000000 +2025-01-07T17:45:55.968885,-0.6800000072,9.7799997330,-0.3299999833,-0.1875000000,1.1250000000,-0.8750000000,-13.0000000000,-20.5625000000,-20.7500000000 +2025-01-07T17:45:56.051414,-0.9199999571,9.8000001907,-0.3499999940,0.0000000000,-0.1250000000,-0.0625000000,-13.7500000000,-19.5000000000,-20.6875000000 +2025-01-07T17:45:56.131055,-0.8499999642,9.7799997330,-0.2699999809,0.0000000000,0.5625000000,-0.1250000000,-12.2500000000,-19.8750000000,-20.2500000000 +2025-01-07T17:45:56.215095,-0.9199999571,9.7799997330,-0.3100000024,0.3125000000,0.0625000000,0.3125000000,-13.3750000000,-19.8750000000,-20.6875000000 +2025-01-07T17:45:56.292211,-0.9499999881,9.8000001907,-0.3400000036,0.0000000000,0.0000000000,0.5625000000,-13.0000000000,-19.8750000000,-19.8750000000 +2025-01-07T17:45:56.371972,-0.9300000072,9.7699995041,-0.3100000024,-0.1875000000,0.5625000000,-0.1875000000,-13.0000000000,-19.8750000000,-19.5000000000 +2025-01-07T17:45:56.452707,-0.7999999523,9.8099994659,-0.3499999940,-0.3125000000,0.1875000000,0.1250000000,-12.6875000000,-19.5000000000,-20.2500000000 +2025-01-07T17:45:56.551917,-0.8999999762,9.7799997330,-0.3100000024,-0.3125000000,0.1250000000,-0.1875000000,-12.2500000000,-19.5000000000,-20.6875000000 +2025-01-07T17:45:56.650230,-1.0000000000,9.7799997330,-0.2399999946,-0.0625000000,0.0000000000,-0.1250000000,-13.0000000000,-20.1875000000,-21.0625000000 +2025-01-07T17:45:56.729698,-0.7799999714,9.7899999619,-0.3100000024,0.0625000000,-0.1875000000,-0.0625000000,-12.2500000000,-19.5000000000,-21.0625000000 +2025-01-07T17:45:56.812723,-1.0099999905,9.7899999619,-0.2899999917,0.3750000000,0.0000000000,0.8125000000,-12.6875000000,-19.8750000000,-19.0625000000 +2025-01-07T17:45:56.895949,-0.8799999952,9.8099994659,-0.2899999917,0.5000000000,-0.3750000000,0.5625000000,-12.0000000000,-19.5000000000,-20.2500000000 +2025-01-07T17:45:56.976409,-0.9599999785,9.7899999619,-0.3299999833,0.0000000000,0.3750000000,-0.3125000000,-12.2500000000,-19.1875000000,-20.6875000000 +2025-01-07T17:45:57.056415,-0.8999999762,9.7699995041,-0.3299999833,-0.2500000000,0.5625000000,0.1875000000,-12.2500000000,-20.5625000000,-20.6875000000 +2025-01-07T17:45:57.136177,-0.8700000048,9.7899999619,-0.3100000024,0.0000000000,-0.0625000000,0.3750000000,-13.0000000000,-19.8750000000,-19.5000000000 +2025-01-07T17:45:57.216527,-0.9499999881,9.8000001907,-0.3700000048,0.1250000000,-0.6250000000,1.1250000000,-13.3750000000,-19.1875000000,-19.8750000000 +2025-01-07T17:45:57.297268,-0.8799999952,9.7799997330,-0.3299999833,-0.6250000000,-0.1875000000,0.0625000000,-12.2500000000,-19.5000000000,-20.6875000000 +2025-01-07T17:45:57.376972,-0.9799999595,9.7899999619,-0.2299999893,-0.6250000000,0.7500000000,0.1250000000,-12.6875000000,-19.8750000000,-20.6875000000 +2025-01-07T17:45:57.457879,-0.8999999762,9.7699995041,-0.2699999809,-0.1875000000,0.1875000000,1.8750000000,-12.2500000000,-20.1875000000,-20.6875000000 +2025-01-07T17:45:57.532355,-0.6899999976,9.8099994659,-0.3499999940,0.1250000000,-1.2500000000,1.9375000000,-13.3750000000,-19.8750000000,-20.6875000000 +2025-01-07T17:45:57.608843,-0.8100000024,9.8000001907,-0.3199999928,-0.4375000000,0.1875000000,0.8750000000,-12.6875000000,-19.8750000000,-21.0625000000 +2025-01-07T17:45:57.693140,-0.7400000095,9.7899999619,-0.2500000000,-0.2500000000,-0.3750000000,0.7500000000,-12.2500000000,-19.8750000000,-21.5000000000 +2025-01-07T17:45:57.770711,-0.7799999714,9.8000001907,-0.2999999821,-0.1875000000,0.0625000000,0.6875000000,-13.3750000000,-19.5000000000,-20.2500000000 +2025-01-07T17:45:57.850232,-0.8599999547,9.7899999619,-0.3499999940,-0.5625000000,0.5000000000,1.1875000000,-13.0000000000,-20.1875000000,-20.2500000000 +2025-01-07T17:45:57.930374,-0.6699999571,9.8199996948,-0.2800000012,-0.8750000000,-0.1250000000,0.3750000000,-12.6875000000,-20.1875000000,-21.5000000000 +2025-01-07T17:45:58.009076,-0.9099999666,9.7699995041,-0.2599999905,-1.1875000000,1.3125000000,-0.6250000000,-12.6875000000,-19.8750000000,-21.0625000000 +2025-01-07T17:45:58.090820,-0.7999999523,9.7799997330,-0.1999999881,-0.4375000000,1.0000000000,-0.3125000000,-12.2500000000,-19.8750000000,-21.5000000000 +2025-01-07T17:45:58.169681,-0.8899999857,9.8099994659,-0.2500000000,0.1250000000,0.0000000000,1.7500000000,-13.0000000000,-20.1875000000,-21.0625000000 +2025-01-07T17:45:58.250541,-0.4699999988,9.8299999237,-0.2299999893,0.5000000000,0.1875000000,-0.2500000000,-12.6875000000,-19.5000000000,-19.8750000000 +2025-01-07T17:45:58.333736,-0.7799999714,9.8099994659,-0.2099999934,1.0625000000,-0.6250000000,-1.6250000000,-12.6875000000,-19.8750000000,-21.5000000000 +2025-01-07T17:45:58.412990,-1.0399999619,9.7899999619,-0.2299999893,1.0000000000,0.3125000000,-0.5625000000,-12.6875000000,-20.1875000000,-20.6875000000 +2025-01-07T17:45:58.493966,-0.7599999905,9.7799997330,-0.2599999905,0.8125000000,0.0000000000,-0.3750000000,-12.0000000000,-19.8750000000,-21.0625000000 +2025-01-07T17:45:58.573059,-0.6299999952,9.8099994659,-0.3599999845,0.6875000000,-1.3125000000,0.3125000000,-12.6875000000,-19.8750000000,-20.6875000000 +2025-01-07T17:45:58.654102,-1.7300000191,9.6799993515,-0.3100000024,0.3125000000,2.7500000000,6.5625000000,-12.2500000000,-19.1875000000,-19.0625000000 +2025-01-07T17:45:58.736044,-3.3199999332,9.9200000763,-0.2099999934,6.6250000000,10.6250000000,32.8125000000,-13.7500000000,-19.8750000000,-20.6875000000 +2025-01-07T17:45:58.814798,1.8399999142,9.0399999619,-0.1299999952,-10.0625000000,17.2500000000,54.4375000000,-13.2500000000,-19.5000000000,-20.6875000000 +2025-01-07T17:45:58.887458,1.2599999905,9.9600000381,-1.8299999237,-2.0625000000,84.3750000000,72.7500000000,-13.6875000000,-19.5000000000,-18.6875000000 +2025-01-07T17:45:58.964572,-1.9699999094,9.9399995804,-1.0799999237,-38.4375000000,28.8750000000,65.6875000000,-15.0000000000,-18.7500000000,-22.3750000000 +2025-01-07T17:45:59.041501,1.7500000000,8.7899999619,0.7599999905,-36.8125000000,19.4375000000,82.0000000000,-17.0625000000,-18.8750000000,-24.0000000000 +2025-01-07T17:45:59.123427,2.0799999237,7.7599997520,2.2400000095,-28.1250000000,2.2500000000,98.5625000000,-20.6875000000,-18.5625000000,-24.3750000000 +2025-01-07T17:45:59.198109,5.7599997520,7.5199999809,2.4400000572,2.5000000000,-12.6250000000,103.4375000000,-21.8750000000,-15.6875000000,-22.3750000000 +2025-01-07T17:45:59.278435,4.9400000572,6.1799998283,0.9899999499,1.5000000000,-24.8750000000,108.7500000000,-22.1875000000,-12.7500000000,-21.0000000000 +2025-01-07T17:45:59.357586,7.4400000572,6.3499999046,0.3100000024,17.1875000000,-37.0625000000,92.5625000000,-24.2500000000,-8.5000000000,-17.2500000000 +2025-01-07T17:45:59.438190,6.6099996567,4.8699998856,0.8199999928,23.5625000000,-15.0625000000,117.8125000000,-27.0000000000,-5.2500000000,-15.8750000000 +2025-01-07T17:45:59.518392,7.0599999428,3.5399999619,-1.0499999523,22.0625000000,-51.8750000000,134.9375000000,-28.0625000000,0.7500000000,-10.6875000000 +2025-01-07T17:45:59.599096,10.8800001144,1.4299999475,-2.2899999619,21.4375000000,-65.8750000000,127.3125000000,-32.0625000000,8.5000000000,4.1875000000 +2025-01-07T17:45:59.678530,11.3199996948,1.0099999905,-2.9299998283,24.6875000000,-27.6250000000,68.8750000000,-35.7500000000,11.1875000000,21.5000000000 +2025-01-07T17:45:59.767782,15.9200000763,-2.8699998856,-1.4199999571,-8.6875000000,18.5625000000,-13.0000000000,-35.6875000000,-3.2500000000,70.5000000000 +2025-01-07T17:45:59.845379,9.5999994278,1.6799999475,-1.1200000048,5.6875000000,31.8750000000,-5.6250000000,-31.1875000000,-7.6875000000,65.6875000000 +2025-01-07T17:45:59.928786,8.6099996567,1.5799999237,-2.0899999142,-0.6875000000,-29.7500000000,5.0625000000,-39.1875000000,-6.8750000000,60.5625000000 +2025-01-07T17:46:00.011361,9.5199995041,1.4599999189,-2.7200000286,-2.5625000000,-34.2500000000,1.0625000000,-34.7500000000,-8.5625000000,65.0625000000 +2025-01-07T17:46:00.089920,9.9799995422,1.4599999189,-2.9900000095,-0.0625000000,-6.2500000000,0.5000000000,-31.5625000000,-10.3750000000,71.3750000000 +2025-01-07T17:46:00.171093,8.9899997711,1.4399999380,-2.7100000381,-0.3125000000,-6.4375000000,1.1250000000,-31.1875000000,-12.0000000000,74.5000000000 +2025-01-07T17:46:00.254163,9.5399999619,1.5000000000,-3.1399998665,-0.2500000000,-6.8125000000,0.4375000000,-30.3750000000,-12.6875000000,75.6875000000 +2025-01-07T17:46:00.331684,8.8599996567,1.4199999571,-2.9600000381,-1.8750000000,-13.5000000000,3.9375000000,-29.0625000000,-13.3750000000,76.7500000000 +2025-01-07T17:46:00.407442,8.9099998474,1.3199999332,-3.3699998856,-6.0000000000,-32.1875000000,6.5000000000,-26.6875000000,-13.3750000000,79.0000000000 +2025-01-07T17:46:00.487482,8.9399995804,1.3899999857,-3.8699998856,-5.1875000000,-44.5625000000,5.4375000000,-22.5000000000,-14.8750000000,81.0000000000 +2025-01-07T17:46:00.570736,8.3099994659,1.2099999189,-4.4699997902,-4.5000000000,-48.8125000000,10.2500000000,-12.5000000000,-16.2500000000,84.0625000000 +2025-01-07T17:46:00.647384,8.5699996948,0.8299999833,-4.5399999619,-10.6875000000,-50.9375000000,9.6875000000,-4.8750000000,-16.5625000000,85.7500000000 +2025-01-07T17:46:00.727021,8.3899993896,0.7899999619,-5.2999997139,-3.5625000000,-47.8750000000,4.8125000000,9.0000000000,-17.7500000000,80.1875000000 +2025-01-07T17:46:00.806000,7.8899998665,0.9099999666,-5.9800000191,1.1875000000,-23.5000000000,1.9375000000,22.5625000000,-16.8750000000,77.2500000000 +2025-01-07T17:46:00.885593,6.6900000572,1.0299999714,-6.1599998474,1.8750000000,-51.4375000000,8.6875000000,25.0625000000,-18.1875000000,78.1875000000 +2025-01-07T17:46:00.960687,-2.5899999142,-1.4800000191,1.8199999332,-3.3125000000,-54.6875000000,20.8125000000,36.3750000000,-15.0625000000,65.0000000000 +2025-01-07T17:46:01.045338,11.3099994659,-1.6599999666,-6.1299996376,25.8125000000,28.1875000000,-27.3750000000,40.7500000000,-9.3750000000,41.0000000000 +2025-01-07T17:46:01.126177,8.2200002670,0.3700000048,-4.6399998665,4.7500000000,34.8750000000,6.3125000000,40.8750000000,-12.6875000000,66.6875000000 +2025-01-07T17:46:01.204133,8.2100000381,0.4699999988,-5.1300001144,1.6875000000,6.7500000000,-0.5625000000,35.0625000000,-11.5625000000,71.5625000000 +2025-01-07T17:46:01.291573,8.6300001144,0.6399999857,-4.8699998856,0.6875000000,14.4375000000,-0.1250000000,34.1875000000,-12.8750000000,70.5000000000 +2025-01-07T17:46:01.368369,8.7100000381,0.5099999905,-4.6500000954,0.1875000000,10.0000000000,1.3125000000,31.7500000000,-10.5625000000,70.6875000000 +2025-01-07T17:46:01.449193,8.6499996185,0.4599999785,-4.7300000191,-0.0625000000,14.5625000000,-0.5000000000,30.8750000000,-11.2500000000,71.1875000000 +2025-01-07T17:46:01.527374,8.8299999237,0.4699999988,-4.4400000572,2.0000000000,25.9375000000,1.1250000000,27.5625000000,-10.5000000000,70.8750000000 +2025-01-07T17:46:01.605288,9.0199995041,0.4199999869,-4.0399999619,2.6250000000,34.4375000000,2.3125000000,26.0000000000,-9.6875000000,72.7500000000 +2025-01-07T17:46:01.686228,9.1700000763,0.2199999988,-3.2599999905,1.1875000000,36.3125000000,2.9375000000,19.3750000000,-9.6875000000,74.0625000000 +2025-01-07T17:46:01.761837,9.3599996567,0.4299999774,-3.1499998569,2.1250000000,55.0000000000,4.5625000000,12.0625000000,-8.0000000000,73.6875000000 +2025-01-07T17:46:01.841921,10.1099996567,-1.3199999332,4.2699999809,12.3125000000,46.5625000000,6.0625000000,8.5000000000,-6.1875000000,73.7500000000 +2025-01-07T17:46:01.917382,9.6300001144,1.6899999380,-3.1899998188,-2.6875000000,-6.6250000000,-4.1250000000,2.0625000000,-4.5000000000,70.2500000000 +2025-01-07T17:46:01.999825,9.4600000381,0.1400000006,-2.0799999237,0.5000000000,0.0000000000,-0.7500000000,0.7500000000,-5.2500000000,71.3750000000 +2025-01-07T17:46:02.080103,9.5699996948,0.3999999762,-2.2300000191,0.1250000000,-0.9375000000,-1.2500000000,2.1875000000,-5.8750000000,70.7500000000 +2025-01-07T17:46:02.153738,9.5799999237,0.3400000036,-2.1299998760,0.1875000000,0.0625000000,0.0000000000,1.3750000000,-5.2500000000,72.7500000000 +2025-01-07T17:46:02.231339,9.4799995422,0.3700000048,-2.1699998379,0.0000000000,0.1250000000,-0.1875000000,2.1875000000,-5.5625000000,72.0000000000 +2025-01-07T17:46:02.317104,9.5699996948,0.3999999762,-2.1599998474,0.1250000000,-0.1250000000,-0.5625000000,2.1875000000,-5.5625000000,71.8750000000 +2025-01-07T17:46:02.392925,9.4499998093,0.3899999857,-2.2500000000,-0.1250000000,-0.2500000000,-0.6250000000,1.3750000000,-5.2500000000,70.3750000000 +2025-01-07T17:46:02.473786,9.5699996948,0.3599999845,-2.1599998474,0.1250000000,0.0625000000,-0.4375000000,1.7500000000,-5.5625000000,72.7500000000 +2025-01-07T17:46:02.553734,9.4799995422,0.3799999952,-2.1699998379,0.0000000000,-0.1250000000,-0.3125000000,1.7500000000,-5.5625000000,71.5625000000 +2025-01-07T17:46:02.634969,9.5500001907,0.4199999869,-2.2200000286,-0.1250000000,-0.3750000000,-0.6875000000,2.7500000000,-5.5625000000,72.0000000000 +2025-01-07T17:46:02.715675,9.6199998856,0.3799999952,-2.2000000477,0.1875000000,-0.1875000000,0.0625000000,1.7500000000,-5.2500000000,71.8750000000 +2025-01-07T17:46:02.794275,9.4499998093,0.3599999845,-2.1199998856,-0.1250000000,0.1875000000,0.1875000000,1.7500000000,-5.5625000000,71.5625000000 +2025-01-07T17:46:02.874033,9.5699996948,0.4099999964,-2.2000000477,0.1875000000,-0.1250000000,-0.0625000000,2.7500000000,-5.2500000000,71.1875000000 +2025-01-07T17:46:02.955848,9.5000000000,0.4099999964,-2.2100000381,-0.1250000000,-0.1250000000,-0.3125000000,1.7500000000,-5.2500000000,72.7500000000 +2025-01-07T17:46:03.035083,9.7199993134,0.3899999857,-2.2500000000,0.0625000000,-0.2500000000,-0.6250000000,1.3750000000,-5.8750000000,72.3750000000 +2025-01-07T17:46:03.113076,9.5000000000,0.3700000048,-2.2000000477,0.0000000000,0.1875000000,-0.0625000000,2.7500000000,-4.8750000000,72.0000000000 +2025-01-07T17:46:03.195464,9.5000000000,0.4499999881,-2.1099998951,0.2500000000,-0.2500000000,-0.2500000000,2.1875000000,-5.8750000000,72.7500000000 +2025-01-07T17:46:03.274580,9.5500001907,0.3599999845,-2.1900000572,-0.0625000000,0.0625000000,-0.2500000000,1.7500000000,-5.8750000000,71.5625000000 +2025-01-07T17:46:03.359508,9.6399993896,0.4099999964,-2.2000000477,0.1250000000,-0.1875000000,0.6250000000,2.1875000000,-5.2500000000,71.8750000000 +2025-01-07T17:46:03.438249,9.4699993134,0.4399999976,-2.2300000191,-0.1250000000,0.1875000000,0.1875000000,2.1875000000,-4.8750000000,71.5625000000 +2025-01-07T17:46:03.517519,9.6499996185,0.2800000012,-2.1599998474,-0.1250000000,0.3750000000,0.1250000000,1.3750000000,-6.6875000000,71.1875000000 +2025-01-07T17:46:03.599221,9.3999996185,0.5399999619,-2.1900000572,0.0000000000,0.0625000000,-0.0625000000,1.0000000000,-5.8750000000,72.7500000000 +2025-01-07T17:46:03.677903,9.5699996948,0.2500000000,-2.0399999619,0.0625000000,0.3750000000,0.0625000000,2.1875000000,-5.2500000000,71.8750000000 +2025-01-07T17:46:03.755578,9.4099998474,0.3999999762,-2.2000000477,0.2500000000,0.0625000000,0.3750000000,2.5000000000,-5.5625000000,70.7500000000 +2025-01-07T17:46:03.838712,9.5000000000,0.3899999857,-2.0699999332,-0.0625000000,-0.0625000000,-0.2500000000,1.7500000000,-5.8750000000,71.5625000000 +2025-01-07T17:46:03.917275,9.4799995422,0.3899999857,-2.2500000000,0.0625000000,0.0625000000,0.2500000000,1.7500000000,-5.8750000000,71.8750000000 +2025-01-07T17:46:03.997955,9.5199995041,0.4699999988,-2.1299998760,0.0625000000,-0.2500000000,-0.1875000000,2.1875000000,-6.2500000000,72.7500000000 +2025-01-07T17:46:04.080224,9.5299997330,0.3100000024,-2.2000000477,0.0625000000,0.0625000000,0.1875000000,1.7500000000,-4.8750000000,72.7500000000 +2025-01-07T17:46:04.158528,9.4799995422,0.4599999785,-2.1099998951,0.0625000000,-0.1875000000,-0.0625000000,1.7500000000,-5.8750000000,73.0625000000 +2025-01-07T17:46:04.240572,9.5399999619,0.4099999964,-2.2500000000,0.0000000000,-0.1250000000,0.0000000000,2.5000000000,-6.2500000000,72.0000000000 \ No newline at end of file diff --git a/sim/resources/zbot2/robot.urdf b/sim/resources/zbot2/robot.urdf index 76283c52..43f281a0 100644 --- a/sim/resources/zbot2/robot.urdf +++ b/sim/resources/zbot2/robot.urdf @@ -1,6 +1,5 @@ - - + diff --git a/sim/resources/zbot2/robot.xml b/sim/resources/zbot2/robot.xml index 9a2ae8f0..157518eb 100644 --- a/sim/resources/zbot2/robot.xml +++ b/sim/resources/zbot2/robot.xml @@ -1,10 +1,11 @@ - + \ No newline at end of file diff --git a/sim/resources/zbot2/robot_fixed.urdf b/sim/resources/zbot2/robot_fixed.urdf index 89b0b86a..d7953e2e 100644 --- a/sim/resources/zbot2/robot_fixed.urdf +++ b/sim/resources/zbot2/robot_fixed.urdf @@ -6,7 +6,7 @@ - + diff --git a/sim/resources/zbot2/robot_fixed.xml b/sim/resources/zbot2/robot_fixed.xml index 2d73cfd0..a44b0830 100644 --- a/sim/resources/zbot2/robot_fixed.xml +++ b/sim/resources/zbot2/robot_fixed.xml @@ -1,4 +1,4 @@ - + \ No newline at end of file diff --git a/sim/scripts/imu_data_comparison.py b/sim/scripts/imu_data_comparison.py new file mode 100644 index 00000000..8487a2a5 --- /dev/null +++ b/sim/scripts/imu_data_comparison.py @@ -0,0 +1,245 @@ +"""Testing the falling down IMU data comparison. + +Run: + python sim/scripts/imu_data_comparison.py --embodiment zbot2 +""" +import argparse +import os +from copy import deepcopy + +import matplotlib.pyplot as plt +import mediapy as media +import mujoco +import mujoco_viewer +import numpy as np +import pandas as pd +from tqdm import tqdm + + +def plot_comparison(sim_data: pd.DataFrame, real_data: pd.DataFrame) -> None: + """Plot the real IMU data. + + Args: + sim_data: The simulated IMU data. + real_data: The real IMU data. + """ + plt.figure(figsize=(10, 15)) + + + real_timestamps = (real_data['timestamp'] - real_data['timestamp'].iloc[0]).dt.total_seconds().to_numpy() + + # Accelerometer plots + plt.subplot(6, 1, 1) + plt.plot(real_timestamps, sim_data['accel_x'].to_numpy(), label='Simulated Accel X') + plt.plot(real_timestamps, real_data['accel_x'].to_numpy(), label='Real Accel X') + plt.title('Accelerometer X') + plt.legend() + + plt.subplot(6, 1, 2) + plt.plot(real_timestamps, sim_data['accel_y'].to_numpy(), label='Simulated Accel Y') + plt.plot(real_timestamps, real_data['accel_y'].to_numpy(), label='Real Accel Y') + plt.title('Accelerometer Y') + plt.legend() + + plt.subplot(6, 1, 3) + plt.plot(real_timestamps, sim_data['accel_z'].to_numpy(), label='Simulated Accel Z') + plt.plot(real_timestamps, real_data['accel_z'].to_numpy(), label='Real Accel Z') + plt.title('Accelerometer Z') + plt.legend() + + # Gyroscope plots + plt.subplot(6, 1, 4) + plt.plot(real_timestamps, sim_data['gyro_x'].to_numpy(), label='Simulated Gyro X') + plt.plot(real_timestamps, real_data['gyro_x'].to_numpy(), label='Real Gyro X') + plt.title('Gyroscope X') + plt.legend() + + plt.subplot(6, 1, 5) + plt.plot(real_timestamps, sim_data['gyro_y'].to_numpy(), label='Simulated Gyro Y') + plt.plot(real_timestamps, real_data['gyro_y'].to_numpy(), label='Real Gyro Y') + plt.title('Gyroscope Y') + plt.legend() + + plt.subplot(6, 1, 6) + plt.plot(real_timestamps, sim_data['gyro_z'].to_numpy(), label='Simulated Gyro Z') + plt.plot(real_timestamps, real_data['gyro_z'].to_numpy(), label='Real Gyro Z') + plt.title('Gyroscope Z') + plt.legend() + + plt.tight_layout() + plt.savefig('imu_data_comparison.png') + + +def read_real_data(data_file: str = "sim/resources/zbot2/imu_data.csv") -> None: + """Plot the real IMU data. + + Args: + data_file: The path to the real IMU data file. + + Returns: + The real IMU data. + """ + # Reading the data from CSV file + df = pd.read_csv(data_file) + + df = df.apply(pd.to_numeric, errors='ignore') + df['timestamp'] = pd.to_datetime(df['timestamp']) + + return df + + +def pd_control( + target_q: np.ndarray, + q: np.ndarray, + kp: np.ndarray, + dq: np.ndarray, + kd: np.ndarray, + default: np.ndarray, +) -> np.ndarray: + """Calculates torques from position commands + + Args: + target_q: The target position. + q: The current position. + kp: The proportional gain. + dq: The current velocity. + kd: The derivative gain. + default: The default position. + + Returns: + The calculated torques. + """ + return kp * (target_q + default - q) - kd * dq + + +def run_simulation( + embodiment: str, + kp: float = 1.0, + kd: float = 1.0, + sim_duration: float = 15.0, + effort: float = 5.0, +) -> None: + """ + Run the Mujoco simulation using the provided policy and configuration. + + Args: + embodiment: The embodiment to use for the simulation. + kp: The proportional gain. + kd: The derivative gain. + sim_duration: The duration of the simulation. + effort: The effort to apply to the robot. + """ + model_info = { + "sim_dt": 0.001, + "tau_factor": 2, + "num_actions": 10, + "num_observations": 10, + "robot_effort": [effort] * 10, + "robot_stiffness": [kp] * 10, + "robot_damping": [kd] * 10, + } + frames = [] + framerate = 30 + model_dir = os.environ.get("MODEL_DIR", "sim/resources") + mujoco_model_path = f"{model_dir}/{embodiment}/robot_fixed.xml" + + model = mujoco.MjModel.from_xml_path(mujoco_model_path) + model.opt.timestep = model_info["sim_dt"] + data = mujoco.MjData(model) + + tau_limit = np.array(list(model_info["robot_effort"])) * model_info["tau_factor"] + kps = np.array(model_info["robot_stiffness"]) + kds = np.array(model_info["robot_damping"]) + + data.qpos = model.keyframe("standing").qpos + default = deepcopy(model.keyframe("standing").qpos)[-model_info["num_actions"] :] + print("Default position:", default) + + target_q = np.zeros((model_info["num_actions"]), dtype=np.double) + viewer = mujoco_viewer.MujocoViewer(model, data,"offscreen") + + force_duration = 400 # Duration of force application in timesteps + force_timer = 0 + + applied_force = np.array([0.0, -3, 0.0]) + + sim_data = { + "timestamp": [], + "gyro_x": [], + "gyro_y": [], + "gyro_z": [], + "accel_x": [], + "accel_y": [], + "accel_z": [], + "mag_x": [], + "mag_y": [], + "mag_z": [], + } + + for timestep in tqdm(range(int(sim_duration / model_info["sim_dt"])), desc="Simulating..."): + if timestep == 500: + force_timer = force_duration + if timestep % 10 == 0: + # Keep the robot in the same position + q = data.qpos.astype(np.double)[-model_info["num_actions"] :] + dq = data.qvel.astype(np.double)[-model_info["num_actions"] :] + tau = pd_control(target_q, q, kps, dq, kds, default) # Calc torques + tau = np.clip(tau, -tau_limit, tau_limit) # Clamp torques + data.ctrl = tau + mujoco.mj_step(model, data) + if timestep % 100 == 0: + img = viewer.read_pixels() + frames.append(img) + + # Obtain an observation + gyroscope = data.sensor("angular-velocity").data.astype(np.double) + accelerometer = data.sensor("linear-acceleration").data.astype(np.double) + magnetometer = data.sensor("magnetometer").data.astype(np.double) + + sim_data["timestamp"].append(timestep * model_info["sim_dt"]) + sim_data["gyro_x"].append(gyroscope[0]) + sim_data["gyro_y"].append(gyroscope[1]) + sim_data["gyro_z"].append(gyroscope[2]) + sim_data["accel_x"].append(accelerometer[0]) + sim_data["accel_y"].append(accelerometer[1]) + sim_data["accel_z"].append(accelerometer[2]) + sim_data["mag_x"].append(magnetometer[0]) + sim_data["mag_y"].append(magnetometer[1]) + sim_data["mag_z"].append(magnetometer[2]) + + if timestep == 12680: + break + + if force_timer > 0: + # Apply force if timer is active + if force_timer > 0: + data.xfrc_applied[1] = np.concatenate([applied_force, np.zeros(3)]) + force_timer -= 1 + else: + data.xfrc_applied[1] = np.zeros(6) + + media.write_video("push_tests.mp4", frames, fps=framerate) + + # sim_data["timestamp"] = np.array(sim_data["timestamp"]) + # sim_data["gyro_x"] = np.array(sim_data["gyro_x"]) + # sim_data["gyro_y"] = np.array(sim_data["gyro_y"]) + # sim_data["gyro_z"] = np.array(sim_data["gyro_z"]) + # sim_data["accel_x"] = np.array(sim_data["accel_x"]) + # sim_data["accel_y"] = np.array(sim_data["accel_y"]) + # sim_data["accel_z"] = np.array(sim_data["accel_z"]) + # sim_data["mag_x"] = np.array(sim_data["mag_x"]) + # sim_data["mag_y"] = np.array(sim_data["mag_y"]) + # sim_data["mag_z"] = np.array(sim_data["mag_z"]) + return pd.DataFrame(sim_data) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Deployment script.") + parser.add_argument("--embodiment", type=str, required=True, help="Embodiment name.") + parser.add_argument("--kp", type=float, default=10.0, help="Path to run to load from.") + parser.add_argument("--kd", type=float, default=1.0, help="Path to run to load from.") + args = parser.parse_args() + + sim_data = run_simulation(args.embodiment, args.kp, args.kd) + real_data = read_real_data() + plot_comparison(sim_data, real_data) \ No newline at end of file