{"action":"created","issue":{"url":"https://api.github.com/repos/robotology/idyntree/issues/98","labels_url":"https://api.github.com/repos/robotology/idyntree/issues/98/labels{/name}","comments_url":"https://api.github.com/repos/robotology/idyntree/issues/98/comments","events_url":"https://api.github.com/repos/robotology/idyntree/issues/98/events","html_url":"https://github.com/robotology/idyntree/issues/98","id":118279628,"number":98,"title":"Optimize the new iDynTree data structures/algorithms ","user":{"login":"traversaro","id":1857049,"avatar_url":"https://avatars.githubusercontent.com/u/1857049?v=3","gravatar_id":"","url":"https://api.github.com/users/traversaro","html_url":"https://github.com/traversaro","followers_url":"https://api.github.com/users/traversaro/followers","following_url":"https://api.github.com/users/traversaro/following{/other_user}","gists_url":"https://api.github.com/users/traversaro/gists{/gist_id}","starred_url":"https://api.github.com/users/traversaro/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/traversaro/subscriptions","organizations_url":"https://api.github.com/users/traversaro/orgs","repos_url":"https://api.github.com/users/traversaro/repos","events_url":"https://api.github.com/users/traversaro/events{/privacy}","received_events_url":"https://api.github.com/users/traversaro/received_events","type":"User","site_admin":false},"labels":[],"state":"open","locked":false,"assignee":null,"milestone":null,"comments":4,"created_at":"2015-11-22T18:24:50Z","updated_at":"2015-11-23T17:15:00Z","closed_at":null,"body":"Over the last months we implemented the new data structures to implement all our algorithm without depending anymore on KDL. \r\nFrom the performance point of view this process was relatively in open loop...  and now it is time to close the loop. : ) \r\nI committed a basic benchmark in https://github.com/robotology/idyntree/commit/520df5b2ef9696f541819c0a192c3080186db309 , the results on my pc with a random load are : \r\n\r\n~~~\r\npegua@pareto:~/src/codyco-superbuild/build/libraries/iDynTree$ ./src/tests/benchmark/DynamicsBenchmark \r\nDynamics benchmark, iDynTree built in Release mode \r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/oneLink.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 0.290871 microseconds\r\n\tCRBA average time 0.33617 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 3.14713 microseconds\r\n\tCRBA average time 0.298023 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 10.8197\r\n\tCRBA average time 0.886525\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/twoLinks.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 1.14679 microseconds\r\n\tCRBA average time 2.15292 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 20.0534 microseconds\r\n\tCRBA average time 8.07524 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 17.4865\r\n\tCRBA average time 3.75083\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/threeLinks.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 1.83582 microseconds\r\n\tCRBA average time 3.4523 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 38.9028 microseconds\r\n\tCRBA average time 21.5983 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 21.1909\r\n\tCRBA average time 6.25622\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/twoLinksFixed.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 0.760555 microseconds\r\n\tCRBA average time 0.972748 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 8.45671 microseconds\r\n\tCRBA average time 1.74522 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 11.1191\r\n\tCRBA average time 1.79412\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/icubTwoLinks.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 0.505447 microseconds\r\n\tCRBA average time 1.02758 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 10.3426 microseconds\r\n\tCRBA average time 4.24862 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 20.4623\r\n\tCRBA average time 4.13457\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/icub.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 12.6314 microseconds\r\n\tCRBA average time 36.0465 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 269.737 microseconds\r\n\tCRBA average time 375.643 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 21.3545\r\n\tCRBA average time 10.4211\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/icub_model.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 15.1253 microseconds\r\n\tCRBA average time 39.7229 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 302.987 microseconds\r\n\tCRBA average time 386.999 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 20.0318\r\n\tCRBA average time 9.74245\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/icub_skin_frames.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 14.8702 microseconds\r\n\tCRBA average time 37.3697 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 290.613 microseconds\r\n\tCRBA average time 369.844 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 19.5434\r\n\tCRBA average time 9.8969\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/bigman.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 18.5442 microseconds\r\n\tCRBA average time 38.1804 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 342.174 microseconds\r\n\tCRBA average time 349.6 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 18.4518\r\n\tCRBA average time 9.15655\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/robotModelTestTwoLinks.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 0.455379 microseconds\r\n\tCRBA average time 1.01328 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 10.2782 microseconds\r\n\tCRBA average time 4.24147 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 22.5707\r\n\tCRBA average time 4.18588\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/icub2BB5Sea.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 25.4083 microseconds\r\n\tCRBA average time 42.6245 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 396.819 microseconds\r\n\tCRBA average time 285.847 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 15.6177\r\n\tCRBA average time 6.70618\r\npegua@pareto:~/src/codyco-superbuild/build/libraries/iDynTree$ ./src/tests/benchmark/DynamicsBenchmark \r\nDynamics benchmark, iDynTree built in Release mode \r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/oneLink.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 0.216961 microseconds\r\n\tCRBA average time 0.193119 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 1.87397 microseconds\r\n\tCRBA average time 0.195503 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 8.63736\r\n\tCRBA average time 1.01235\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/twoLinks.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 0.61512 microseconds\r\n\tCRBA average time 1.17302 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 11.301 microseconds\r\n\tCRBA average time 4.63963 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 18.3721\r\n\tCRBA average time 3.95528\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/threeLinks.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 0.734329 microseconds\r\n\tCRBA average time 1.52111 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 17.6311 microseconds\r\n\tCRBA average time 9.51767 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 24.0097\r\n\tCRBA average time 6.25705\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/twoLinksFixed.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 0.445843 microseconds\r\n\tCRBA average time 0.579357 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 5.42402 microseconds\r\n\tCRBA average time 1.00136 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 12.1658\r\n\tCRBA average time 1.7284\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/icubTwoLinks.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 0.572205 microseconds\r\n\tCRBA average time 1.02282 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 12.0878 microseconds\r\n\tCRBA average time 4.25577 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 21.125\r\n\tCRBA average time 4.16084\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/icub.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 12.126 microseconds\r\n\tCRBA average time 35.0285 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 281.32 microseconds\r\n\tCRBA average time 367.453 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 23.1998\r\n\tCRBA average time 10.4901\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/icub_model.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 14.3313 microseconds\r\n\tCRBA average time 37.5533 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 291.858 microseconds\r\n\tCRBA average time 392.578 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 20.365\r\n\tCRBA average time 10.4539\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/icub_skin_frames.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 13.7496 microseconds\r\n\tCRBA average time 36.5472 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 284.963 microseconds\r\n\tCRBA average time 358.534 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 20.7252\r\n\tCRBA average time 9.81016\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/bigman.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 18.0888 microseconds\r\n\tCRBA average time 37.2195 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 331.836 microseconds\r\n\tCRBA average time 346.487 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 18.3448\r\n\tCRBA average time 9.30927\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/robotModelTestTwoLinks.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 0.531673 microseconds\r\n\tCRBA average time 1.02043 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 10.3855 microseconds\r\n\tCRBA average time 4.22239 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 19.5336\r\n\tCRBA average time 4.13785\r\nBenchmarking dynamics algorithms for /home/pegua/src/codyco-superbuild/libraries/iDynTree/src/tests/data/icub2BB5Sea.urdf\r\nKDL-based implementation : \r\n\tRNEA average time 25.2318 microseconds\r\n\tCRBA average time 41.6279 microseconds\r\niDynTree-based implementation : \r\n\tRNEA average time 395.868 microseconds\r\n\tCRBA average time 280.576 microseconds\r\niDynTree/KDL ratio : \r\n\tRNEA average time 15.6892\r\n\tCRBA average time 6.74009\r\n~~~\r\n\r\nSo in average the pure iDynTree implementation are now one order of magnitude slower then the previous implementation. I actually have on which is the bottleneck that is causing this slowdown (in a nutshell : the old joint system was caching the joint transforms, while the new systems is computing them everytime). However I prefer to be methodologically and see the results of running this benchmark on some profiling tools and see if that is actually the bottleneck, and then solve it. "},"comment":{"url":"https://api.github.com/repos/robotology/idyntree/issues/comments/158999757","html_url":"https://github.com/robotology/idyntree/issues/98#issuecomment-158999757","issue_url":"https://api.github.com/repos/robotology/idyntree/issues/98","id":158999757,"user":{"login":"traversaro","id":1857049,"avatar_url":"https://avatars.githubusercontent.com/u/1857049?v=3","gravatar_id":"","url":"https://api.github.com/users/traversaro","html_url":"https://github.com/traversaro","followers_url":"https://api.github.com/users/traversaro/followers","following_url":"https://api.github.com/users/traversaro/following{/other_user}","gists_url":"https://api.github.com/users/traversaro/gists{/gist_id}","starred_url":"https://api.github.com/users/traversaro/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/traversaro/subscriptions","organizations_url":"https://api.github.com/users/traversaro/orgs","repos_url":"https://api.github.com/users/traversaro/repos","events_url":"https://api.github.com/users/traversaro/events{/privacy}","received_events_url":"https://api.github.com/users/traversaro/received_events","type":"User","site_admin":false},"created_at":"2015-11-23T17:15:00Z","updated_at":"2015-11-23T17:15:00Z","body":"I discussed with @francesco-romano and he is ok with what we discussed in https://github.com/robotology/idyntree/issues/98#issuecomment-158823148 . I think I will first implement this and the removing the initialization in `VectorFixSize` and `MatrixFixSize`, because they are clearly performance bugs, while the caching of transforms in joint classes is a bit more complicated and controversial (it basically breaks thread safety of the Joint classes ."}}