Implement TorqueEstimation without depending on KDL

[traversaro]

Similarly to https://github.com/robotology/idyntree/issues/93 , this meta-issue is useful to track what to we miss to perform torque estimation (i.e. wholeBodyDynamicsTree) using only iDynTree data structures. (As for #93 this is not a big priority, but it would definitely simplify the use of the wholeBodyDynamicsTree/wholeBodyEstimator for all the people using just the iCub infrastructure and not the CoDyCo one, see also https://github.com/robotology/codyco-modules/issues/158 ):

• [x] Implement a class to represent location of contact points coming from the skin (similar to dynContactList but without dynamic memory re-allocation at each resize, and with only the details about the contact that concern the wrench estimation. Done in https://github.com/robotology/idyntree/commit/6ead0fad702ded04595b36a5792d8871f7019633 .
  • [x] Write conversion functions (from and to) between this new class and dynContactList , to read the data coming from the skinManager and write data to the ../contacts:o port of wholeBodyDynamicsTree . Done in https://github.com/robotology/idyntree/commit/b458d50807902577c9b6c5a2928a285a974fdb18 .
• [x] Implement a set of function/classes for estimation of external wrenches given the information of where the external wrenches are applied and which sensors are available. Done in https://github.com/robotology/idyntree/commit/61a8c4fe81f9589647f8e9dc592199cad1863a87 .
  • [x] Use case to cover: estimating the external wrenches when the skin and the FT sensor measures are available. Done in https://github.com/robotology/idyntree/commit/61a8c4fe81f9589647f8e9dc592199cad1863a87 , with the ExtWrenchesAndJointTorquesEstimator::estimateExtWrenchesAndJointTorques method.
  • [x] Use case to cover: estimating the external wrenches without using the sensor information, but doing some assumption on the external forces (as we do when we perform calibration of the FT sensor offset or macumba). Done in https://github.com/robotology/idyntree/commit/61a8c4fe81f9589647f8e9dc592199cad1863a87 , with the ExtWrenchesAndJointTorquesEstimator::computeExpectedFTSensorsMeasurements method.
• [x] Once the external wrenches are available, run the usual inverse dynamics (already implemented in https://github.com/robotology/idyntree/blob/master/src/model/include/iDynTree/Model/Dynamics.h#L29 ).
• [x] Write consistency test on the old TorqueEstimation class as already done for other classes in https://github.com/robotology/idyntree/tree/master/tests/kdl_consistency (added in https://github.com/robotology/idyntree/commit/df883c774a32c20649530c7fd0f95528ea1f3c6a )
• [x] Check that we are not degrading performance, see https://github.com/robotology/idyntree/issues/98

[naveenoid]

Can i handle one or more of the sub tasks? Is there any task assignments so far?

[traversaro]

Actually this task has quite a low priority (it is not blocking anything else, and the current KDL-based TorqueEstimation class is working fine) so the task have not be assigned to anyone, but if you like feel free to work on this.

[traversaro]

Just to clarify: this issue is tracking how to implement in iDynTree the algorithm already implemented in the TorqueEstimation . It is not tracking how to implement the torque estimation using berdy.

[traversaro]

@nunoguedelha I added some docs to the data structures currently used for external wrenches computations in https://github.com/robotology/idyntree/commit/5973ce7608c67640806046a28fab720c1d8cd93a .

As most of the dynamics function are currently not documented (documenting all of them is definitely is a desirable goal, but it would take some time) @nunoguedelha can you tell me which one do you prefer that I document because their use/semantics is not clear?

[traversaro]

The tricky part is that we currently have the following data structure for modeling external forces :

• dynContactList is class in the ICUB skinDynLib inefficient for its use of YARP vectors, used to model:
• the location of the contact points identified by the skinManager
• the knowledge if an external force is a pure force or a wrench (obtained from an heuristic implemented in skinManager)
• once the estimation of external wrenches magnitude is known, this result is stored in dynContactList, to publish it.

The use of dynContactList is (partially) documented in this part of the icub wiki : http://wiki.icub.org/wiki/Tactile_sensors_%28aka_Skin%29#Tactile_Data_Flow .

[traversaro]

Other interesting documents are :

• Paper on how the external forces are estimated : http://www.researchgate.net/profile/Andrea_Del_Prete/publication/236152161_Contact_Force_Estimations_Using_Tactile_Sensors_and_Force__Torque_Sensors/links/00b495166e74a5369d000000.pdf . In TorqueEstimationTree an extended version for dealing also with tree with multiple F/T sensors mounted on them is implemented.
• The source code of TorqueEstimationTree is the ultimate reference on how things are currently implemented.

[nunoguedelha]

my last commit on current implementation here: https://github.com/robotology/idyntree/tree/feature/97/contactListWoKDL

[traversaro]

See https://github.com/robotology/idyntree/pull/130 . Issue update to link the relevant commits for each bullet point.