Merge pull request #154 from robotology/reducedModelAndSensors

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

src/core/CMakeLists.txt

@@ -98,6 +98,10 @@ target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURR
 
 set_property(TARGET ${libraryname} PROPERTY PUBLIC_HEADER ${IDYNTREE_CORE_EXP_HEADERS})
 
+if(MSVC)
+   add_definitions(-D_USE_MATH_DEFINES)
+endif()
+
 install(TARGETS ${libraryname}
         EXPORT iDynTree
         COMPONENT runtime

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
 {