`getSensorIndex` from `sensors` class returns index shifted by 1.

[nunoguedelha]

@naveenoid , @traversaro , I create an estimator class object, load the iCub model from an URDF file and then print out the model (links, joints, sensors frames):

% Create estimator class
estimator = iDynTree.ExtWrenchesAndJointTorquesEstimator();

% Load model and sensors from the URDF file
estimator.loadModelAndSensorsFromFile('../models/iCubGenova02/iCubFull.urdf');

% Check if the model was correctly created by printing the model
estimator.model().toString()

I show below the partial display of the model, which seems correct:

  Frames: 
    [33] base_link --> root_link
    [34] root_link_ems_acc_eb5 --> root_link
    [35] root_link_ems_gyro_eb5 --> root_link
    [36] r_upper_leg_ems_acc_eb8 --> r_upper_leg
    [37] r_upper_leg_ems_acc_eb11 --> r_upper_leg
    [38] r_upper_leg_ems_gyro_eb8 --> r_upper_leg
    [39] r_upper_leg_ems_gyro_eb11 --> r_upper_leg
    [40] r_upper_leg_mtb_acc_11b1 --> r_upper_leg
    [41] r_upper_leg_mtb_acc_11b2 --> r_upper_leg
    [42] r_upper_leg_mtb_acc_11b3 --> r_upper_leg
    [43] r_upper_leg_mtb_acc_11b4 --> r_upper_leg
    [44] r_upper_leg_mtb_acc_11b5 --> r_upper_leg
    [45] r_upper_leg_mtb_acc_11b6 --> r_upper_leg
    [46] r_upper_leg_mtb_acc_11b7 --> r_upper_leg
    [47] r_lower_leg_ems_acc_eb9 --> r_lower_leg
    [48] r_lower_leg_ems_gyro_eb9 --> r_lower_leg
    [49] r_lower_leg_mtb_acc_11b8 --> r_lower_leg

But then, estimator.sensors.getSensorIndex(iDynTree.ACCELEROMETER,char('r_upper_leg_mtb_acc_11b1')) returns 41 instead of 40, and I get the same until label 11B7. From 11B8 on, the returned index is correct.

[naveenoid]

checking this morning.

On Mon, Feb 15, 2016 at 3:00 AM, Nuno Guedelha notifications@github.com wrote:

@naveenoid https://github.com/naveenoid , @traversaro https://github.com/traversaro , I create an estimator class object, load the iCub model from an URDF file and then print out the model (links, joints, sensors frames):

% Create estimator class estimator = iDynTree.ExtWrenchesAndJointTorquesEstimator();

% Load model and sensors from the URDF file estimator.loadModelAndSensorsFromFile('../models/iCubGenova02/iCubFull.urdf');

% Check if the model was correctly created by printing the model estimator.model().toString()

I show below the partial display of the model, which seems correct:

Frames: [33] base_link --> root_link [34] root_link_ems_acc_eb5 --> root_link [35] root_link_ems_gyro_eb5 --> root_link [36] r_upper_leg_ems_acc_eb8 --> r_upper_leg [37] r_upper_leg_ems_acc_eb11 --> r_upper_leg [38] r_upper_leg_ems_gyro_eb8 --> r_upper_leg [39] r_upper_leg_ems_gyro_eb11 --> r_upper_leg [40] r_upper_leg_mtb_acc_11b1 --> r_upper_leg [41] r_upper_leg_mtb_acc_11b2 --> r_upper_leg [42] r_upper_leg_mtb_acc_11b3 --> r_upper_leg [43] r_upper_leg_mtb_acc_11b4 --> r_upper_leg [44] r_upper_leg_mtb_acc_11b5 --> r_upper_leg [45] r_upper_leg_mtb_acc_11b6 --> r_upper_leg [46] r_upper_leg_mtb_acc_11b7 --> r_upper_leg [47] r_lower_leg_ems_acc_eb9 --> r_lower_leg [48] r_lower_leg_ems_gyro_eb9 --> r_lower_leg [49] r_lower_leg_mtb_acc_11b8 --> r_lower_leg

