[Sensors] Create function to extract a reduced model and sensors list

Furthermore, fix various issues around in the code.

The spreading of code related to this in the library is suboptimal,
and mostly due to the fact that model functions can't depend on sensors data structure.
The definite fix for this is to merge the model and sensors libraries, but
this require that first we solve this issue: #132 .

src/estimation/include/iDynTree/Estimation/ExtWrenchesAndJointTorquesEstimator.h

@@ -128,6 +128,24 @@ class ExtWrenchesAndJointTorquesEstimator
      */
     bool loadModelAndSensorsFromFile(const std::string filename, const std::string filetype="");
 
+    /**
+     * Load model and sensors from file, specifieng the dof considered for the estimation.
+     *
+     * @param[in] filename path to the file to load.
+     * @param[in] consideredDOFs list of dof to consider in the model.
+     * @param[in] filetype (optional) explicit definiton of the filetype to load.
+     *                     Only "urdf" is supported at the moment.
+     * @return true if all went well (files were correctly loaded and consistent), false otherwise.
+     *
+     *
+     * \note this will create e a reduced model only with the joint specified in consideredDOFs and  the
+     *       fixed joints in which FT sensor are mounted.
+     */
+    bool loadModelAndSensorsFromFileWithSpecifiedDOFs(const std::string filename,
+                                                      const std::vector<std::string> & consideredDOFs,
+                                                      const std::string filetype="");
+
+
     /**
      * Get used model.
      *

src/estimation/include/iDynTree/Estimation/ExternalWrenchesEstimation.h

@@ -76,6 +76,19 @@ enum UnknownWrenchContactType
  */
 struct UnknownWrenchContact
 {
+    /**
+     * Constructor
+     */
+    UnknownWrenchContact()
+    {}
+
+    UnknownWrenchContact(const UnknownWrenchContactType _unknownType,
+                         const Position & _contactPoint,
+                         const Direction & _forceDirection = iDynTree::Direction::Default()): unknownType(_unknownType),
+                                                                                              contactPoint(_contactPoint),
+                                                                                              forceDirection(_forceDirection)
+    {}
+
     /**
      * Type of the unknown contact.
      */

src/estimation/src/ExtWrenchesAndJointTorquesEstimator.cpp

@@ -32,6 +32,8 @@
 #include <iDynTree/Model/JointState.h>
 #include <iDynTree/Model/ContactWrench.h>
 #include <iDynTree/Model/Dynamics.h>
+#include <iDynTree/Model/ModelTransformers.h>
+#include <iDynTree/Sensors/ModelSensorsTransformers.h>
 
 #include <iDynTree/Sensors/Sensors.h>
 #include <iDynTree/Sensors/SixAxisFTSensor.h>
@@ -170,6 +172,53 @@ bool ExtWrenchesAndJointTorquesEstimator::loadModelAndSensorsFromFile(const std:
     return setModelAndSensors(_model,_sensors);
 }
 
+bool ExtWrenchesAndJointTorquesEstimator::loadModelAndSensorsFromFileWithSpecifiedDOFs(const std::string filename,
+                                                                                       const std::vector< std::string >& consideredDOFs,
+                                                                                       const std::string filetype)
+{
+    Model _modelFull;
+    SensorsList _sensorsFull;
+
+    bool parsingCorrect = false;
+
+    parsingCorrect = modelFromURDF(filename,_modelFull);
+
+    if( !parsingCorrect )
+    {
+        reportError("ExtWrenchesAndJointTorquesEstimator","loadModelAndSensorsFromFileWithSpecifiedDOFs","Error in parsing model from URDF.");
+        return false;
+    }
+
+    parsingCorrect = sensorsFromURDF(filename,_modelFull,_sensorsFull);
+
+    if( !parsingCorrect )
+    {
+        reportError("ExtWrenchesAndJointTorquesEstimator","loadModelAndSensorsFromFileWithSpecifiedDOFs","Error in parsing sensors from URDF.");
+        return false;
+    }
+
+    // We need to create a reduced model, inclusing only the consideredDOFs and the fixed joints used by the FT sensors
+    std::vector< std::string > consideredJoints = consideredDOFs;
+
+    // Add FT fixed joints
+    std::vector< std::string > ftJointNames;
+    getFTJointNames(_sensorsFull,ftJointNames);
+
+    for(size_t i=0; i < ftJointNames.size(); i++)
+    {
+        consideredJoints.push_back(ftJointNames[i]);
+    }
+
+
+    Model _modelReduced;
+    SensorsList _sensorsReduced;
+
+    //
+    iDynTree::createReducedModelAndSensors(_modelFull,_sensorsFull,consideredJoints,_modelReduced,_sensorsReduced);
+
+    return setModelAndSensors(_modelReduced,_sensorsReduced);
+}
+
 
 const Model& ExtWrenchesAndJointTorquesEstimator::model() const
 {

src/icub-kdl/src/TorqueEstimationTree.cpp

@@ -202,12 +202,12 @@ void TorqueEstimationTree::TorqueEstimationConstructor(KDL::Tree & icub_kdl,
     //Setting a custom dof serialization (\todo TODO FIXME : quite an hack, substitute with proper)
     if( dof_serialization.size() != 0 )
     {
-        YARP_ASSERT(dof_serialization.size() == serial.getNrOfDOFs());
+        yAssert(dof_serialization.size() == serial.getNrOfDOFs());
         for(std::size_t dof=0; dof < dof_serialization.size(); dof++)
         {
             std::string dof_string = dof_serialization[dof];
-            YARP_ASSERT(serial.getDOFID(dof_string) != -1);
-            YARP_ASSERT(serial.getJunctionID(dof_string) != -1);
+            yAssert(serial.getDOFID(dof_string) != -1);
+            yAssert(serial.getJunctionID(dof_string) != -1);
         }
 
         for(std::size_t dof=0; dof < dof_serialization.size(); dof++)

src/icub-kdl/src/idyn2kdl_icub.cpp

@@ -443,16 +443,16 @@ bool toKDL(const iCub::iDyn::iCubWholeBody & icub_idyn,
 
     KDL::Frame root_link_H_l_foot_dh_frame, root_link_H_l_foot,l_foot_dh_frame_H_l_foot;
     int ret = pos_solv.JntToCart(pos,root_link_H_l_foot_dh_frame,"l_foot_dh_frame");
-    YARP_ASSERT(ret == 0);
+    yAssert(ret == 0);
     ret = pos_solv.JntToCart(pos,root_link_H_l_foot,"l_foot");
-    YARP_ASSERT(ret == 0);
+    yAssert(ret == 0);
     l_foot_dh_frame_H_l_foot = root_link_H_l_foot_dh_frame.Inverse()*root_link_H_l_foot;
 
     KDL::Frame root_link_H_r_foot_dh_frame, root_link_H_r_foot,r_foot_dh_frame_H_r_foot;
     ret = pos_solv.JntToCart(pos,root_link_H_r_foot_dh_frame,"r_foot_dh_frame");
-    YARP_ASSERT(ret == 0);
+    yAssert(ret == 0);
     ret = pos_solv.JntToCart(pos,root_link_H_r_foot,"r_foot");
-    YARP_ASSERT(ret == 0);
+    yAssert(ret == 0);
     r_foot_dh_frame_H_r_foot = root_link_H_r_foot_dh_frame.Inverse()*root_link_H_r_foot;
 
 
@@ -488,13 +488,13 @@ bool toKDL(const iCub::iDyn::iCubWholeBody & icub_idyn,
     // Export chest_skin_frame, see https://github.com/robotology/icub-main/issues/124 for more info
     KDL::Frame root_link_H_chest;
     int retChest = pos_solv.JntToCart(pos,root_link_H_chest,"chest");
-    YARP_ASSERT(retChest == 0);
+    yAssert(retChest == 0);
     KDL::Frame root_link_H_chest_skin_frame;
     iCub::iKin::iCubTorso chest_skin_frame_chest;
     yarp::sig::Vector qTorso(chest_skin_frame_chest.getN());
     qTorso.zero();
     chest_skin_frame_chest.setAng(qTorso);
-    YARP_ASSERT(YarptoKDL(chest_skin_frame_chest.getH(),root_link_H_chest_skin_frame));
+    yAssert(YarptoKDL(chest_skin_frame_chest.getH(),root_link_H_chest_skin_frame));
 
     std::cout << "root_link_H_chest_skin_frame is " << root_link_H_chest_skin_frame << std::endl;
 

src/model/include/iDynTree/Model/ModelTransformers.h

@@ -21,6 +21,7 @@
 namespace iDynTree
 {
 class Model;
+class SensorsList;
 
 /**
  * \function Remove all fake links in the model, transforming them in frames.
@@ -47,15 +48,15 @@ bool removeFakeLinks(const Model& modelWithFakeLinks,
                      Model& modelWithoutFakeLinks);
 
 /**
- * This function will take in input a iDynTree::Model and
- * an ordered list of joints and will return a model with
+ * This function takes in input a iDynTree::Model and
+ * an ordered list of joints and returns a model with
  * just the joint specified in the list, with that exact order.
  *
- * All other joints will be removed by lumping (i.e. fusing together)
+ * All other joints are be removed by lumping (i.e. fusing together)
  * the inertia of the links that are connected by that joint, assuming the joint
  * to be in "rest" position (i.e. zero for revolute joints). The links eliminated
- * with this process will be added back to the reduced model as "frames",
- * and will be copied in the same way all the additional frames of the lumped links.
+ * with this process are be added back to the reduced model as "frames",
+ * and are copied in the same way all the additional frames of the lumped links.
  * 
  */
 bool createReducedModel(const Model& fullModel,

src/model/src/ModelTransformers.cpp

@@ -373,7 +373,7 @@ bool createReducedModel(const Model& fullModel,
         Transform newLink1_X_oldLink1 = subModelBase_X_link(oldLink1);
         Transform newLink2_X_oldLink2 = subModelBase_X_link(oldLink2);
 
-        // \todo handle this in a joint-agnostic way,
+        // \todo TODO handle this in a joint-agnostic way,
         // possibly extending the joint interface
         IJointPtr newJoint = 0;
         if( dynamic_cast<const FixedJoint*>(oldJoint) )
@@ -419,5 +419,4 @@ bool createReducedModel(const Model& fullModel,
 }
 
 
-
 }

src/sensors/CMakeLists.txt

@@ -14,13 +14,15 @@ set(IDYNTREE_SENSORS_HEADERS include/iDynTree/Sensors/Sensors.h
                              include/iDynTree/Sensors/SixAxisFTSensor.h
                              include/iDynTree/Sensors/GyroscopeSensor.h
                              include/iDynTree/Sensors/AccelerometerSensor.h
-                             include/iDynTree/Sensors/PredictSensorsMeasurements.h)
+                             include/iDynTree/Sensors/PredictSensorsMeasurements.h
+                             include/iDynTree/Sensors/ModelSensorsTransformers.h)
 
 set(IDYNTREE_SENSORS_SOURCES src/Sensors.cpp
                              src/SixAxisFTSensor.cpp
                              src/AccelerometerSensor.cpp
                              src/GyroscopeSensor.cpp
-                             src/PredictSensorsMeasurements.cpp)
+                             src/PredictSensorsMeasurements.cpp
+                             src/ModelSensorsTransformers.cpp)
 
 
 SOURCE_GROUP("Source Files" FILES ${IDYNTREE_SENSORS_SOURCES})

src/sensors/include/iDynTree/Sensors/AccelerometerSensor.h

@@ -123,6 +123,11 @@ namespace iDynTree {
          */
         Sensor * clone() const;
 
+        /*
+         * Documented in Sensor
+         */
+        bool updateIndeces(const Model & model);
+
         /**
          * Get the transform from the sensor to the specified link.
          *

src/sensors/include/iDynTree/Sensors/GyroscopeSensor.h

@@ -122,6 +122,10 @@ namespace iDynTree {
          */
         Sensor * clone() const;
 
+        /*
+         * Documented in Sensor
+         */
+        bool updateIndeces(const Model & model);
 
         /**
          * Get the transform from the sensor to the parent link.

src/sensors/include/iDynTree/Sensors/ModelSensorsTransformers.h

@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia
+ * Authors: Silvio Traversaro
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ *
+ */
+
+/**
+ * \file ModelSensorsTransformers.h
+ *  \brief Collection of function to modify model and sensors in various ways
+ *
+ *
+ */
+
+
+#ifndef IDYNTREE_MODEL_SENSORS_TRANSFORMERS_H
+#define IDYNTREE_MODEL_SENSORS_TRANSFORMERS_H
+
+namespace iDynTree
+{
+class Model;
+class SensorsList;
+
+/**
+ * Variant of createReducedModel function that also process the sensorList .
+ *
+ *
+ */
+bool createReducedModelAndSensors(const Model& fullModel,
+                                  const SensorsList& fullSensors,
+                                  const std::vector<std::string>& jointsInReducedModel,
+                                        Model& reducedModel,
+                                        SensorsList& reducedSensors);
+
+
+}
+
+#endif

src/sensors/include/iDynTree/Sensors/Sensors.h

@@ -33,6 +33,7 @@ namespace iDynTree {
 
 #include <iDynTree/Core/VectorDynSize.h>
 
+#include <iDynTree/Model/Model.h>
 #include <iDynTree/Model/Indeces.h>
 
 namespace iDynTree {
@@ -84,10 +85,6 @@ namespace iDynTree {
         virtual bool isValid() const = 0;
 
         /**
-         *  Return a pointer to a copy of this sensor.
-         *
-         */
-                /**
          * Set the id (name) of sensor.
          */
         virtual bool setName(const std::string &) = 0;
@@ -97,6 +94,11 @@ namespace iDynTree {
          *
          */
         virtual Sensor* clone() const = 0;
+
+        /**
+         * Update all the indeces (link/frames) contained in this sensor.
+         */
+        virtual bool updateIndeces(const Model & model) = 0;
     };
 
     /**

src/sensors/include/iDynTree/Sensors/SixAxisFTSensor.h

@@ -153,16 +153,21 @@ namespace iDynTree {
         // Documented in JointSensor
         JointIndex getParentJointIndex() const;
 
-        /**
+        /*
          * Documented in Sensor
          */
         bool isValid() const;
 
-        /**
+        /*
          * Documented in Sensor
          */
         Sensor * clone() const;
 
+        /*
+         * Documented in Sensor
+         */
+        bool updateIndeces(const Model & model);
+
         /**
          * The Six Axis Force Torque sensor measure the Force Torque (wrench)
          * applied by a link on another link. This method returns the link

src/sensors/src/AccelerometerSensor.cpp

@@ -131,6 +131,20 @@ Sensor* AccelerometerSensor::clone() const
     return (Sensor *)new AccelerometerSensor(*this);
 }
 
+bool AccelerometerSensor::updateIndeces(const Model& model)
+{
+    iDynTree::LinkIndex linkNewIndex = model.getLinkIndex(this->pimpl->parent_link_name);
+
+    if( (linkNewIndex == iDynTree::LINK_INVALID_INDEX) )
+    {
+        return false;
+    }
+
+    this->pimpl->parent_link_name = linkNewIndex;
+
+    return true;
+}
+
 
 std::string AccelerometerSensor::getName() const
 {

src/sensors/src/GyroscopeSensor.cpp

@@ -116,6 +116,21 @@ Sensor* GyroscopeSensor::clone() const
     return (Sensor *)new GyroscopeSensor(*this);
 }
 
+bool GyroscopeSensor::updateIndeces(const Model& model)
+{
+    iDynTree::LinkIndex linkNewIndex = model.getLinkIndex(this->pimpl->parent_link_name);
+
+    if( (linkNewIndex == iDynTree::LINK_INVALID_INDEX) )
+    {
+        return false;
+    }
+
+    this->pimpl->parent_link_name = linkNewIndex;
+
+    return true;
+}
+
+
 
 std::string GyroscopeSensor::getName() const
 {