Right now we are computing the energy consumption by "integrating" the torque (force) over each joint and summing them up. Look [here](https://github.com/resibots/hexapod_simu/blob/2b6ec5873463b19cf6a96d2bbd10c115d377580a/hexapod_dart/src/hexapod_dart_simu.cpp#L62-L63) and [here](https://github.com/resibots/hexapod_simu/blob/2b6ec5873463b19cf6a96d2bbd10c115d377580a/hexapod_dart/src/hexapod_dart_simu.cpp#L94). Maybe this is incorrect or there is a better way.