Need a PredictMeasurement component and tests on Predictions

[naveenoid]

Following the incorporation of sensor tags within URDF ( ca35b6b3400587eb5275db8e22daf3e04df8eecb ) which can be extracted to build the SensorList, there is an important need to build a PredictMeasurement component that uses iDynTree computations to generate (forward predict) sensor data in their own frames for a given state of the robot. This will useful for future calibration work and is closely related to the Y matrix discussed in the developments of https://github.com/robotology/idyntree/issues/78 .

( @iron76 @traversaro @nunoguedelha @jeljaik )

[traversaro]

I think this can be a implemented as a class in the Sensor part of iDynTree.

[traversaro]

At runtime, this class will take in input the current robot positions, velocities and acceleration (and eventually external forces, for predicting FT sensor offset). For this reason I think it will be interesting to a look at the classes that we used as an input to the whole-body forward kinematics : https://github.com/robotology/idyntree/blob/master/model/include/iDynTree/Model/ForwardKinematics.h#L57 .

[naveenoid]

:+1:

[naveenoid]

As discussed with @nunoguedelha it think its best if this component directly computes the BERDY (https://github.com/robotology/codyco-superbuild/wiki/Road-to-BERDY) measurement equation y = Yd by building up parameterised Y and d (matrix / vector). This is in sense a modified version of task T0 in the road to BERDY in link above.

The structure of Y and d is dictated by the URDF and the outcome is a simple linear operation on Eigen derived components. We will in the first implementation use the Eigen DenseMatrix and then ignore the F/T sensors in the Y and y since we focus on accelerometers and gyroscopes. The same can then be expanded to use FT.

@nunoguedelha and I are of the opinion that the semantic consistency may be propagated to the linkwise dynamic quantities vector (d) and the the Measurement matrix (Y) and thus to the measurements themselves (y).

FYI the usage of component in the title is a reference to the notion that it is meant to be a binary in which the computation is wrapped. We can consider a better name if desired.

[iron76]

Not sure if this is the appropriate issue to raise the issue but don't forget that we also need the b_Y term since not all measurements can be expressed as y = Y d. The general formulation is y = Y(q, dq) d + b_Y(q, dq). One example of sensor with non-null b_Y term is a linear accelerometer which is roti-translated with respect the link frame. See Section 2.11 in Featherstone's book and ask @traversaro for details.

[naveenoid]

Yes sure..didnt mean to leave it out!

[traversaro]

Two points :

• I think it is easier to think before in terms of computations enclosed in classes or functions, and only after we defined and implemented the computational part we can think how to enclose it in framework-specific components. In particular, I don't think it make sense to a have a PredictMeasurement as a "component", at least not in the sense of a YARP module. We are already designing the wholeBodyEstimator to be able to flexibly run all the whole-body estimators that we may want to implement, and reuse among them all the YARP-specific code (how the estimates are published on yarp ports).
• Implementing the PredictMeasurement using the Y matrix implemented as a dense matrix would have a cost quadratic in the number of degrees of freedom of the robot (and the actual number of operation involved would be quite high, given that the Y matrix is quite big). Implement it has a matrix is necessary because implementing directly BERDY as a recursive algorithm will require a lot of tools (basically bayesian networks) that we currenty don't have. However for predict measurement the recursive linear algorithm is quite simple:

Input: q, dq, ddq, fext 
Compute velocity and proper acceleration of each link:
for sensor in sensorList:
    measurment[sensor] = computed as a function of its link velocity and proper acceleration 

Where computed as a function of its link velocity and proper acceleration will be a type-specific sensor method, that we can implement in the sensor class. This is actual quite simpler that implement the berdy matrices, and can be actually be used as a test for when that matrices will be computed.

[naveenoid]

I agree this is simpler and can be immediately tested. However I was considering if building the Y in an automated way from a SensorList class might be more scalable for the future. In anycase the direct way can be implemented immediately.

[traversaro]

I agree that having the berdy Y is useful, but I don't think the main predict measurement function should be implemented with Y.

[naveenoid]

:+1:

[iron76]

I agree with @traversaro when he says that measurement prediction should be implemented in the Sensor part of iDynTree. I think however that we should also implement a unit test in BERDY that should predict a measurement y by computing d with a classical RNEA (as Silvio said with input q, dq, ddq, fext) and then checking that measurement prediction (based iDynTree, Sensor) matches Y d + b_Y.

[naveenoid]

progress : https://github.com/robotology/idyntree/pull/104

[naveenoid]

Modified the issue to include testing.

[naveenoid]

@traversaro Can we merge the PR and close this?

[traversaro]

I still need to check that the matlab bindings tests are still working.. I plan to merge both #103 and #104 this evening and push the updated bindings (to avoid polluting the history with too many matlab bindings commits).

[traversaro]

Closed by https://github.com/robotology/idyntree/pull/103 .