Add COM_ACCELEROMETER_SENSOR to berdy sensor types

[yeshasvitirupachuri]

This issue aims at adding a new COM_ACCELEROMETER_SENSOR berdy sensor to the berdy helper class. This sensor lets us add additional constraint on the estimation of dynamic variables. Additional details will be updated.

[yeshasvitirupachuri]

The idea of adding a new berdy sensor type COM_ACCELEROMETER_SENSOR is to update the measurement matrices with an additional constraint based on the idea that the net external force acting on the center of mass is equal to the rate of change of the linear momentum. In all the terms are expressed with respect to the inertial frame of reference (i.e. world frame), we can write the condition as

$ m \ ^{w}\ddot{x}_{com} = m \ ^{w}g + \sum_1^n \ ^{w}f_i^{ext}$

where, $m$ is the mass, $w$ is the world frame, $g$ is the gravity vector, $f_i^{ext}$ is the net external wrench acting on the link $i$, $n$ is the total number of links in the model.

The sensor values considered for the linear and angular parts of the accelerometer berdy sensor types are proper accelerometerions. To be consisistent with them, the COM accelerometer sensor measurements are also considered to be proper accelerations. In addition, the estimated external wrench on each of the link are expressed with respect to the link body frame. So, the previous relation when expressed with respect to the body frame becomes:

$ m \ ^{base}R_w \ (^{w}\ddot{x}_{com} - \ ^{w}g ) = \sum_1^n \ ^{base}R_L \ ^{L}f_i^{ext}$

Where, $L$ indicates the link body frame

[yeshasvitirupachuri]

Given this constraint, the sensor matrices in berdy are updated as following,

$Y d + by = y$

• The total number of additional rows added are 3
• The column elements in the three rows are zero at all locations except for the column location corresponding to the first three rows of the estimated net external wrench dynamical variable in the dynamical variables vector d
• The corresponding elements in the vector $by$ are zero
• The 3 new elements in the measurement vector $y$ corresponds to the proper acceleration measurements of the CoM.

[yeshasvitirupachuri]

As discussed with @DanielePucci we can modify the following constraint and instead of considering all the links i.e. n we can consider only the links that are considered to have external wrench sources i.e. feet and hands

$ m \ ^{base}R_w \ (^{w}\ddot{x}_{com} - \ ^{w}g ) = \sum_1^n \ ^{base}R_L \ ^{L}f_i^{ext}$

So, the constraint becomes,

LH is LeftHand RH is RightHand LF is LeftFoot RF is RightFoot

This is updated in https://github.com/robotology/idyntree/pull/571/commits/4605789b5cd2ec6cc1a3f541cd25acc181e730db