IVIGIT.

unitTesting/python/test_dyndrift.py

+# Test the computation of the dynamic values, with respect to values computed
+# in the sot v1 with Layale spatial computations.
+# All displayed values should be small.
+
+
+from numpy import *
+from dynamic_graph import plug
+from dynamic_graph.sot.core import *
+from dynamic_graph.sot.dynamics import *
+from dynamic_graph.sot.dyninv import *
+
+# --- Dynamic parameters ---
+hrp2_14_pkgdatarootdir = "/home/nmansard/compil/devgiri/hpp2/share/hrp2_14"
+modelDir = hrp2_14_pkgdatarootdir
+xmlDir = hrp2_14_pkgdatarootdir
+modelName = 'HRP2JRLmainsmall.wrl'
+specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
+jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
+robotDim=36
+
+# --- Dynamic model
+dyn = Dynamic("dyn")
+dyn.setFiles(modelDir, modelName,specificitiesPath,jointRankPath)
+dyn.parse()
+
+dyn.inertiaRotor.value = (0,0,0,0,0,0,1.01e-4,6.96e-4,1.34e-4,1.34e-4,6.96e-4,6.96e-4,1.01e-4,6.96e-4,1.34e-4,1.34e-4,6.96e-4,6.96e-4,6.96e-4,6.96e-4,1.10e-4,1.10e-4,6.96e-4,6.60e-4,1.00e-4,6.60e-4,1.10e-4,1.00e-4,1.00e-4,6.96e-4,6.60e-4,1.00e-4,6.60e-4,1.10e-4,1.00e-4,1.00e-4)
+dyn.gearRatio.value = (0,0,0,0,0,0,384.0,240.0,180.0,200.0,180.0,100.0,384.0,240.0,180.0,200.0,180.0,100.0,207.69,381.54,100.0,100.0,219.23,231.25,266.67,250.0,145.45,350.0,200.0,219.23,231.25,266.67,250.0,145.45,350.0,200.0)
+
+dyn.setProperty('ComputeBackwardDynamics','true')
+dyn.setProperty('ComputeAccelerationCoM ','true')
+
+
+dyn.position.value =(0.0274148975459,0.143849747317,0.646921274968,0.00221087236523,0.101404957372,1.37614964546e-05,-0.00911630330837,-0.0914099637938,-0.471978743283,0.840380192617,-0.470232799053,0.0896624078591,0.00950781802257,0.0911102868041,-0.469450351848,0.835307995386,-0.467686190904,-0.0938029466367,-8.75646582964e-05,0.00326431877374,-7.83380820086e-06,0.000194742578013,0.258370257314,-0.175099102142,-6.1173425555e-05,-0.524953548768,3.1825099713e-06,-0.000257600047423,-3.41210481921e-06,0.258367271628,0.174322097546,-8.89023954997e-05,-0.524983690846,-3.46102941488e-07,-0.000265401439772,1.00498481453e-06)
+dyn.velocity.value = 36*(0,)
+dyn.acceleration.value = 36*(0,)
+
+dyn.dynamicDrift.recompute(0)
+ddrift = array(dyn.dynamicDrift.value)
+
+ref = array(( 6.15826833972e-15, 2.04697370165e-15, 557.874599016, 0.641696754304, -18.1784009655, -3.53009451098e-16, -0.561345854374, -5.59139028064, -1.66325193145, 3.79937151465, -0.042195289264, -0.0753854655127, 0.577468383095, 5.68293013333, -1.64852865759, 3.77694588165, -0.0421792204661, 0.0753854655127, 0.0760309106086, 2.98886493437, 0.00199513302756, -0.206211080736, 2.37751559015, -2.48334149087, -0.506787659257, -0.865989320327, -0.000989247550942, -0.201930987365, -0.0213613521542, 2.41976949542, 2.5218506675, 0.505404479576, -0.823853743424, -0.00357458175468, -0.161145047381, -0.00793718359791 ))
+
+
+print((abs(ref-ddrift)>1e-7)*(ddrift-ref)/ref)
+
+
+
+dyn.position.value = ( 0.02603, 0.09835, 0.6453, -0.01779, 0.09934, -0.01558, 0.01489, 0.01004, -0.4972, 0.8887, -0.4911, 0.00623, 0.008817, 0.1831, -0.4878, 0.824, -0.4412, -0.1657, -0.01737, 0.02778, 0.0002533, 0.001301, 0.2731, -0.2241, -0.0132, -0.5199, -0.0001437, 0.0008949, -0.0005441, 0.2579, 0.1192, -0.01429, -0.5177, 0.0001058, 0.001301, 0.0005515)
+dyn.ffposition.value = ( 0.02617, 0.09882, 0.6449, -0.01741, 0.09943, -0.01509)
+dyn.velocity.value =  ( -0.04147, -0.4612, -0.03386, -0.3734, -0.08667, -0.4593, 0.5794, 1.172, -0.2305, 0.3704, -0.06163, -0.8505, -0.3068, 0.8128, -1.248, 0.8956, 0.2726, -0.4367, -0.202, 0.8302, 0.0031, 0.03749, 0.3079, -0.2942, -0.09078, 0.1925, -0.001377, 0.04434, -0.003849, 0.1717, -0.499, -0.1323, 0.2262, 0.0007263, 0.04999, 0.004995 )
+
+dyn.dynamicDrift.recompute(1)
+ddrift = array(dyn.dynamicDrift.value)
+
+ref = ( -10.4462414565, -4.3292836684, 572.739982784, 4.3826925928, -20.6895818638, -0.309654185483, -0.509501347316, -3.96038629169, -1.09416860245, 4.58301173612, -0.000338502715373, -0.0845375737968, 0.334196137848, 5.4536935243, -2.48838005393, 3.68382917018, -0.0741738186845, -0.0323824017553, 0.16063411303, 3.3095960491, -0.00598500403802, -0.232210715353, 3.33374916121, -2.53815497569, -0.493590609397, -0.539999494748, -0.00130282444639, -0.139868810641, -0.0205679407975, 3.59400850451, 1.9369355767, 0.434182418961, -0.435862556043, -0.00358566323652, -0.0793825642475, -0.00601992786276)
+
+print((abs(ref-ddrift)>1e-7)*(ddrift-ref)/ref)
+
+
+
+
+
+dyn.inertia.recompute(2)
+inertia = dyn.inertia.value
+
+inertiaRef = array((( 56.86795097, 0, 0, 0, 9.02470818748, -0.513553850349, 0.340916888107, -0.0381020152154, -2.34844376815, -0.943308251678, -0.10877179732, -4.95751437323e-05, -0.764265726167, 0.0874688770777, -2.37397921028, -0.965110373621, -0.108460788158, 0.0024987286952, 0.0479596069503, 1.32694523125, -0.00128311347439, 0.0758969910749, -1.32827533222, -0.092977682241, 0.00815005591961, -0.553139030052, 0.00159196118054, -0.105716386044, -0.000320902708953, -1.36094402765, 0.114588879074, 0.00450752777591, -0.550959788883, -0.00043926070454, -0.10571364841, -0.000908742104883),   (0, 56.86795097, 0, -9.02470818748, 0, 1.92652295354, 0.410448520579, 2.31787491793, -0.00219429646583, 0.00194762764744, -7.49864040047e-05, 0.107147785642, 0.451848291201, 2.25147436824, 0.0911743743159, -0.0373173165435, 0.00327376633775, 0.107115480145, -0.444475901252, -0.0349576830043, 0.0127225130237, -0.00274148720609, 0.0462340335434, 1.33386790108, 0.261942546091, -0.0400791410749, 0.00219381634416, -0.00848285880832, 0.00877606731312, 0.0471559048689, 1.36934242699, 0.264640327235, 0.0551021198473, -0.00210238178572, 0.0105770275177, -0.00917176051472),   (0, 0, 56.86795097, 0.513553850349, -1.92652295354, 0, -0.0406590950292, -0.383949789425, -0.191330099074, 0.406429435016, -0.00430983902285, -0.00764423390295, 0.0692100189475, 0.735056817302, -0.219332540854, 0.35540037299, -0.00469098704753, 0.00768862834576, 0.0023110444299, 0.300385891187, -2.80490075177e-05, -0.0227959493311, 0.289188322556, -0.341878103124, -0.0702260943961, -0.0626805730269, -0.000301899269019, -0.015551972784, -0.00279824362629, 0.285006035381, 0.153085795534, 0.0305416055484, -0.0700059385031, -0.000216927339369, -0.0136158548917, -7.416382281e-05),   (0, -9.02470818748, 0.513553850349, 9.60483303449, -0.0603020252951, -0.553706541733, 0.155119773391, 1.20377852895, 0.0249828097142, -0.0422949887711, 0.000467975474715, 0.0696401381491, 0.181963830508, 1.25458718499, -0.00114626822162, 0.0474244798953, 0.00125024320328, 0.0700185628768, 0.310661063016, 0.0411928322304, -0.00767210589372, 0.00183520593937, -0.097581694413, -0.0349493522061, 0.0262794477324, 0.0235880355895, 0.000471517595361, 0.00544512921934, 0.00194216877554, 0.079249973408, -0.0808789890779, 0.0153615837082, -0.0210545611783, -0.000234618827988, -0.00343662312502, -0.00119439216176),   (9.02470818748, 0, -1.92652295354, -0.0603020486212, 8.34725231947, 0.0819925414426, -0.101304604736, 0.0258591604313, 1.19441562662, 0.546731159933, 0.0733363639979, -7.35866779443e-05, 0.304251621797, -0.0617408110411, 1.19414481323, 0.548942990746, 0.0728607120291, -0.00161991630966, 0.00101765106769, 1.53077078055, -0.000782267065671, 0.0618079596602, -0.138789634747, -0.00587244545582, 0.00867770310809, -0.0052148685875, 0.000354174660425, 0.00667801240344, 0.000177313621399, -0.119984195089, 0.00736682005592, -0.00818007825022, 0.00957290890096, -0.000489342591947, 0.00955416948939, 0.00011301822002),   (-0.513553850349, 1.92652295354, 0, -0.553706501441, 0.0819925579913, 1.90394172822, 0.0909290659, 0.0228226849751, -0.263661495897, -0.10138538624, -0.0114133106788, -0.00208521612127, 0.187706767901, 0.0376948047466, 0.449780370203, 0.186908661156, 0.022085164062, -0.000785847487639, 1.20345682711, -0.0635702286675, 0.00768894524829, -0.00128476384184, -0.462393049379, -0.00248658463989, 0.0133541088622, -0.204943135534, 0.00192646920944, -0.0407963597097, 0.000357624409966, 0.415089367132, 0.0240006497361, 0.0155201689623, 0.172886483011, 0.00129859081842, 0.0335849525676, -0.000256631470019),   (0.340916888107, 0.410448520579, -0.0406590950292, 0.155119783324, -0.101304586707, 0.0909290646183, 0.0840653013456, 0.144972951912, -0.101777905605, -0.0376446142388, -0.00367431128744, 0.00490756075627, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (-0.0381020152154, 2.31787491793, -0.383949789425, 1.20377852795, 0.0258591594187, 0.0228226750305, 0.144972941903, 1.17459649308, 0.0255032627736, -0.0134357482844, 0.000685458302335, 0.0691476806116, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (-2.34844376815, -0.00219429646583, -0.191330099074, 0.0249828088689, 1.19441562652, -0.263661513935, -0.101777923636, 0.0255032637275, 1.1977018955, 0.547646845317, 0.0734801291354, -8.74182099774e-05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (-0.943308251678, 0.00194762764744, 0.406429435016, -0.0422949934877, 0.54673115988, -0.101385395058, -0.0376446234817, -0.0134357521001, 0.547646845317, 0.333924789259, 0.0428925119194, -0.000902514838823, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (-0.10877179732, -7.49864040047e-05, -0.00430983902285, 0.000467975474715, 0.0733363639979, -0.0114133106788, -0.00367431128744, 0.000685458302335, 0.0734801291354, 0.0428925119194, 0.0147286187714, 9.02274643345e-05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (-4.95751437323e-05, 0.107147785642, -0.00764423390295, 0.0696401381491, -7.35866779443e-05, -0.00208521612127, 0.00490756075627, 0.0691476806116, -8.74182099774e-05, -0.000902514838823, 9.02274643345e-05, 0.0110420682603, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (-0.764265726167, 0.451848291201, 0.0692100189475, 0.181963839957, 0.304251603466, 0.187706767249, 0, 0, 0, 0, 0, 0, 0.163254181536, 0.160170658895, 0.325880322905, 0.139712387715, 0.0168065543991, 0.00616628253964, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (0.0874688770777, 2.25147436824, 0.735056817302, 1.25458718405, -0.0617408080086, 0.0376947951628, 0, 0, 0, 0, 0, 0, 0.160170649315, 1.19843460822, -0.0260807810106, 0.00978647853516, -0.000478883372156, 0.0691823954613, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (-2.37397921028, 0.0911743743159, -0.219332540854, -0.00114626782989, 1.19414481019, 0.449780388273, 0, 0, 0, 0, 0, 0, 0.325880340873, -0.0260807823984, 1.22583333697, 0.562451586378, 0.0743328961827, -0.000136392545999, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (-0.965110373621, -0.0373173165435, 0.35540037299, 0.0474244840937, 0.548942989154, 0.186908670091, 0, 0, 0, 0, 0, 0, 0.139712396997, 0.00978648183418, 0.562451586378, 0.335402829911, 0.0436477262098, -0.0012674254967, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (-0.108460788158, 0.00327376633775, -0.00469098704753, 0.00125024320328, 0.0728607120291, 0.022085164062, 0, 0, 0, 0, 0, 0, 0.0168065543991, -0.000478883372156, 0.0743328961827, 0.0436477262098, 0.0147610066997, 2.56247486237e-05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (0.0024987286952, 0.107115480145, 0.00768862834576, 0.0700185628768, -0.00161991630966, -0.000785847487639, 0, 0, 0, 0, 0, 0, 0.00616628253964, 0.0691823954613, -0.000136392545999, -0.0012674254967, 2.56247486237e-05, 0.0110420682603, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (0.0479596069503, -0.444475901252, 0.0023110444299, 0.310661083485, 0.00101768015229, 1.20345682459, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.24245973753, -0.0340764471647, 0.00646747938307, -6.96590261451e-05, -0.472708219318, -0.0488682348116, 0.00761650025662, -0.200151948953, 0.00189818242039, -0.0396327037015, 0.000269695005692, 0.418184455413, -0.0278245740519, 0.00838808914902, 0.168497705139, 0.00132817840113, 0.03292331812, -7.85988331203e-05),   (1.32694523125, -0.0349576830043, 0.300385891187, 0.0411928194549, 1.53077078039, -0.0635702572802, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0340764769053, 1.09321211632, -0.000570724376908, 0.034068390017, 0.345972430779, 0.0183202194207, 0.00484135835504, 0.185939076177, -0.000212556311594, 0.0430187672094, 0.000258738964378, 0.364495432076, -0.00874507575316, -0.0043858069072, 0.192042455675, -0.000408506030894, 0.0445583546738, 0.000281561648034),   (-0.00128311347439, 0.0127225130237, -2.80490075177e-05, -0.00767210589372, -0.000782267065671, 0.00768894524829, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00646747938307, -0.000570724376908, 0.00644481042723, -7.04226400633e-05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (0.0758969910749, -0.00274148720609, -0.0227959493311, 0.00183520593937, 0.0618079596602, -0.00128476384184, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -6.96590261451e-05, 0.034068390017, -7.04226400633e-05, 0.0109045159936, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),   (-1.32827533222, 0.0462340335434, 0.289188322556, -0.0975816961869, -0.138789634882, -0.462393059342, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.472708229409, 0.345972430779, 0, 0, 0.592174948809, 0.0324476565225, 0.00280829753607, 0.282459805405, -0.000492629816743, 0.0613507325545, 0.000282868124954, 0, 0, 0, 0, 0, 0, 0),   (-0.092977682241, 1.33386790108, -0.341878103124, -0.0349493561253, -0.00587244776109, -0.00248659369337, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0488682442456, 0.0183202171693, 0, 0, 0.032447654271, 0.579536586606, 0.140883107463, 0.00438836775012, 0.000930141242212, 0.000846557934588, 0.00557118768364, 0, 0, 0, 0, 0, 0, 0),   (0.00815005591961, 0.261942546091, -0.0702260943961, 0.0262794564971, 0.00867771234889, 0.0133541027151, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0076164951599, 0.00484136797544, 0, 0, 0.00280830715647, 0.140883117594, 0.0458431010571, -0.000998301777211, 0.00168624568716, -4.65638045739e-05, 0.00198981416568, 0, 0, 0, 0, 0, 0, 0),   (-0.553139030052, -0.0400791410749, -0.0626805730269, 0.0235880355895, -0.0052148685875, -0.204943135534, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.200151948953, 0.185939076177, 0, 0, 0.282459805405, 0.00438836775012, -0.000998301777211, 0.169135171558, -0.000508104339599, 0.0402025912998, 1.75394069784e-07, 0, 0, 0, 0, 0, 0, 0),   (0.00159196118054, 0.00219381634416, -0.000301899269019, 0.000471517595361, 0.000354174660425, 0.00192646920944, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00189818242039, -0.000212556311594, 0, 0, -0.000492629816743, 0.000930141242212, 0.00168624568716, -0.000508104339599, 0.00126029506669, -4.63335149203e-05, 9.95356639797e-05, 0, 0, 0, 0, 0, 0, 0),   (-0.105716386044, -0.00848285880832, -0.015551972784, 0.00544512921934, 0.00667801240344, -0.0407963597097, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0396327037015, 0.0430187672094, 0, 0, 0.0613507325545, 0.000846557934588, -4.65638045739e-05, 0.0402025912998, -4.63335149203e-05, 0.013424078746, 5.64047173551e-07, 0, 0, 0, 0, 0, 0, 0),   (-0.000320902708953, 0.00877606731312, -0.00279824362629, 0.00194216877554, 0.000177313621399, 0.000357624409966, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.000269695005692, 0.000258738964378, 0, 0, 0.000282868124954, 0.00557118768364, 0.00198981416568, 1.75394069784e-07, 9.95356639797e-05, 5.64047173551e-07, 0.000675783014847, 0, 0, 0, 0, 0, 0, 0),   (-1.36094402765, 0.0471559048689, 0.285006035381, 0.0792499761332, -0.119984194985, 0.41508937697, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.418184465474, 0.364495432076, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.616403629102, -0.0161744887043, -0.00264498545019, 0.288525051974, -0.000349555102925, 0.0630650067676, 0.000385857807184),   (0.114588879074, 1.36934242699, 0.153085795534, -0.0808789927191, 0.00736682116714, 0.024000640645, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0278245834414, -0.00874507458109, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0161744875323, 0.582784027046, 0.138738863605, 0.00396307423271, -0.00169510899408, 0.000936549733259, -0.00570053882302),   (0.00450752777591, 0.264640327235, 0.0305416055484, 0.0153615920753, -0.00818008867135, 0.0155201643373, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00838808521431, -0.00438581697711, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0026449955201, 0.138738873461, 0.044057934197, 0.00024704864082, 0.000860267466088, -0.000198919905024, -0.00182087441397),   (-0.550959788883, 0.0551021198473, -0.0700059385031, -0.0210545611783, 0.00957290890096, 0.172886483011, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.168497705139, 0.192042455675, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.288525051974, 0.00396307423271, 0.00024704864082, 0.169133219334, -0.000328239076966, 0.0402016106633, -7.90368878813e-08),   (-0.00043926070454, -0.00210238178572, -0.000216927339369, -0.000234618827988, -0.000489342591947, 0.00129859081842, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00132817840113, -0.000408506030894, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.000349555102925, -0.00169510899408, 0.000860267466088, -0.000328239076966, 0.00131933930816, -0.000188699048521, 0.000102942264638),   (-0.10571364841, 0.0105770275177, -0.0136158548917, -0.00343662312502, 0.00955416948939, 0.0335849525676, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.03292331812, 0.0445583546738, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0630650067676, 0.000936549733259, -0.000198919905024, 0.0402016106633, -0.000188699048521, 0.013424077006, 5.64793897724e-07),   (-0.000908742104883, -0.00917176051472, -7.416382281e-05, -0.00119439216176, 0.00011301822002, -0.000256631470019, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -7.85988331203e-05, 0.000281561648034, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.000385857807184, -0.00570053882302, -0.00182087441397, -7.90368878813e-08, 0.000102942264638, 5.64793897724e-07, 0.000675783014847 )))
+
+delta = (inertiaRef-inertia)
+print((abs(delta)>1e-7)*delta )
+
+
+
+