But then, estimator.sensors.getSensorIndex(iDynTree.ACCELEROMETER,char('r_upper_leg_mtb_acc_11b1')) returns 41 instead of 40, and I get the same until label 11B7. From 11B8 on, the returned index is correct.

— Reply to this email directly or view it on GitHub https://github.com/robotology/idyntree/issues/138.

[traversaro]

SensorList::getSensorIndex(iDynTree.ACCELEROMETER,char('r_upper_leg_mtb_acc_11b1')) should return a integer (between 0 and SensorList::getNrOfSensors(iDynTree.ACCELEROMETER)-1) that uniquely identifies the r_upper_leg_mtb_acc_11b1 sensor in the given SensorList.

Unfortunatly in the model there is also a frame (see [1][2][3]) named r_upper_leg_mtb_acc_11b1 . The FrameIndex (a integer between 0 and Model::getNrOfFrames()-1) that uniquely identifies this frame in Model is the one that is printed in the Model::toString() function.

This leads to some confusion, but in general there is no relation betwen the sensor indices and the frame indeces.

[1] : http://wiki.icub.org/codyco/dox/html/idyntree/html/classiDynTree_1_1Model.html [2] : http://wiki.icub.org/codyco/dox/html/idyntree/html/group__iDynTreeModelIO.html#ga92a1cb6d04adfefde7ae7d97390d23f8 [3] : http://wiki.icub.org/codyco/dox/html/idyntree/html/namespaceiDynTree.html#aab77480c8f9d5add110768cf75226508

[nunoguedelha]

@traversaro Thanks Silvio. It can indeed be misleading. Well, actually in some situations it could speed up things if we could use the same index, typicaly if we are doing some processing using all sensor frames. As per current design, we have to match the sensor data and frames through the sensor name string right? which induces more processing.

[naveenoid]

To each sensor, you are supposed to query the parent joint name ( http://wiki.icub.org/codyco/dox/html/idyntree/html/classiDynTree_1_1JointSensor.html) to obtain the relationship. Probably you are are not using the appropriate API?

On Mon, Feb 15, 2016 at 10:48 AM, Nuno Guedelha notifications@github.com wrote:

@traversaro https://github.com/traversaro Thanks Silvio. It can indeed be misleading. Well, actually in some situations it could speed up things if we could use the same index, typicaly if we are doing some processing using all sensor frames. As per current design, we have to match the sensor data and frames through the sensor name string right? which induces more processing.

— Reply to this email directly or view it on GitHub https://github.com/robotology/idyntree/issues/138#issuecomment-184136763 .

[traversaro]

The current design is that for each LinkSensor you can access the parent link (not the parent frame, notice!) with the getParentLink/getParentLinkIndex.

The transform between the sensor frame and the parent link frame (link_H_sensor) can be retrieved with the getLinkSensorTransform method.

Notice that at the moment there is not relation possible between a LinkSensor and any link "additional frame" (see http://wiki.icub.org/codyco/dox/html/idyntree/html/classiDynTree_1_1Model.html). The sensor frames were added to the model just as a temporary workaround until the sensor parsing stuff was ready.

[traversaro]

If you have a LinkPositions linkPos array containing the position of all the link with respect to a common frame (for example the world/inertial frame) i.e. inertial_H_link = linkPos(linkIndex), the sensor position with respect to the inertial can be found as:

// inertial_H_sensor = inertial_H_link*link_H_sensor
inertial_H_sensor = linkPos(sensor.getParentLinkIndex())*sensor.getLinkSensorTransform(); 

[nunoguedelha]

Understood. So sensor.getLinkSensorTransform() should return he same transform as model.getFrameTransform(model.getFrameIndex(sensor.getName())), but 2nd method shouldn't be used anymore right?

[naveenoid]

Yes because we will delete all of those frames from the URDF shortly. However the sensor tags will include the info of transform to an associated "parent" frame.

On Mon, Feb 15, 2016 at 12:22 PM, Nuno Guedelha notifications@github.com wrote:

Understood. So sensor.getLinkSensorTransform() should return he same transform as model.getFrameTransform(model.getFrameIndex(sensor.getName())), but 2nd method shouldn't be used anymore right?

— Reply to this email directly or view it on GitHub https://github.com/robotology/idyntree/issues/138#issuecomment-184173314 .