robotology / idyntree

Multibody Dynamics Library designed for Free Floating Robots
BSD 3-Clause "New" or "Revised" License
175 stars 67 forks source link

Need a PredictMeasurement component and tests on Predictions #95

Closed naveenoid closed 8 years ago

naveenoid commented 8 years ago

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 commented 8 years ago

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

traversaro commented 8 years ago

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 commented 8 years ago

:+1:

naveenoid commented 8 years ago

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 commented 8 years ago

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 commented 8 years ago

Yes sure..didnt mean to leave it out!

traversaro commented 8 years ago

Two points :

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 commented 8 years ago

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 commented 8 years ago

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 commented 8 years ago

:+1:

iron76 commented 8 years ago

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 commented 8 years ago

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

naveenoid commented 8 years ago

Modified the issue to include testing.

naveenoid commented 8 years ago

@traversaro Can we merge the PR and close this?

traversaro commented 8 years ago

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 commented 8 years ago

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